程序员经验分享-hwhale

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

2023-05-16

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 <stdbool.h>
#include <string.h>
#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 <stdint.h>
#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

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

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

随机推荐

  • Ubuntu vscode 配置c/c++环境 ---- 静态代码检查

    我曾一度因为vscode中c语言的静态代码检查问题而困扰 xff0c 想想还是太懒了 xff0c 不愿意折腾 xff0c 今天搞一下 首先在ubuntu中装vscode xff0c xff0c xff0c 然后装clang span cla

  • ubuntu22.04 运行qq音乐闪退

    修改 usr share applications中的qqmusic desktop xff0c 在Exec后加上 no sandbox 如下图所示 xff1a

  • PIXHawk用QGC刷Firmware

    1 准备版本 2 开始刷 确定后会出现弹窗 xff0c 再弹窗的文件名处粘贴

  • Xfce4快捷键个性化配置（个人向）

    将xfce4部分常用快捷键设置为Gnome下的快捷键 窗口操作 xff08 在 窗口管理器 gt 键盘 里 xff09 切换同一应用的窗口 xff1a Alt 43 96 最大化窗口 xff1a Super 43 上隐藏窗口 xff1a S

  • 树莓派安装Dronekit连接PIX读取基础数据

    本文主要参考于苍穹四轴DIY的公众号文章 xff1a http span class token operator span span class token operator span span class token operator

  • PX4调试起飞

    一 下载固件 在px4的git上找到相应对于硬件型号的固件 xff1b 二 烧录固件 在qgc上 xff0c 点击高级上的自定义 xff0c 即可烧录相应固件 xff1b 三 选择机架 校准传感器 设置遥控器 选择机架类型之后 xff0c

  • PX4编译——搭建你的第一个应用（Hello Shy）

    一 编译环境 1 ubuntu20 04 2 px4 V2 二 编写测试程序 下载好px4code之后 xff0c 在src examples px4 simple app里面有cmakelist txt和px4 sample app cp

  • PX4开发说明

    本栏文档主要参考PX4的用户指导 xff1b 记录在px4开发过程中的心得体会和备忘 xff1b PX4 User Guide https docs px4 io master en dev setup dev env html

  • prometheus学习

    记录一下在阿木实验室 学习开源项目prometheus的过程

  • Error: No valid host was found.

    使用openstack创建虚拟机经常会遇到以下的这个错误 Error No valid host was found There are not enough hosts available 从字面意思就可以看出是无法找到可用的host的资

  • debian sid 安装 sopcast

    刚刚装了sopcast 由于是编译的 xff0c 所以记录一下以便以后删除干净 http sopcast com download linux html 上有详细说明 1 xff09 下载 sp auth tgz xff0c 把sp sc

  • 2.1 mavros发布位置指令控制px4

    1 说明 写一个节点给px4发送位置控制指令 xff0c 比如我想让飞机飞到10 xff0c 10 xff0c 10这个坐标 xff1b 2 发布和订阅的mavros主题 发布的主题 xff1a mavros setpoint positi

  • 2.2 mavros发布姿指令控制PX4

    说明 使用遥控飞行 px4在stablize模式下 xff0c 我们使用遥控器去控制px4飞行 xff1b 在飞行过程中 xff0c 通常我们用4个通道就可以控制飞机飞行 xff1b 其中roll pitch yaw打杆的量就是我们期望无人

  • 关于PX4上PID调参

    使用PX4 log view 工具 地址 setp response for roll rate 找到setp response for roll rate这个图片 从图片中可以看到 xff0c roll方向的角速度响应时间不够快 xff1

  • 【record】1、Prometheus-V2 初体验

    一 环境搭建 平时习惯使用虚拟机 xff0c 刚好阿木的公众号里面有送镜像 xff0c 于是在V1的时候就用这个镜像在run了 xff0c 这次V2出来 xff0c 直接pull就可以开始起飞了 xff1b xff08 感觉用虚拟机加镜像是

  • 【record】2、使用非官方遥控器适配prometheus的驱动修改

    0 前言 xff1a prometheus V2推荐使用阿木的遥控器 但是家里遥控器实在太多了 xff0c 所以就尝试修改一下prometheus里关于joystick的驱动 xff0c 使其适配prometheus的控制 xff1b 本篇

  • 【recode】3、地面站使用步骤与体验

    一 前言 从Prometheus的V1到V2 xff0c 无人机的状态显示是在终端中 xff0c 在一堆字符中寻找想要关注的信息 xff0c 确实硬核 xff1b 而今 xff0c 随着社会与科技的发展 xff0c Prometheus也开

  • 【recode】4、二维码自主降落与重复测试code修改

    0 前言 使用二维码辅助无人机降落 xff0c 模拟飞机先飞到二维码上空一定的高度 xff0c 然后切换到command control模式 xff1b 飞机会自动识别二维码的位置然后调整自身的X和Y位置信息 xff0c 同时控制高度进行下

  • 【code review】2、关于高度的估计过程

    0 前言 在定高模式中 xff0c 飞控需要有当前高度的信息 xff0c 也就是z的position信息 xff0c 进行Z轴的位置环控制 xff1b 那么这个Z轴的位置信息是怎么来的呢 xff1f 本文为在解读wukong FPV源码中Z

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

    1 说明 xff1a 前几篇文章讲述了如何使用tof的数据实现飞机的定高 xff1b 接下来分享的是如何使用光流来定点 xff1b 主要分为以下几个步骤 xff1a 1 xff09 添加光流驱动 xff0c 获得x y轴方向的观测速度 xf

热门标签