匿名飞控openmv寻色块解读

/*************************************************************************************
作者：不会写代码的菜鸟
时间：2019.7.26
源码：匿名TI板飞控源码+openmvH4
说明：限于本人水平有限，并不能写的很详细，还望各位能够补充
*************************************************************************************/

文件：Drv.Uart.c
这个文件夹里面写的是串口配置的初始化，大致写法都一样，Openmv接的是串口3，所以这里我们看串口3的驱动
/*************************************************************************************
1.函数：void UART3_IRQHandler(void)；
名称：串口3中断函数（匿名TI板openmv接串口3）
功能：实现串口定时的数据获取
定时器中断实现对openmv传感器数据的获取，com_data=ROM_UARTCharGet(UART)
,OpenMV_Byte_Get(com_data),这两个函数得到openmv的数据
*************************************************************************************/

/*************************************************************************************
2.函数：void Drv_Uart3Init(uint32_t baudrate)；
名称：串口3基本配置
功能：就是一些基本的串口配置
*************************************************************************************/

/*************************************************************************************
3.函数：Drv_Uart3SendBuf(u8 *data, u8 len)；
名称：串口发送函数
功能：通过串口进行一些数据的发送
*************************************************************************************/

文件：Drv_OpenMv.c
这个文件夹写的是对从串口获取的数据包进行解析，对Openmv硬件的检测是否正常连接，还有Openmv掉线复位
/*************************************************************************************
1.函数：static void OpenMV_Data_Analysis(u8 buf_data,u8 len)
名称：Openmv的数据解析
功能：用于对从串口获取的数据进行解析
if((buf_data+3)==0x41)
{
opmv.cb.color_flag = (buf_data+5);
opmv.cb.sta = (buf_data+6);
opmv.cb.pos_x = (s16)(((buf_data+7)<<8)|(buf_data+8));
opmv.cb.pos_y = (s16)(((buf_data+9)<<8)|(buf_data+10));
}，对于这一部分代码可以先看下Openmv源码的数据打包，0x41是openmv里面数据打包时点检测特有的功能字，如果解析到数据包里面有0x41，说明下面代码要进行的是对于色块追踪而处理的代码，之后里面的都是将数据包里面的参数传出来
*opmv.cb.color_flag(找寻的颜色)
*opmv.cb.sta（openmv正常检测到色块后标志位）
*opmv.cb.sta(openmv正常检测到色块后的标志位)
*opmv.cb.pos_x （寻到的色块的中心x坐标）
*opmv.cb.pos_y（寻到的色块的中心y坐标）
对于这一部分代码结合openmv的数据打包代码更易于理解
************************************************************************************/

/*************************************************************************************
2.函数：void OpenMV_Offline_Check(u8 dT_ms)
名称：opnmv掉线检测
功能：用于检测openmv是否还正常连接到主控板
*************************************************************************************/

/*************************************************************************************
3…函数：static void OpenMV_Check_Reset()
名称：opnmv掉线检测复位
功能：当openmv掉线后重新复位openmv，相当于看门狗的作用
*************************************************************************************/

文件：Ano_OPMV_CBTracking_Ctrl.c
这个文件里面写的是当从串口获取到Openmv数据后，并经过数据包解析之后，将解析得到的数据开始进行飞行处理，实现色块追踪的功能
/*************************************************************************************
1.函数：static void ANO_CBTracking_Decoupling(u8 *dT_ms,float rol_degs,float pit_degs)
名称：色块跟踪解耦合
功能：openmv检测到色块后，利用一些数学换算，以及滤波结合IMU得到的Pitch，rol，解算出平移偏移量
*opmv.cb.sta（这个标志位是串口解析数据包得到的flag，检测到色块点位1）
*ano_opmv_cbt_ctrl.opmv_pos[0] = opmv.cb.pos_y;
*ano_opmv_cbt_ctrl.opmv_pos[1] = -opmv.cb.pos_x（这两句是将串口解析数据包得到的色块中心点的坐标换算到飞机的机体坐标）
*ref_carrier_velocity[0] = VELOCITY_CMPS_X
*ref_carrier_velocity[1] = VELOCITY_CMPS_Y（VELOCITY_CMPS_X,VELOCITY_CMPS_Y，这两个值是通过光流的图像处理得到的水平x,y的平移速度）
*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】（这两个值是最后这个函数得出的平移偏移量，这两个值将用到下面一个函数的色块跟踪计算处理）
************************************************************************************/

/*************************************************************************************
2.函数：static void ANO_CBTracking_Calcu(u8 *dT_ms,s32 relative_height_cm)
名称： 色块跟踪计算处理
功能：这个函数主要是利用前面色块跟踪解耦合函数得到的偏差，以及openmv的值将偏差转换到实际的距离上去
*g_pos_err_old[0]（这个数组的0，1分别是x,y轴的，在这里就说x轴，这个是过去偏差，用于后面要用到的微分）
*ano_opmv_cbt_ctrl.ground_pos_err_h_cm[0]（这个是根据实际得到的数值进行解算得到的实际偏差）
ano_opmv_cbt_ctrl.ground_pos_err_d_h_cmps[0] = (ano_opmv_cbt_ctrl.ground_pos_err_h_cm[
0]- g_pos_err_old[0])(1000/(*dT_ms));(这个是对平移偏差进行微分)
*ano_opmv_cbt_ctrl.target_gnd_velocity_cmps[0]（这个是参考传感器测到的高度计算用于下面对偏差的前馈控制）
这个函数计算出来的值都是用于下面实际跟踪任务控制的前馈+PD的处理
************************************************************************************/

/*************************************************************************************
3.函数：void ANO_CBTracking_Ctrl_Task(u8 dT_ms)
名称：色块跟踪任务控制
功能：这个函数主要是用上面计算处理得出的结果进行前馈+PD的控制
*switchs.of_flow_on（这个标志位是光流正常连接的时候置1）
*switchs.opmv_on（这个标志位是openmv连接正常的时候置1）
*ano_opmv_cbt_ctrl.exp_velocity_h_cmps[0]（只分析了x轴的，y轴的也是同理，这个值是最终的控制后的偏差）
= CBT_KF *ano_opmv_cbt_ctrl.target_gnd_velocity_cmps[0] （前馈值x前馈系数）
+CBT_KP *(ano_opmv_cbt_ctrl.ground_pos_err_h_cm[0])（比例系数x偏差）
+CBT_KD *ano_opmv_cbt_ctrl.ground_pos_err_d_h_cmps[0]（微分系数x差值）
*ano_opmv_cbt_ctrl.exp_velocity_h_cmps[0] = ano_opmv_cbt_ctrl.exp_velocity_h_cmps[1] = 0（如果没有偏差就赋0，保持原位不动）
*Program_Ctrl_User_Set_HXYcmps(ano_opmv_cbt_ctrl.exp_velocity_h_cmps[0],ano_opmv_cbt_ctrl.exp_velocity_h_cmps[1])（将最终得出的偏差结果放到程控函数里面进行飞控的自主飞行，也就是利用openmv，脱离遥控器进行前后左右等的飞行）
*************************************************************************************/

文件：Ano_ProgramCtrl_User.c
这个文件里面的函数是计算出x,y,z,yaw的平移速度，之后用到FlightCtrl.c文件里面进行飞机的自主飞行
/*************************************************************************************
1.函数：void Program_Ctrl_User_Set_HXYcmps(float hx_vel_cmps,float hy_vel_cmps)
名称：水平坐标系程控速度功能函数
功能：程控功能，航向水平坐标系下速度设定（实时控制），X速度（厘米每秒，正为前进，负为后退，Y速度（厘米每秒，正为左移，负为右移）
*************************************************************************************/

/*************************************************************************************
2.函数：void Program_Ctrl_User_Set_Zcmps(float z_vel_cmps)
名称：通用程控速度功能接口函数，速度（厘米每秒，正为上升，负为下降）
功能：程控功能，上升下降速度设定（实时控制）
************************************************************************************/

/*************************************************************************************
3.函数：void Program_Ctrl_User_Set_YAWdps(float yaw_pal_dps)
名称：YAW角程控功能接口函数
功能：程控功能，航向速度设定（实时控制），速度（度每秒，正为右转，负为左转）
*************************************************************************************/
这个文件里面的函数得出的是各个方向的平移速度，之后在FlightCtrl.c文件里面去查看得出的结果的使用