匿名无人机飞控代码整理
匿名⽆⼈机飞控代码整理
看了半天，各部分都看得都很零散，写的也很简略，但⽬前为⽌还是⽆法对系统各部分做具体⼀点的整合，今天试试看。。。
1、先从主函数起，进⼊主轮询函数
Main_Task();
这个主函数的调⽤就在⼀个while⾥，循环没有设置延时。但是程序和任务的执⾏是需要时间的，我们通常是需要定时调⽤的，也就是说隔10ms，调⽤⼀次这个任务，就⽐如说是读取某个硬件的数据的任务，需要每10ms读⼀次数据，那就执⾏完这⼀次之后延时10ms，再执⾏下⼀次。
这⾥的进⼊函数后，会发现延时是在⾥⾯的。通过实时时钟，读取当前时间和上次执⾏时间，如果时间差⼤于某个时间差，就执⾏该任务，否则不执⾏，⽤这个⽅式进⾏延时执⾏管理其实也很有效。
但有⼀个⽐较特殊的部分：就是loop_task_0并不是按这种⽅式控制。
if(lt0_run_flag!=0)
{
lt0_run_flag--;
Loop_Task_0();
}
这个任务通过检测lt0_run_flag变量来判断是否应该执⾏。
loop_task_0⽐较特殊是因为它是负责硬件数据读取的任务：
既然是通过判断lt0_run_flag是否为零来决定该任务是否执⾏，那么lt0_run_flag是受谁控制？了发现是在icm20602的中断服务函数⾥lt0_run_flag++的，icm20602就是陀螺仪加速度计模块。
什么时候icm20602会发出中断请求？
查了⼀下datasheet，⽤六级飘过的英语⽔平也能读出，发出中断的场景之⼀就是在数据准备好的时候，也就是说每次数据准备好可以被读取时，向cpu发出中断请求。
其实很容易理解，因为其他任务都是⼀些计算等等，数据就在寄存器⾥，定时调⽤计算、更新结果就可以。但这个任务涉及到对硬件的读取，不是什么时候想读就能读的，要等到硬件给出准备好可以被读取的信号时，才能开启读取任务。
其他⼏个任务的作⽤也列⼀下：
Loop_Task_1：姿态⾓的⾓速度环控制+电机输出控制刘烨与谢娜
Loop_Task_2：获取姿态⾓（不是读取，⽽是根据读取的数据解算出姿态信息）+姿态⾓的⾓控制环
Loop_Task_5：这个就⽐较复杂，包括：遥控器数据处理+飞⾏模式设置+⾼度数据融合+⾼度速度环控制+⾼度环控制+灯光控制
Loop_Task_8：罗盘数据处理（对罗盘数据进⾏解算得到当前航向信息）+程序指令控制（还不知道是⼲嘛的，知道了回来加）+位置速度环控制+openmv检测和⾊块追踪任务
Loop_Task_9：看不懂。。。
后⾯还会有循迹任务。
2、总的执⾏流程就这么简单：不停读数据、不停计算、得出输出量、然后输出
但是实际的控制过程仍然不清楚，想要实现对飞机的控制，还需要对过程了解的⾜够详细。
⽆⼈机的运动状态主要有上升下降、前进后退、左右移动、转向、姿态⾃稳、⾼度⾃稳、定位（位就是位置location）。
我们要做的也就是能够有效的对这⼏个状态进⾏控制，因为理论上这些状态的组合可以实现⼤部分我们想要的功能。
⽽具体想要实现这些功能，我们需要控制的量为：
上升下降：油门增减，油门对应加速度，
前进后退：pitch⾓变化，根据所需的速度不同，pitch⾓变化幅度也不同
电信呼叫转移设置左右移动：roll⾓变化，规律同上
转向：pitch⾓变化，规律⼤概同上？
姿态⾃稳：通过三个⾓的⽬标量和测量量，利⽤pid计算出油门量实现⾃稳
哈尔滨所有小区封闭管理⾼度⾃稳：利⽤测得的当前速度和⾼度，控制油门量
定位：就是利⽤光流传感的数据得到⾃⾝运动状态，通过控制前后左右运动进⾏抑制
接下来就详细分析⼀下各个控制过程，他们的测量值和期望值的来源，控制量的计算过程，然后得到想实现特定的控制要如何对程序修改。玛雅大预言
3、根据上⾯的思路，我们先对姿态部分做分析
姿态数据主要是从icm20602中读取，当然还需要磁⼒计。
读取到的数据放在这个什么什么original⾥⾯，代表原始的未被处理的数据。
然后在此基础上除去偏置、滤波、得到⾓加速度。
再之后，求出⾓加速度的⽬的是对加速度计的结果进⾏修正。最终得到⾓速度、加速度：
这些准备好之后，就是姿态的解算了，这⾥的数学原理实在理解不了的话，可以先放过，总之就是根
据⾓速度和加速度计的修正得到了当前的姿态，暂时以四元数形式保存。保存的位置是⼀个结构体：
仔细看的话可以看出这个结构体很全的涵盖了有关⽆⼈机运动状态的各种数据。
姿态解算完了，有关⾓度的控制才刚刚开始。尽管⼀般对串级pid的调节都是先整定内环再外环，但从⽅便理解的⾓度，肯定先从外环开始讲起。
外环也就是⾓度环，通过期望⾓度和反馈⾓度得到应有的⾓速度。
期望⾓度来⾃定位环的输出，也就是loc_ctrl_1.out[Y]和loc_ctrl_1.out[X]。因为我们的期望⾓，实质上就是我们希望⽆⼈机保持什么样的状态，⽐如倾斜⼀定⾓度以达到前进的⽬的，或者不倾斜到达稳定的⽬的。（如果⽬的是⾃稳要积分先⾏，因为有偏差的话肯定稳不了）
总之得到了roll和pitch的期望⾓度，对于yaw⾓：
set_yaw_av_tmp = (s32)(0.0023f *my_deadzone(CH_N[CH_YAW],0,65) *max_yaw_speed) + (-program_ctrl.yaw_pal_dps) + pc_user.pal_dps_set;
可见是三个量的叠加。这三个量相加的式⼦看过很多次，很关键，但我没看懂。（。。。）
att_1l_ct.set_yaw_speed += LIMIT((set_yaw_av_tmp - att_1l_ct.set_yaw_speed),-30,30);
att_p_yaw += att_1l_ct.set_yaw_speed *dT_s;
这⾥是设置期望航偏⾓yaw，采⽤积分的⽅式。很精髓，跟⾼度环很像。
对于⾓度反馈值，直接读imu_data。
然后开始pid，就得到了三个⾓的期望⾓速度。
接下来就是内环，也就是⾓速度控制环。刚刚外环得到了期望⾓速度，那么根据测量得到的⾓速度，我们就能得到需要的⾓加速度，也就是应该输出的油门值的⼤⼩。
这个过程就很明了，直接就算出来三个输出，然后乘以电机输出⽐例系数，得到要输出的油门量：
<_val_rol =                  FINAL_P *ct_val[ROL];
<_val_pit = X_PROPORTION_X_Y *FINAL_P *ct_val[PIT];
<_val_yaw =                  FINAL_P *ct_val[YAW];
其中mc就是专门管油门的结构体：
typedef struct
{
s32 ct_val_rol;
s32 ct_val_pit;
s32 ct_val_yaw;
s32 ct_val_thr;
} _mc_st;
extern _mc_st mc;
可以看出，三个⾓的和总的，⼀共四个。
分析⼯程代码⼀个很难的地⽅就在于，所定义的变量和结构体的作⽤不明确，虽然命名肯定提供了信息，有的变量和结构体⽐较简单的话也能很快看懂，但仍然有很多看不懂的地⽅，有时结合上下代码也⽆法推断，常常瓶颈也是因此出现。
这⾥的mc也是现在才明⽩，应该是motor control的意思。
到此姿态部分就完成了。总结⼀下的话，后⾯的流程都是固定的，想要控制飞机，重点在于最开始，
江歌遇害案经过13分钟视频也就是外环的期望⾓度，它决定了飞机的运动状态，由定位环输出，另⼀⽅⾯，只要有了这个期望速度，通过两个⾓度环，加上观测量，最终就可以
得到电机的输出。
也就是说，我们要实现控制飞机的飞⾏状态，就要从定位环⼊⼿。
4、定位环分析
刚刚说过了，定位是位置固定的意思，也就是这个函数（巨长⽆⽐）：
Loc_1level_Ctrl(20,CH_N);
虽然长，⽽且判断语句多，但实际上由于没有gps和uwb，我们需要阅读的只有⼀部分。通过对判断语句的分析，五个代码块我们只需要看⼀个。
另外光流这点也需要理清概念（因为以前⼀直没有理清），⽐如代码⾥哪部分是光流模块传感、哪部分是openmv的光流，光流如何接收以及接收到什么数据，得到的数据如何处理和使⽤，等等。
先处理下⾯光流的问题。阅读匿名的光流模块带的pdf，可以发现这个光流模块集成了很多附加的功能。
有提供⾼度数据的激光测距（tof）、光流数据、以及ium也集成在了上⾯。同时还集成了计算模块，类似于mpu6050的dmp，可以直接输出滤波后甚⾄解算好的数据。（虽然还不是很清楚，但在飞控程序中有滤波和解算等步骤，暂且认为我们并没有⽤这些计算模块，⽽是处理了原始数据，或者只是简单滤波）
接下来开始直接对定位任务进⾏分析（仅有光流的情况）：
loc_p[X] = fs.speed_set_h[X];
loc_p[Y] = fs.speed_set_h[Y];
这⾥的fs，是flight state的意思，命名加h，暂时认为是物坐标系的意思。这⾥的期望值是fs.speed_set_h，也就是说，通过对这个变量进⾏调节，⽐如让它等于0，或者等于某个速度值，我们就可以让飞机保持静⽌，或者以某个速度移动。
中国著名画家然后再利⽤光流测得的实际速度值，pid之后就得到了所需要的输出。
那么这个期望值fs.speed_set_h怎么得到的呢？⼀步⼀步回退，⼜发现这个式⼦：
speed_set_tmp[X] = fc_stv.vel_limit_xy *fs.speed_set_h_norm_lpf[X] + program_ctrl.vel_cmps_h[X] + pc_user.vel_cmps_set_h[X];
speed_set_tmp[Y] = fc_stv.vel_limit_xy *fs.speed_set_h_norm_lpf[Y] + program_ctrl.vel_cmps_h[Y] + pc_user.vel_cmps_set_h[Y];
详细看了⼀遍代码之后，姑且得到的结论是：
第⼀项是遥控器对运动状态的控制
第⼆项是上位机对飞机的控制（？）
第三项是⽤户调⽤可以调⽤函数来实现对运动状态的控制，⾊块追踪任务就调⽤了这个
因此我们想要编写程序对飞机进⾏控制，那就是利⽤第三项。
具体怎么控制，怎么编写控制函数。。。明天继续

版权声明：本站内容均来自互联网，仅供演示用，请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系QQ:729038198，我们将在24小时内删除。