匿名飞控openmv寻色块代码分析
本人最近一段时间在学习匿名飞控（使用的是匿名拓空者），也去网上看了很多资料和一些大佬的见解，学到了一些东西，所以想和大家一起学习讨论一下，互帮互助，共同进步。同时也是为了方便查找，防止遗忘。若是我有什么理解不对的地方，大家可以指出来。
1.首先先看Drv_OpenMV.C这个C文件`
void OpenMV_Byte_Get(u8 bytedata)
{ static u8 len = 0,rec_sta;u8 check_val=0;//openmv_buf[rec_sta] = bytedata;//if(rec_sta==0){if(bytedata==0xaa){rec_sta++;}else{rec_sta=0;}}else if(rec_sta==1){if(1)//(bytedata==0x29)未确定{rec_sta++;} else{rec_sta=0;} }else if(rec_sta==2){if(bytedata==0x05){rec_sta++;}else{rec_sta=0;} }else if(rec_sta==3){if(bytedata==0x41 || bytedata==0x42){rec_sta++;}else{rec_sta=0;} }else if(rec_sta==4){//len = bytedata;if(len<20){rec_sta++;} else{rec_sta=0;}}else if(rec_sta==(len+5)){//for(u8 i=0;i<len+5;i++){check_val += openmv_buf[i];}//if(check_val == bytedata){//解析成功OpenMV_Data_Analysis(openmv_buf,len+6);//rec_sta=0;}else{rec_sta=0;} }else{// rec_sta++;}}
很明显，这个函数的作用没有什么好说的，就是从串口中获取openmv的字节数据然后对数据进行解析，获得四轴需要的数据。
static void OpenMV_Data_Analysis(u8 *buf_data,u8 len)
{/*0x41为openmv检测色块时特有的功能位*/if(*(buf_data+3)==0x41){/*找寻的颜色*/opmv.cb.color_flag = *(buf_data+5);/*openmv正常检测到色块后的标志位*/opmv.cb.sta = *(buf_data+6);/*寻找到的色块中心X,Y坐标*/opmv.cb.pos_x = (s16)((*(buf_data+7)<<8)|*(buf_data+8));opmv.cb.pos_y = (s16)((*(buf_data+9)<<8)|*(buf_data+10));opmv.cb.dT_ms = *(buf_data+11);//色块追踪模式opmv.mode_sta = 1;//f.send_omv_ct = 1;}/*openmv飞控循迹功能字*/else if(*(buf_data+3)==0x42){/*巡线检测标志位*/opmv.lt.sta = *(buf_data+5);//偏航角度opmv.lt.angle = (s16)((*(buf_data+6)<<8)|*(buf_data+7));//偏移距离opmv.lt.deviation = (s16)((*(buf_data+8)<<8)|*(buf_data+9));//是否悬停标志位opmv.lt.p_flag = *(buf_data+10);//中心点x坐标opmv.lt.pos_x = (s16)((*(buf_data+11)<<8)|*(buf_data+12));//中心点y坐标opmv.lt.pos_y = (s16)((*(buf_data+13)<<8)|*(buf_data+14));opmv.lt.dT_ms = *(buf_data+15);//巡线模式opmv.mode_sta = 2;//f.send_omv_lt = 1;}//OpenMV_Check_Reset();
}
大家可以注意到，这个函数已经在第一个函数中被调用过了，而且根据注释的意思可以明白这个函数的作用就是对openmv发送过来的数据进行解析，得到相应的数据。因为在这里我们研究的是寻色块，所以主要对寻色块部分代码进行分析。
void OpenMV_Offline_Check(u8 dT_ms)
{if(offline_check_time<OPMV_OFFLINE_TIME_MS){offline_check_time += dT_ms;}else{opmv.offline = 1;opmv.mode_sta = 0;}}
这个函数也很简单，主要作用就是用于检测openmv是否连接到主控板。
static void OpenMV_Check_Reset()
{offline_check_time = 0;opmv.offline = 0;
}
这个函数的作用是openmv掉线后重新复位openmv。
总体来说，这个c文件的内容非常简单容易理解。接下来我们要去查找解析过后的数据在哪里被调用，如何被调用。也就是确立从视觉识别到飞行控制等一系列流程是如何通过代码实现的。
2.Ano_OPMV_CBTracking_Ctrl.c文件
#define IMU_ROL (imu_data.rol) //横滚角
#define IMU_PIT (imu_data.pit) //俯仰角
#define RELATIVE_HEIGHT_CM (jsdata.valid_of_alt_cm) //相对高度
//
#define OF_DATA_SOURCE ((sens_hd_check.of_ok)?1:0)
#define VELOCITY_CMPS_X_S1 (OF_DX2FIX) //载体运动速度h_x
#define VELOCITY_CMPS_Y_S1 (OF_DY2FIX) //载体运动速度h_y
#define VELOCITY_CMPS_X_S2 (of_rdf.gnd_vel_est_h[0]) //载体运动速度h_x
#define VELOCITY_CMPS_Y_S2 (of_rdf.gnd_vel_est_h[1]) //载体运动速度h_y
//
#define CBT_KP (0.80f) //比例项
#define CBT_KD (0.20f) //微分项
#define CBT_KF (0.20f) //前馈项//需要操作赋值的外部变量：//===============================
//全局变量：
static u16 target_loss_hold_time;
_ano_opmv_cbt_ctrl_st ano_opmv_cbt_ctrl;
static s16 ref_carrier_velocity[2];
static float decou_pos_pixel_lpf[2][2];//参数设定?
/**/
#define PIXELPDEG_X 2.4f //每1角度对应的像素个数，与分辨率和焦距有关，需要调试标定。//3.2f->(160*120,3.6mm) 2.4f->(160*120,2.8mm)
#define PIXELPDEG_Y 2.4f //每1角度对应的像素个数，与分辨率和焦距有关，需要调试标定。
#define CMPPIXEL_X 0.01f //每像素对应的地面距离，与焦距和高度有关，需要调试标定。//目前粗略标定
#define CMPPIXEL_Y 0.01f //每像素对应的地面距离，与焦距和高度有关，需要调试标定。
#define TLH_TIME 1000 //判断目标丢失的保持时间。
一进入这个c文件我就感觉头疼，看着这满屏幕的定义我脑子里都是“这是啥，这又是啥”。不过没关系，我们大体浏览一下接着往下看，说不定下面很简单呢。
void ANO_CBTracking_Task(u8 dT_ms)
{//开启控制的条件，可以自己修改//为色块追踪模式if(opmv.mode_sta==1 && opmv_ct_sta.en){//跟踪数据旋转解耦合ANO_CBTracking_Decoupling(&dT_ms,IMU_ROL,IMU_PIT);//跟踪数据计算ANO_CBTracking_Calcu(&dT_ms,(s32)RELATIVE_HEIGHT_CM);}else{//resetano_opmv_cbt_ctrl.target_loss = 1;}
}
这个函数的作用就是经过一系列的数学处理，计算出四轴追踪色块时的水平速度，简单来说就是实现色块追踪的功能。
static void ANO_CBTracking_Decoupling(u8 *dT_ms,float rol_degs,float pit_degs)
{float dT_s = (*dT_ms) *1e-3f;//有识别到目标if(opmv.cb.sta != 0)//(opmv.cb.color_flag!=0){//ano_opmv_cbt_ctrl.target_loss = 0;target_loss_hold_time = 0;}else{//延迟一定时间if(target_loss_hold_time<TLH_TIME){target_loss_hold_time += *dT_ms;}else{//目标丢失标记置位ano_opmv_cbt_ctrl.target_loss = 1;}}//将串口解析到的色块中心点坐标数据换到飞控坐标系ano_opmv_cbt_ctrl.opmv_pos[0] = opmv.cb.pos_y;ano_opmv_cbt_ctrl.opmv_pos[1] = -opmv.cb.pos_x;//if(opmv.cb.sta != 0){//更新姿态量对应的偏移量ano_opmv_cbt_ctrl.rp2pixel_val[0] = -PIXELPDEG_X *pit_degs;ano_opmv_cbt_ctrl.rp2pixel_val[1] = -PIXELPDEG_Y *rol_degs;/*对得到的结果进行限幅*/ano_opmv_cbt_ctrl.rp2pixel_val[0] = LIMIT(ano_opmv_cbt_ctrl.rp2pixel_val[0],-60,60);//高度120pixelano_opmv_cbt_ctrl.rp2pixel_val[1] = LIMIT(ano_opmv_cbt_ctrl.rp2pixel_val[1],-80,80);//宽度160pixel//赋值参考的载体运动速度if(OF_DATA_SOURCE==1){//来源1 ANO_OFref_carrier_velocity[0] = VELOCITY_CMPS_X_S1;ref_carrier_velocity[1] = VELOCITY_CMPS_Y_S1; }else{//来源2 UP_OFref_carrier_velocity[0] = VELOCITY_CMPS_X_S2;ref_carrier_velocity[1] = VELOCITY_CMPS_Y_S2; }}else{//姿态量对应偏移量保持不变//参考的载体运动速度复位0ref_carrier_velocity[0] = 0;ref_carrier_velocity[1] = 0;}//if(ano_opmv_cbt_ctrl.target_loss==0) //有效，没丢失{//得到平移偏移量，并低通滤波decou_pos_pixel_lpf[0][0] += 0.2f *((ano_opmv_cbt_ctrl.opmv_pos[0] - ano_opmv_cbt_ctrl.rp2pixel_val[0]) - decou_pos_pixel_lpf[0][0]);decou_pos_pixel_lpf[0][1] += 0.2f *((ano_opmv_cbt_ctrl.opmv_pos[1] - ano_opmv_cbt_ctrl.rp2pixel_val[1]) - decou_pos_pixel_lpf[0][1]);//再滤一次decou_pos_pixel_lpf[1][0] += 0.2f *(decou_pos_pixel_lpf[0][0] - decou_pos_pixel_lpf[1][0]);decou_pos_pixel_lpf[1][1] += 0.2f *(decou_pos_pixel_lpf[0][1] - decou_pos_pixel_lpf[1][1]);//赋值，这两个值将应用到下一个函数的色块跟踪处理计算，得到的是平移偏移量ano_opmv_cbt_ctrl.decou_pos_pixel[0] = decou_pos_pixel_lpf[1][0];ano_opmv_cbt_ctrl.decou_pos_pixel[1] = decou_pos_pixel_lpf[1][1];}else //丢失目标{// ano_opmv_cbt_ctrl.decou_pos_pixel[0] = ano_opmv_cbt_ctrl.decou_pos_pixel[1] = 0;//低通复位到0LPF_1_(0.2f,dT_s,0,ano_opmv_cbt_ctrl.decou_pos_pixel[0]);LPF_1_(0.2f,dT_s,0,ano_opmv_cbt_ctrl.decou_pos_pixel[1]);}//}
这个函数在实现色块追踪的函数中被调用过，该函数的主要作用就是解耦合。耦合简单来说就是两个事物之间相互作用和相互影响。在这里的耦合会影响到飞控对色块位置的解算，所以要消除这种关系。我们可以分析这种关系产生的原因，因为openmv固定在四轴上，有四轴基础的人都知道四轴的水平移动会引起四轴飞行姿态的改变，而飞行姿态的改变则会导致openmv镜头的移动，从而使得openmv对色块的定位不准确。好了，知道了这层关系后事情就变的简单了，无非就是消除这种关系使openmv发送回来的数据变得更准确而已。这里我会详细分析一下解耦合的算法。
在这里我们假设该图片为openmv识别到的图像。A0为四轴水平时色块的所在位置（也就是色块真实的位置）。但随后四轴会开始追踪色块，引起自身姿态的改变，镜头偏移，导致openmv发回的是A1的坐标。因为我们要获得A0的坐标。由高中物理可知，四轴沿A0A1的运动可以分解为沿roll和pitch方向的运动，据图分析，我们可以列出两个方程。
y1-Δy=y0
x1-Δx=x0
我们可以知道Δx的大小与pitch的姿态量有关，pitch越大，Δx越大，存在一个正比关系。同理Δy与roll也存在正比关系。同时注意到
#define PIXELPDEG_X 2.4f //每1角度对应的像素个数，与分辨率和焦距有关，需要调试标定。//3.2f->(160*120,3.6mm) 2.4f->(160*120,2.8mm)
#define PIXELPDEG_Y 2.4f //每1角度对应的像素个数，与分辨率和焦距有关，需要调试标定。
也就是roll和pitch每偏转一个角度，坐标（像素）就移动相应大小。也就是说我们只要能够得到姿态量就能根据方程知道该色块的真实坐标。得到了色块的真实坐标就完成了解耦合。这就是匿名飞控openmv寻色块的解耦合算法。
static void ANO_CBTracking_Calcu(u8 *dT_ms,s32 relative_height_cm)
{//有效高度static float relative_height_cm_valid;/*过去的偏差*/static float g_pos_err_old[2];//相对高度赋值if(relative_height_cm<500){relative_height_cm_valid = relative_height_cm;}else{//null}//记录历史值g_pos_err_old[0] = ano_opmv_cbt_ctrl.ground_pos_err_h_cm[0];g_pos_err_old[1] = ano_opmv_cbt_ctrl.ground_pos_err_h_cm[1];//得到地面偏差，单位厘米/*这是根据实际得到的数值进行解算得到的实际偏差*/ano_opmv_cbt_ctrl.ground_pos_err_h_cm[0] = CMPPIXEL_X *relative_height_cm_valid *ano_opmv_cbt_ctrl.decou_pos_pixel[0];ano_opmv_cbt_ctrl.ground_pos_err_h_cm[1] = CMPPIXEL_Y *relative_height_cm_valid *ano_opmv_cbt_ctrl.decou_pos_pixel[1];//计算微分偏差，单位厘米每秒float gped_tmp[2];//微分偏差=（实际偏差-过去偏差）cm*（1000/周期）;单位为cm/sgped_tmp[0] = (ano_opmv_cbt_ctrl.ground_pos_err_h_cm[0] - g_pos_err_old[0])*(1000/(*dT_ms));gped_tmp[1] = (ano_opmv_cbt_ctrl.ground_pos_err_h_cm[1] - g_pos_err_old[1])*(1000/(*dT_ms));//增加了对微分的滤波ano_opmv_cbt_ctrl.ground_pos_err_d_h_cmps[0] += 0.2f *(gped_tmp[0] - ano_opmv_cbt_ctrl.ground_pos_err_d_h_cmps[0]);ano_opmv_cbt_ctrl.ground_pos_err_d_h_cmps[1] += 0.2f *(gped_tmp[1] - ano_opmv_cbt_ctrl.ground_pos_err_d_h_cmps[1]);//计算目标的地面速度，单位厘米每秒
// s16 temp[2];
// temp[0] = (s16)(ano_opmv_cbt_ctrl.ground_pos_err_d_h_cmps[0] + ref_carrier_velocity[0]) ;
// temp[1] = (s16)(ano_opmv_cbt_ctrl.ground_pos_err_d_h_cmps[1] + ref_carrier_velocity[1]) ;ano_opmv_cbt_ctrl.target_gnd_velocity_cmps[0] = (ano_opmv_cbt_ctrl.ground_pos_err_d_h_cmps[0] + ref_carrier_velocity[0]) ;ano_opmv_cbt_ctrl.target_gnd_velocity_cmps[1] = (ano_opmv_cbt_ctrl.ground_pos_err_d_h_cmps[1] + ref_carrier_velocity[1]) ;
}
这个函数首先将解耦合函数中得到的真实偏移量（像素）换成真实偏移量（厘米），其中涉及到的数学运算在文末会给大家推演一遍。这个函数最后算出的结果用于下面函数的前馈控制。
void ANO_CBTracking_Ctrl(u8 *dT_ms,u8 en)
{//开启控制if(en){//距离偏差PD控制和速度前馈，该值为最终控制后的偏差//最终偏差=反馈系数*反馈值+比例系数*偏差+微分系数*差值ano_opmv_cbt_ctrl.exp_velocity_h_cmps[0]\= CBT_KF *ano_opmv_cbt_ctrl.target_gnd_velocity_cmps[0]\+ CBT_KP *(ano_opmv_cbt_ctrl.ground_pos_err_h_cm[0])\+ CBT_KD *ano_opmv_cbt_ctrl.ground_pos_err_d_h_cmps[0];ano_opmv_cbt_ctrl.exp_velocity_h_cmps[1]\= CBT_KF *ano_opmv_cbt_ctrl.target_gnd_velocity_cmps[1]\+ CBT_KP *(ano_opmv_cbt_ctrl.ground_pos_err_h_cm[1])\+ CBT_KD *ano_opmv_cbt_ctrl.ground_pos_err_d_h_cmps[1];}else{//如果没有偏差就赋0，保持原位不动ano_opmv_cbt_ctrl.exp_velocity_h_cmps[0] = ano_opmv_cbt_ctrl.exp_velocity_h_cmps[1] = 0;}} 这是该C文件的最后一个函数，就是前馈+PD控制算出x,y的速度，很简单。该C文件的作用就是将openmv发送回来的数据建立飞行控制追踪到目标色块。
最后来到
3.Ano_OPMV_Ctrl.c文件
void ANO_OPMV_Ctrl_Task(u8 dT_ms)
{//判断高度标记的条件if( RELATIVE_HEIGHT_CM >40){//起飞大于40厘米，标记置位opmv_ct_sta.height_flag =1;}//复位高度标记的条件if(flag.unlock_sta ==0){//上锁后标记复位opmv_ct_sta.height_flag =0;}//判断开启条件（可自行设计）if(switchs.of_flow_on //光流有效&& switchs.opmv_on //mv有效&& opmv_ct_sta.height_flag !=0 //高度标记不为0&& flag.flight_mode2 == 1 //AUX2通道模式值为1，参考FlightCtrl.c内程序){opmv_ct_sta.en = 1;}else{opmv_ct_sta.en = 0;}//不同模式执行不同功能//当模式为色块追踪时，只进行pit、roll程控if(opmv.mode_sta==1){//opmv_ct_sta.reset_flag = 0;//ANO_CBTracking_Ctrl(&dT_ms,opmv_ct_sta.en);//调用用户程控函数赋值控制量//openmv的水平速度控制Program_Ctrl_User_Set_HXYcmps(ano_opmv_cbt_ctrl.exp_velocity_h_cmps[0],ano_opmv_cbt_ctrl.exp_velocity_h_cmps[1]);}//当模式为巡线模式时同时进行pit、yaw、roll程控else if(opmv.mode_sta == 2){//opmv_ct_sta.reset_flag = 0;//ANO_LTracking_Ctrl(&dT_ms,opmv_ct_sta.en);//调用用户程控函数赋值控制量//openmv的水平速度控制Program_Ctrl_User_Set_HXYcmps(ano_opmv_lt_ctrl.exp_velocity_h_cmps[0],ano_opmv_lt_ctrl.exp_velocity_h_cmps[1]); Program_Ctrl_User_Set_YAWdps(ano_opmv_lt_ctrl.exp_yaw_pal_dps);}else{//resetif(opmv_ct_sta.reset_flag==0){opmv_ct_sta.reset_flag = 1;Program_Ctrl_User_Set_HXYcmps(0,0); Program_Ctrl_User_Set_YAWdps(0); }}}
这个函数也很简单，就是对巡线和寻色块的一个综合控制，就是将xy的速度输出。
最后，这是openmv从像素到厘米单位转换的数学推演：
首先，我们假设偏移量为x，像素为p，openmv镜头中的距离为d，高度为h，夹角为θ，如图
当高度固定时，实际的偏移量为 (d/p)*x cm，其中d/p单位为 cm/像素 。当高度变化时，d会发生改变。d/p会发生变化，因此我们要计算一种所有高度都通用的计算方法。 首先我们根据图中列出一个方程：
tanθ=2h/d
之后我们假设四轴高度变化为H，距离变化为D，θ不会改变，同理可以列出一个方程：
tanθ=2H/D
我们要求的是D，因为知道了D，就可以通过(D/P)*x算出实际偏移量了。
所以联立两个方程，消去θ，得到
D=（H/h）*d
所以实际的偏移量为[(d/h)*1/p]xH
分析公式会发现，只有x,H为变量，d,h为第一次测量出来的数据，为定值，p是像素，为定值，所以可以将第一项看作是一个常数。
//得到地面偏差，单位厘米/*这是根据实际得到的数值进行解算得到的实际偏差*/ano_opmv_cbt_ctrl.ground_pos_err_h_cm[0] = CMPPIXEL_X *relative_height_cm_valid *ano_opmv_cbt_ctrl.decou_pos_pixel[0];ano_opmv_cbt_ctrl.ground_pos_err_h_cm[1] = CMPPIXEL_Y *relative_height_cm_valid *ano_opmv_cbt_ctrl.decou_pos_pixel[1];
这就是这部分代码的计算。
匿名飞控openmv寻色块代码分析相关推荐
- 匿名飞控码STM32版代码整理之Ano_AttCtrl.c
代码 #include "Ano_AttCtrl.h" #include "Ano_Imu.h" #include "Drv_icm20602.h&q ...
- ANO匿名飞控STM32代码解读（一）任务调度——Ano_Scheduler.c
我所学习的代码是匿名飞控使用STM32芯片ANO_PioneerPro-20190825的版本. 匿名飞控的整体代码是跑裸机的,任务调度是用STM32F4芯片中的系统时钟计时,做了一个任务调度系统,举 ...
- ANO匿名飞控分析（2）— 任务调度
准备电赛,简单写一下匿名飞控的分析 基于TM4C主控的匿名拓空者飞控,介绍见匿名科创–匿名拓空者PRO-TI版全开源飞控使用入门-TM4C123 一.简介 匿名飞控的任务调度还是比较简单的,没有操作系 ...
- ANO匿名飞控STM32代码解读（二）数据传输——Ano_DT.c
我使用代码对应的上位机版本为v6,具体通信协议格式在打开V6版本上位机–帮助信息–通信协议里可以看到. 这部分数据传输的核心简单的解帧.这部分的讲解以及匿名的通信协议的简介可以看另一篇文章**< ...
- 基于tiva的匿名飞控学习笔记（1）
基于tiva的匿名飞控学习笔记(1) 开关状态任务 遥控器数据处理任务 数传数据交换 延时存储任务 开关状态任务 匿名飞控的开关状态任务为函数Swtich_State_Task(u8 dT_ms),定 ...
- 匿名飞控TI版_姿态解算
匿名飞控TI版_姿态解算 准备电赛 准备大创 先看看匿名姿态解算的代码 文章目录 匿名飞控TI版_姿态解算 一,姿态解算原理 1.介绍 2.方向余弦矩阵 (1)方向余弦 (2)DCM矩阵 3.欧拉角 ...
- 【开源飞控】匿名飞控TI版解析（1）
准备电赛的飞控题,买来了匿名的飞控学习一下,这里整理了一下匿名飞控中比较关键的几部分,学习了一下原理,然后代码解读都写注释里了,篇幅较长. 目录 一.遥控器信号接收 1.代码解读 2.ppm原理 二. ...
- 匿名飞控修改pwm输出脉宽和频率
项目中要用到飞控板去控制一个单独的pwm电子开关,但是这个电子开关只有在输入特定脉冲频率和脉宽的作用下会执行开关作用.因为也是一直做上层算法,缺少底层开发经验,网上也没有现成的技术文档告诉我怎么改,只 ...
- 匿名飞控TI版_PID部分，串级PID，微分先行，前馈控制
文章目录 PID介绍 有趣的故事 控制模型 位置式PID和增量式PID 位置式PID 增量式PID 串级PID 前馈控制 微分先行 匿名代码分析 PID介绍 PID介绍 有趣的故事 PID的故事 \s ...
最新文章
- Java基础-绘图技术
- AB1601 烧写程序后上电后无反应
- html5中加亮文本,html实现高亮关键字
- 深入理解HashMap和TreeMap的区别
- mitdump爬取当当网APP图书目录
- Hyper-V第1代虚拟机和第2代虚拟机特性对照表
- 树莓派安装mosquitto提示“MOSQ_1.5” not found
- 复习----使用链表实现队列(先进先出)及迭代
- python画图代码大全-Python实现画图软件功能方法详解
- python和java选择哪个-Python与Java-你首选哪个？
- echarts graph图重叠_借官方关系图尝试下屏蔽鼠标浮在 links 上弹出的提示框
- Python异步高并发批量读取URL链接
- 对称矩形C语言,c语言编程求任意对称正定矩阵的逆。
- 电压源 电流源 置零时的作用
- 论文阅读：A Survey on Why-Type Question Answering Systems
- 【大数据AI人工智能】图数据库的发展脉络与技术演进
- WPF——【关键字：WPF】TextBox输入框提示文字
- 摄像头网络直播方案LiveGBS部署问题
- 校园网页设计成品 学校班级网页制作模板 dreamweaver网页作业 简单网页课程成品 大学生静态HTML网页源码
- 网页文档输出、对话框