匿名无人机飞控代码整理

看了半天，各部分都看得都很零散，写的也很简略，但目前为止还是无法对系统各部分做具体一点的整合，今天试试看。。。
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_2l_ct.exp_yaw += att_1l_ct.set_yaw_speed *dT_s;

这里是设置期望航偏角yaw，采用积分的方式。很精髓，跟高度环很像。

对于角度反馈值，直接读imu_data。

然后开始pid，就得到了三个角的期望角速度。

接下来就是内环，也就是角速度控制环。刚刚外环得到了期望角速度，那么根据测量得到的角速度，我们就能得到需要的角加速度，也就是应该输出的油门值的大小。
这个过程就很明了，直接就算出来三个输出，然后乘以电机输出比例系数，得到要输出的油门量：

mc.ct_val_rol =                   FINAL_P *ct_val[ROL];
mc.ct_val_pit = X_PROPORTION_X_Y *FINAL_P *ct_val[PIT];
mc.ct_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的意思。

到此姿态部分就完成了。总结一下的话，后面的流程都是固定的，想要控制飞机，重点在于最开始，也就是外环的期望角度，它决定了飞机的运动状态，由定位环输出，另一方面，只要有了这个期望速度，通过两个角度环，加上观测量，最终就可以
得到电机的输出。
也就是说，我们要实现控制飞机的飞行状态，就要从定位环入手。

4、定位环分析

刚刚说过了，定位是位置固定的意思，也就是这个函数（巨长无比）：

Loc_1level_Ctrl(20,CH_N);

虽然长，而且判断语句多，但实际上由于没有gps和uwb，我们需要阅读的只有一部分。通过对判断语句的分析，五个代码块我们只需要看一个。

另外光流这点也需要理清概念（因为以前一直没有理清），比如代码里哪部分是光流模块传感、哪部分是openmv的光流，光流如何接收以及接收到什么数据，得到的数据如何处理和使用，等等。

先处理下面光流的问题。阅读匿名的光流模块带的pdf，可以发现这个光流模块集成了很多附加的功能。有提供高度数据的激光测距（tof）、光流数据、以及ium也集成在了上面。同时还集成了计算模块，类似于mpu6050的dmp，可以直接输出滤波后甚至解算好的数据。（虽然还不是很清楚，但在飞控程序中有滤波和解算等步骤，暂且认为我们并没有用这些计算模块，而是处理了原始数据，或者只是简单滤波）

接下来开始直接对定位任务进行分析（仅有光流的情况）：

loc_ctrl_1.exp[X] = fs.speed_set_h[X];
loc_ctrl_1.exp[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];

详细看了一遍代码之后，姑且得到的结论是：
第一项是遥控器对运动状态的控制
第二项是上位机对飞机的控制（？）
第三项是用户调用可以调用函数来实现对运动状态的控制，色块追踪任务就调用了这个
因此我们想要编写程序对飞机进行控制，那就是利用第三项。

具体怎么控制，怎么编写控制函数。。。明天继续