EEDrone开源四旋翼从零开始（9）——控制框架对比分析
其实控制是一件很简单的事情，无非就是状态监测、控制和输出，但是控制出来的效果却是有好有怀，无非就是上面三个步骤的差别。放在四旋翼上，状态监测就是使用更好的传感器；控制就是使用鲁棒性更好的算法；输出就是使用延迟更小效率更高的电机。这其中还有个隐藏的优化项，那就是三者间的延迟，从状态监测到控制间的延迟和从控制到输出间的延迟，后者由于技术的原因无法提高了，但 是前者却是存在着优化的空间。
四旋翼传感器数据的读取到控制时间应该是要越短越好。但是由于操作系统的加入，这方面做得越来越不好了。下面我从CrazyFlie2，APM，PX4，bataflight和匿名领航者来分析对比控制流程。
CrazyFlie2
CrazyFlie2使用的是FreeRTOS操作系统，其控制主要是两个任务，如下图所示：
其中SensorTask任务主要是用来获取传感器数据的，首先是通过xSemaphoreTake函数获取中断的信号量，在传感器数据准备好后（不确定是是采用的MPU6500还是BMI160，设置的采样率为800Hz），通过引脚中断通知mcu，在中断响应函数中发送中断信号量，然后该任务得以运行，首先进入临界区，然后使用IIC读取传感器数据，再将传感器数据通过信号量发送出去，最后退出临界区。
接下来是姿态控制任务，采用的是精确定时1ms的方案，首先通过SensorAcquire函数获取SensorTask任务发送的传感器的数据，然后通过Madgwick算法解算出欧拉角，采用PID控制算法控制姿态，最后通过不同的机架类型（X或+）分配电机的控制量。
CF2的结构非常清晰，但是其中有个非常大的问题，传感器的采样频率比控制频率低，然后传感器信号量是通过信号量传递的，这样就会导致从传感器数据产生到处理会最多导致2ms的延时。
APM
下面来看看APM的框架，APM是没有使用操作系统的，最多算是使用了调度算法。它有个scheduler_tasks表，里面有各个任务的运行周期，但是却没有优先级。有一个频率是400Hz的快速循环（fast_loop()），姿态控制任务就放在这里里面。关于APM的运行流程我这里贴一张官方的图，如下：
APM初始化后，进入主循环，主循环运行频率为400Hz，在任务调度中有各个任务的调度，任务后台一直在读取传感器，包括：惯性传感器、气压计和GPS等；然后将数据进行扩展卡尔曼滤波解算出姿态和位置；然后接收遥控器的控制信息，根据不同的飞行模式进行不同的设置；然后进入位置控制，位置控制为控制的外环，然后根据位置控制的指令进行姿态控制，姿态控制为控制内环；最后根据控制量转换为电机的控制值。
APM中不同的飞行模式有对应着不同的控制算法，下面以自稳模式为例来介绍四旋翼的姿态控制流程，程序首先运行Fligth_mode.cpp中的Set_mode()，在该函数中调用Control_stabilize.cpp中的Stabilize_init()，将模式设置为自稳模式，然后在400Hz的主循环中频率调用Update_flight_mode()，该函数中调用Control_stabilize.cpp中的Stabilize_run()，该函数中调用AC_AttitudeControl.cpp中的input_euler_angle_roll_pitch_euler_rate_yaw()进行角度的控制，同时在主循环中还会调用rate_controller_run()进行角速度控制，最后通过AP_MotorMatrix.cpp文件中的output_to_motors()得出各个电机控制量。
官方说是有个后台在不断的收集数据，不过我还是没有发现哪里有个后台在专门的收集数据，也没有发现有使用UORB。不过可以确认的是IMU设置的芯片采样率是最高的8KHz，所以猜测传感器读取策略是需要时直接读取，由于采样率高，并不会造成太多的延迟。如果有哪位小伙伴知道传感器具体是如何读取的，还请不吝赐教。
PX4
PX4使用了NUTTX操作系统和uORB数据通信，具体框架借用官网的一张图如下：
主要分为三个部分，一个是传感器数据采集，采用的是高频率的采集，然后进行数据验证；之后通过uORB将数据发布，之后就是数据处理与控制了。高频率的采集可以有效的避免错误数据产生干扰，提高系统的容错性，不过实时性和系统的开销会受点影响。
Betaflight
Betaflight（cleanflight）是专门为穿越机和小型四旋翼开发的飞控程序，使用CC3D的硬件，麻雀虽小五脏俱全。Betaflight没有使用操作系统，使用的是程序调度，应该是非抢占式的，也有优先级的概念。主要的控制框架分为三个部分：主循环、加速度值采集和姿态解算。如下图：
Betaflight的主循环采用100Hz循环，最高优先级。设计的相当巧妙，陀螺仪数据的读取和控制放在了一起，通常情况下陀螺仪的数据相当于角速度，相当于姿态控制中的微分部分，直接乘以微分系数就可以了，由于微分本来就是用来超前调节的，所以对实时性要求很高，这里读取陀螺仪数据后直接控制。至于直接读取陀螺仪值当做角速度相比于利用角度值进行微分哪个效果好就不得而知了。不过有个不理解的地方，那就是弄了个控制计数，也就是4次主循环才进行一次控制，这样控制周期就变成了25Hz，这样的设定很不理解。控制采用自适应积分的PID控制。
加速度计值的采集采用1000Hz，这里采用高频率采集主要是用来滤波，不过滤波的效果不得而知。
姿态解算部分也设计的也比较巧妙，将姿态解算另外做了一个线程，姿态角用来做P运算，通常不需要很高的实时性，况且姿态解算本身也不是那么可靠，看是将其拉到控制外面，确实可以大大的减少其控制延迟，最大程度的减少了延迟。姿态解算采用的Mahong算法，占用资源小，速度快。
匿名领航者
匿名领航者是匿名科创目前最新的产品，程序没有使用操作系统也没有使用程序调度，而是使用了一个循环，其中设置不同时间标志，分为1、2、5、10、20和50ms的分组，所有的任务都放到这些组里面，关于控制的主要任务如下：
其控制分为内环姿态控制和外环角度控制，内环是2ms一个周期，外环是5ms一个周期，使用Mahong姿态解算，PID控制算法。
My
分析了这么多飞控的控制流程，也有了自己的想法，仍然是坚持延迟最低原则，由于以后会有高新能传感器，可能会有两种形式，一种是做成导航模块，包含MCU和IMU，直接输出就是姿态信息，需要中断引脚通知主MCU；另一种是采用模拟的芯片加高新能ADC，采用主MCU读取的形式。所以最后的姿态控制流程分为两种情况如下：
传感器IMU情形下将传感器的数据更新设为最高，精确定时2ms，然后获取传感器数值、姿态解算、姿态控制和电机控制量分配一气呵成，这样做可以最大程度的减少延时。其中的控制主要是姿态控制，位置控制不需要如此高的更新频率，所以放在另外的任务中。导航IMU由于本身输出的就是姿态角需要使用引脚中断来进一步减少延时，所以采用中断信号量的形式，但是这样以IMU为时钟节拍也是有风险的，如果IMU的中断引脚出了问题，那么整个系统就不会工作了。
好啦，其主要的控制框架就大概闲聊这些，接下来会对比分析，数据传输，文件结构，结构体定义，任务分配，姿态解算算法，控制方法等。