一个简单无人机工程的总结

用了好些天才看完一个工程代码，北京中科浩电（好像是这个名字）的无人机源码，msp430的板子，imu是mpu6050，定高用了气压计spl06，遥控传输数据用的nrf24l01，开发平台IAR，小型的无人机没有拍摄功能，没有磁力计，没有光流传感，应该可以作为无人机控制的入门手段，用来了解控制流程和方法，对时序和原理的理解，以及一些必要的算法的运用。
不知道该按照什么顺序来整理，因为自己现在也还顺不清，只能想到什么写什么了。。。尽量通俗

一、主函数、核心函数和各个部分的初始化
主函数就很简单，虽然看过实际工程不多，但好像主函数都要很简洁？
首先关中断，然后运行硬件初始化函数，然后开中断
再然后就是while 1的一个循环，循环里只有一个函数PollingKernel()，意思是对核心函数进行轮询。
然后就没了。

先看初始化函数里干了啥，复制代码看起来太麻烦，直接上图了：

可以看到也是调用了各个部分的初始化函数，具体包括：
系统时钟（统一时钟系统，一般不需要我们改动）、
IIC通信协议（IO口配置）、
电机（其实就是四个引脚和时钟是PWM输出配置）、
LED（LED主要作为状态指示灯，比如准备状态、锁定状态等等）、
MPU6050（对里面的一些配置寄存器赋值，一般是默认值，应该不需要我们改）
spl0601（配置各种寄存器，同时对管理气压数据的全局变量进行初始化，比如校准参数、对地面海拔进行读取等等）
NRF24l01也就是无线收发部分（这个模块就有点复杂，寄存器控制字和地址我现在都还没看懂，而且也懒得看（）直接看代码就是设置首发地址、设置模式、以及管理nrf模块的全局变量的初始化等等。。。）
地面工作站初始化（对一个地面信息结构体进行一些初始化，好像所有信息在这个里面都有提及）

PID初始化（对角度角速度pid的一些参数进行设置）
这里还把模式设置成了Altitude_Hold，定高模式？
串口和时钟（就不说了）

好初始化完了，开始进入主轮询函数PollingKernel（）。
还是贴代码吧：

void PollingKernel()
{

    if (Thread_3MS)
    {
        Thread_3MS = false;
        
        GetMPU6050Data();    
        WZ_Est_Calcu(0.003f);
        ATT_Update(&g_MPUManager,&g_Attitude, 0.003f); 
        FlightPidControl(0.003f); 
        MotorControl();
    }

    if (Thread_8MS)
    {
        Thread_8MS = false;

        GetAngle(&g_Attitude);
    }
    
    if (Thread_10MS)
    {
        Thread_10MS = false;
        
        PollingNRF(); 
        PollingGCS();
    }

    if (Thread_20MS)
    {
        Thread_20MS = false;
      
//        //气压计数据采集OK 
        UpdateSPL06Info();
       
//        //更新和选择Z轴观测量
//       WZ_Obs_Calcu(0.02f);

//        //数据融合
        WZ_Fix_Calcu(0.02f);

//        //高度控制
        ALT_Ctrl(0.02f);
       
    }
}

实质上是
1、每3ms：
更新一次mpu6050的数据，
预估下一时刻的高度和速度数据（没太看懂这个操作），
解算出当前姿态信息（三个欧拉角），以及更新出当前Z轴上的加速度（这里没看懂怎么做的），
接下来是个状态机，状态转换大概流程是这样的：
a、开始是等待状态，如果解锁了，（解锁标志位为1），进入下一状态，
b、下一状态即准备状态，在这个状态中，重置pid，把偏置、偏差、积分、期望值、测量值什么的统统置零，航向角因为没有磁力计，怎么放的哪里就是基准（0），代表姿态的四元数也统统置零。这些东西重置好之后，脱离准备状态，进入下一状态，姑且叫进行状态，
c、进行状态中，主要任务也是对pid的一些量进行设置：
装入三个角速度的测量值（物坐标系，直接陀螺仪测出来的）、装入横滚角和俯仰角的角度值（解算之后得到的）、装入航向角的期望值（这里还没太明白，不过应该不怎么变）、利用串级pid处理三个角度和角速度（等下再讲什么是串级pid）、
d、最后一个状态是结束状态，在这个状态里pid被置零，然后跳回等待状态。在进行状态中，是不会自动跳进结束状态的，结束状态的跳转条件由其他条件触发，比如突然锁定等等

最后一步，前面姿态更新了，pid也算出来了，接下来要去改变电机状态了。这里仍然是状态机，甚至跟其前一个函数非常类似，流程如下：
a、等待状态1，如果解锁了，就跳到等待状态2中，一键起飞（应该是这个意思）标志置零，高度锁定标志置零（这些标志是一组全局变量，用来改变和描述飞机状态的），
b、进入等待状态2中，如果此时并未设置一键起飞，也没动油门，那就说明还不想起飞，进入进行状态。否则如果设置了一键起飞，那么就把高度锁定标志位置位，意思是自动定高，也进入进行状态
c、进行状态中，先设定一个临时的变量temp，然后会判断飞机自身的状态，如果是Stabilize_Mode（自稳模式），油门-1000作为准备给pwm输出的值，也就是传给temp。如果是Altitude_Hold（定高模式），将一个全局变量中的油门值作为准备给pwm输出的值，一样是传给temp。这两者的区别在于，自稳模式是自己控制高度，即取决于你遥控的油门，定高模式是自动控制高度，用户无法去控制，它会自己计算出油门值以到达设定的高度。

然后四个螺旋桨处加的pwm怎么确定呢？四个值都要以这个temp为基础，加以微调。为什么要微调？因为temp只跟高度有关，想要无人机姿态平稳或者姿态依据控制做出改变，那么必须各自在temp上叠加一部分，原理是横滚和俯仰运动要相邻转速改变，航向角变化要对角的转速一起改变，有大体的理解就够了，具体是这样的：

这样一来四个电机要输出的值（也就是占空比）就确定了，只要按照这个去改变TIM的比较寄存器就可以了。。。

d、同样是退出状态，进入这个状态后，电机输出清零，无人机停止工作。

2、每8ms，
运行一次GetAngle函数，看名字也知道，是对当前无人机三个姿态角进行解算，以及求物坐标系三个坐标轴相对于地坐标系Z轴的分量，后者对理解飞行状态没有啥意义，不懂也没关系，实质上是程序中用来求解无人机在地坐标系Z轴方向的加速度的过程变量。

3、每10ms
运行一次地面工作站和NRF的轮询函数：
PollingNRF();
PollingGCS();
至于轮询里干了些什么，就不仔细写了，只需要知道是和通信有关的，地面站和无人机肯定要在不断地交换数据对吧。

4、每20ms
更新一次当前的高度信息，也就是当前高度height
然后调用WZ_Fix_Calcu（）函数，算是滤一下波？用估计量修正一下观测量，这部分才注意到，具体等下次再看高度控制的时候详细分析。这部分确实算是目前遇到最难的地方，甚至比姿态解算还难一点，涉及到高度速度和加速度的测量、估计、修正，各种观测值估计值等等，过程中还加了莫名其妙的pid。。。

但总之，这部分主要与定高有关，给定期望高度，算法会不断更新油门值，motorcontrol函数轮询时，如果模式是定高，就会把这个值输出给电机，达到了自动控制高度的目的。

嗯。。。第一部分算是整理完了，但实际上整个控制流程也基本全了（？），剩下就想到什么写什么了，目前还存在的问题就有1、刚刚提到的高度控制部分的代码，要仔细分析出来，2、然后就是通信部分还比较薄弱，串口通信还好，但无线通信这部分，模块太难上手，代码也很迷，有机会要动手做实验，把各种通信都能熟练使用，3、循迹和识别部分，内容估计还是会有一些，但应该不会太难了，先看一遍新飞机的代码再说