（开源）正点原子飞控+北醒tof+优象光流——室内定点（一）

1、说明：

前几篇文章讲述了如何使用tof的数据实现飞机的定高；接下来分享的是如何使用光流来定点；主要分为以下几个步骤：
1）添加光流驱动，获得x,y轴方向的观测速度；
2）光流速度与加速度数据的互补滤波，获得state.velocity.x 与state.velocity.y；
3)添加遥控器处理，输出setpoint.velocity.x,setpoint.velocity.y；
4)PID控制，实现x轴与y轴方向的速度环控制；
本文最后分享开源git地址与B站飞行效果视频；

2、硬件连接

本篇文章采用正点原子开源飞控、与北醒tof（TF-mini）以及优象光流（302）实现无人机的室内定点功能；
其中tof连接在飞控的串口5、光流模块连接飞控的串口3上；

3、源码解析：

（1）添加光流驱动

优象光流使用串口输出数据，所有首先需要添加串口接收的驱动，该部分在前几篇tof定高环节以有概述，本文不再赘述；其次，需要添加优象光流的解码驱动，按照其通信协议进行解析，获得光流输出的速度信息；
这里要注意的是，光流输出的原始速度值是角速度，还需要乘上高度信息；
主要源码：opticalflow.c 与opticalflow.h

#include 
#include 
#include "config.h"
#include "tof.h"
#include "atkp.h"
#include "config_param.h"
#include "usblink.h"
#include "maths.h"
#include "stabilizer.h"
#include "filter.h"
/*FreeRTOS相关头文件*/
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"
#include "usart.h"
#include 
#include "opticalflow.h"


#define FLOW_INTERFACE_V1_30

#define POS_CALC_OUT_MAX 	2.5f    //单位
#define POS_CALC_OUT_MIN 	-2.5f

#define POS_CONTROL_LIMIT_MAX 	100 /*单位是厘米*/
#define POS_CONTROL_LIMIT_MIN 	-100

#define RESOLUTION			(0.2131946f)/*1m高度下 1个像素对应的位移，单位cm*/
#define OULIER_LIMIT 		(100)		/*光流像素输出限幅*/
#define VEL_LIMIT			(150.f)		/*光流速度限幅*/

#define VEL_LPF_FILTER			/*低通滤波*/

#define LIMIT( x,min,max ) ( ((x) < (min)) ? (min) : ( ((x) > (max))? (max) : (x) ) )

struct flow_integral_frame  flow_data_frame;
struct flow_float flow_dat;

float speed_x=0.0,speed_y=0.0;
float sum_flow_x=0.0,sum_flow_y=0.0;

TaskHandle_t opFlowTaskHandle = NULL;
static xQueueHandle opticalflowDataQueue;
static bool opticalflow_present_flag =false;
static bool isInit = false;
static u8 outlierCount = 0;			/*数据不可用计数*/
opFlow_t opFlow;

biquadFilter_t opticalflowFilterLPF[2];//二阶低通滤波器
//软件二阶低通滤波参数（单位Hz）
#define OPTICALFLOW_LPF_CUTOFF_FREQ 	10.0f
#define OPTICALFLOW_UPDATE_RATE			50

typedef struct
{
  float speed_x;
  float speed_y;
  float sum_flow_x;
  float sum_flow_y;
}opticlaflow_t;

typedef enum {
    X = 0,
    Y,
    Z
} axis_e;


void flow_Decode(const unsigned char *f_buf) {
  flow_data_frame.pixel_flow_x_integral = f_buf[0] + (f_buf[1] << 8);
  flow_data_frame.pixel_flow_y_integral = f_buf[2] + (f_buf[3] << 8);
  flow_data_frame.integration_timespan =
      f_buf[4] + (f_buf[5] << 8) ;
  flow_data_frame.ground_distance = f_buf[6] + (f_buf[7] << 8);
  flow_data_frame.qual = f_buf[8];
  flow_data_frame.gyro_temperature = f_buf[9]; //实际为新版本中的valid
  flow_dat.x = -1.0 * flow_data_frame.pixel_flow_x_integral / 10000.0f;
  flow_dat.y = 1.0 * flow_data_frame.pixel_flow_y_integral / 10000.0f; //为了保留精度，在传输前*10000，所以使用时再/10000，负号是为了匹配传感器方向
  flow_dat.qual = flow_data_frame.qual;
  flow_dat.dt = flow_data_frame.integration_timespan;
  flow_dat.update = 1;
}

bool opticalflow_init(void)
{
	opticalflowDataQueue = xQueueCreate(1, sizeof(tof_t));
  resetOpFlowDataPossum();
  for(u8 i= 0 ; i< 2;i++)
  {
    biquadFilterInitLPF(&opticalflowFilterLPF[i], OPTICALFLOW_UPDATE_RATE, OPTICALFLOW_LPF_CUTOFF_FREQ);
  }
	return opticalflow_present_flag;
}
bool opticalflow_present(void)
{
	return opticalflow_present_flag;
}

static enum {
  waitForStartByte1,
  waitForStartByte2,
  waitForBUF,
  waitForCRC,
  waitForOver,
} rxState;

// USB虚拟串口接收ATKPPacket任务
void OpticalFlowRxTask(void *param) {
  u8 c;
  u8 crc_cal = 0;
  u8 crc_get = 0;
  u8 count = 0;
  u8 Rx_buf_[12] = {0};
  rxState = waitForStartByte1;
  opticalflow_init(); //静态队列的声明 要在本地
  while (1) {
    if (uart3GetDataWithTimout(&c)) {
      switch (rxState) {
      case waitForStartByte1:
        rxState = (c == 0xFE) ? waitForStartByte2 : waitForStartByte1;        
        break;
      case waitForStartByte2:
        rxState = (c == 0x0A) ? waitForBUF : waitForStartByte1;
        count = 0;
        crc_cal = 0;
        break;
      case waitForBUF:
        Rx_buf_[count++] = c;
        crc_cal ^= c;
        if (count == 10) {
          count = 0;
          rxState = waitForCRC;
        }
        break;
      case waitForCRC:
        crc_get = c;
        rxState = waitForOver;
        break;
      case waitForOver:
        if (c == 0x55) {
          if (crc_get == crc_cal) {
            flow_Decode(Rx_buf_);
            opticalflow_present_flag = true;
            opFlow.isOpFlowOk = true;
            if (!isInit) 
            {
              isInit = true;
              if(opFlowTaskHandle == NULL)
              {
                //xTaskCreate(opticalFlowTask, "OPTICAL_FLOW", 300, NULL, 4, &opFlowTaskHandle);	/*创建光流模块任务*/
              }
            }
          }
        }
        rxState = waitForStartByte1;
        break;
      default:
        ASSERT(0);
        break;
      }
    } else /*超时处理*/
    {
      rxState = waitForStartByte1;
    }
  }
}

//后期这里的输入数据是消息队列里面的数据
//输出的数据接入sensor中；
void getOpFlowData(void)
{
	static u8 cnt = 0;
	float height = 0.01f * state.position.z;/*读取高度信息 单位m*/
	
	if(height<4.0f&&flow_dat.update)	/*4m范围内，光流可用*/
	{
    flow_dat.update = 0;
    opFlow.velLpf[X] = (height * flow_dat.x/(flow_dat.dt * 0.000001)) *100 ;  //速度 cm/s  
		opFlow.velLpf[Y] = (height * flow_dat.y/(flow_dat.dt * 0.000001)) *100 ;  //速度 cm/s	
    // opFlow.velLpf[X] = (flow_dat.x/(flow_dat.dt * 0.000001))*100;  //速度 cm/s  
		// opFlow.velLpf[Y] = (flow_dat.y/(flow_dat.dt * 0.000001))*100;  //速度 cm/s	 

   //low pass filter
    opFlow.velLpf[X] = biquadFilterApply(&opticalflowFilterLPF[0], opFlow.velLpf[X]);
    opFlow.velLpf[Y] = biquadFilterApply(&opticalflowFilterLPF[1], opFlow.velLpf[Y]);
   //对速度进行限幅		
		opFlow.velLpf[X] = LIMIT(opFlow.velLpf[X],-62.0f,62.0f);  //速度限制在-62cm/s到62cm/s之间
		opFlow.velLpf[Y] = LIMIT(opFlow.velLpf[Y],-62.0f,62.0f);				
		//位移计算方法：speed_x*0.04 为0.04s内的位移量，将这个位移量累加，就得到距离开始悬停点的位移量。
		opFlow.posSum[X] +=  opFlow.velLpf[X]*(flow_dat.dt * 0.000001);	/*累积位移 cm*/
		opFlow.posSum[Y] +=  opFlow.velLpf[Y]*(flow_dat.dt * 0.000001);	/*累积位移 cm*/
    opFlow.posSum[X] = LIMIT(opFlow.posSum[X],-100.0f,100.0f);
    opFlow.posSum[Y] = LIMIT(opFlow.posSum[X],-100.0f,100.0f);
	}
}

void resetOpFlowDataPossum(void) {
  opFlow.velLpf[X] = 0;
  opFlow.velLpf[Y] = 0;
  opFlow.posSum[X] = 0;
  opFlow.posSum[Y] = 0;

  state.position.x = 0;
  state.position.y = 0;
  state.velocity.x = 0;
  state.velocity.y = 0;

  setpoint.position.x = 0;
  setpoint.position.y = 0;
  setpoint.velocity.x = 0;
  setpoint.velocity.y = 0;
}

（2）光流速度与加速度数据的互补滤波

该互补滤波的核心思想是使用加速度数据计算飞机当前的速度作为估计值，将光流输出得到的速度值作为观测值，两者进行做差，将差值乘上一个修正值，最后将修正值加到估计值上作为速度的最优估计；
此处需要注意的点有三个，一个是加速度需要坐标转换到地理坐标系下，一个是方向对齐问题，一个是修正值大小问题；

关于方向对齐问题，意思就是说：
假设1）飞机向前运行，加速度x方向是正值；
假设2）光流是Y方向表示飞机的前后运动；
则当飞机向前运动，光流Y方向的数值需要是正值；

关于修正值大小问题：
修正值大小短期只能靠试出来；
将加速度的数据和光流原始数据、以及最终融合的数据打印到上位机上，手动移动飞机，感受数据的波动情况，适当调整几个修正值；

	if(opticalflow_present() == true)	/*光流模块可用*/
	{		
		float opflowDt = dt;
		
		float opResidualX = opFlow.posSum[Y] - posEstimator.est.pos.x;
		float opResidualY = opFlow.posSum[X] - posEstimator.est.pos.y;
		float opResidualXVel = opFlow.velLpf[Y] - posEstimator.est.vel.x;
		float opResidualYVel = opFlow.velLpf[X] - posEstimator.est.vel.y;
		
		float opWeightScaler = 1.0f;
		
		float wXYPos = wOpflowP * opWeightScaler;
		float wXYVel = wOpflowV * sq(opWeightScaler);
		
		/* 位置和速度用加速度预估: XY-axis */
		posAndVelocityPredict(X, opflowDt, posEstimator.imu.accelNEU.x);
		posAndVelocityPredict(Y, opflowDt, posEstimator.imu.accelNEU.y);
		/* 位置校正: XY-axis */
		// posAndVelocityCorrect(X, opflowDt, opResidualX, wXYPos);
		// posAndVelocityCorrect(Y, opflowDt, opResidualY, wXYPos);
		/* 速度校正: XY-axis */
		inavFilterCorrectVel(X, opflowDt, opResidualXVel, wXYVel);
		inavFilterCorrectVel(Y, opflowDt, opResidualYVel, wXYVel);
	}

此处将位置的估计注释掉了，因为本文使用速度环控制；

（3)添加遥控器处理；

遥控器处理的目的就是通过roll和pitch方向的拨杆输出roll和pitch方向的期望速度，当拨杆处于中位的死区内时，则将期望的速度设置为0，飞机就是定点了；当拨杆处于非死区内时，则将拨杆的大小除以量程，再乘上速度比例值，就是期望的速度了，飞机于是就可以前后左右的运动；

	//if POSHOLD_MODE set , the hight mode set too
	if (FLIGHT_MODE(NAV_POSHOLD_MODE))
	{
		//初始化定点模式
		if (setupPositionHoldFlag == false)
		{
			setupPositionHold();
			setupPositionHoldFlag = true;
		}

		static bool isAdjustPicth = false;		
		int16_t rcPitchAdjustment = applyDeadbandForPitchRoll(rcCommand[PITCH] , POS_HOLD_DEADBAND);
		if (rcPitchAdjustment == 0 && isAdjustPicth == true)
		{
			//setpoint->mode.x = modeAbs;
			//setpoint->position.x = state->position.x;
			setpoint->mode.x = modeVelocity;
			setpoint->velocity.x  = 0;
			isAdjustPicth = false;
		}
		else
		 if (rcPitchAdjustment > 0)
		{
			stateControlSetVelocityXYPIDIntegration(0);
			setpoint->mode.x = modeVelocity;
			setpoint->velocity.x = rcPitchAdjustment * CLIMB_RATE_MOVE / ( RC_ROLL_PITCH_MID - POS_HOLD_DEADBAND);
			isAdjustPicth = true;
		}
		else if(rcPitchAdjustment < 0)
		{
			stateControlSetVelocityXYPIDIntegration(0);
			setpoint->mode.x = modeVelocity;
			setpoint->velocity.x = rcPitchAdjustment * CLIMB_RATE_MOVE / (RC_ROLL_PITCH_MID - POS_HOLD_DEADBAND);
			isAdjustPicth = true;
		}

		static bool isAdjustRoll = false;
		int16_t rcRollAdjustment = applyDeadbandForPitchRoll(rcCommand[ROLL], POS_HOLD_DEADBAND);
		if (rcRollAdjustment == 0 && isAdjustRoll == true)
		{
			// setpoint->mode.y = modeAbs;
			// setpoint->position.y = state->position.y;
			setpoint->mode.y = modeVelocity;
			setpoint->velocity.y  = 0;
			isAdjustRoll = false;
		}
		else if (rcRollAdjustment > 0)
		{
			stateControlSetVelocityXYPIDIntegration(0);
			setpoint->mode.y = modeVelocity;
			setpoint->velocity.y = rcRollAdjustment * CLIMB_RATE_MOVE / (RC_ROLL_PITCH_MID - POS_HOLD_DEADBAND);
			isAdjustRoll = true;
		}
		else if(rcRollAdjustment < 0)
		{
			stateControlSetVelocityXYPIDIntegration(0);
			setpoint->mode.y = modeVelocity;
			setpoint->velocity.y = rcRollAdjustment * CLIMB_RATE_MOVE / (RC_ROLL_PITCH_MID - POS_HOLD_DEADBAND);			
			isAdjustRoll = true;
		}
	}
	else
	{
		setpoint->mode.x = modeDisable;
		setpoint->mode.y = modeDisable;
	}

注意:此处没有使用高度环方向的摇杆处理，因为油门方向的遥控处理与roll、pitch的摇杆处理不同

（4)PID控制

最后就是pid控制，这里使用正点原子飞控自带的pid控制函数即可；

注意，正点原子飞控源码没有实现位置环控制，所有没有x、y轴方向独立的pid变量，所以需要另外声明；

	//速度PID
	if (RATE_DO_EXECUTE(VELOCITY_PID_RATE, tick))
	{
		//Z轴
		if (setpoint->mode.z != modeDisable)
		{
			altThrustAdj = pidUpdate(&pid[VELOCITY_Z], setpoint->velocity.z - state->velocity.z);
			
			altHoldThrust = altThrustAdj + commanderGetALtHoldThrottle();
		}
		//XY轴
		if (setpoint->mode.x != modeDisable)
		{
			setpoint->attitude.pitch = pidUpdate(&pid_x_v, setpoint->velocity.x - state->velocity.x);						
		}
		if (setpoint->mode.y != modeDisable)
		{
			setpoint->attitude.roll = pidUpdate(&pid_y_v, setpoint->velocity.y - state->velocity.y);			
		}		
	}

最终的pid值为：

4、总结与展望

本文讲述了使用光流实现无人机的定点功能，虽然最终定点效果尚可，但是还存在以下几点不足；
（1）由于没有融合陀螺仪数据进行像素旋转补偿，飞机在转yaw之后，将出现震荡炸鸡现象；
（2）定点功能使用的是速度环控制，没有位置控制，存在小幅度的漂移；
（3）光流数据与加速度数据的融合效果有待提供，比如精确控制

5、git地址

https://gitee.com/ggxxd/wukong_dronefly.git

6、视频分享

https://www.bilibili.com/video/BV1QP4y1m7hS/?vd_source=6d49f1af36ee59119860204ad2fae52f