写在前面
深感自己对飞控软件、算法的知识点过于杂乱，很久没有进行系统的总结了，因此决定写几篇文章记录一些飞控开发过程的知识点。主要是针对一些软件、算法部分进行讨论，如内容有错误，欢迎指出。
1 飞控软件的基本模块
无人机能够飞行主要是依靠传感器系统获取位姿信息并反馈到微处理器进行控制系统的运算。所以飞控软件设计主要负责搭建合理软件流程，使各功能模块协调有效的工作。
一个飞控系统的基本工作主要有：
1、CPU接收遥控器的操作指令和传感器信号；
2、传感器的数据处理和数据融合算法运算，得到位置、姿态信息；
3、根据控制指令完成相应的控制器（姿态、位置）计算，得出控制量并输出到电机驱动；
2 软件设计方法的讨论
刚接触飞控的时候，实验室在设计之初，为了方便快捷，软件系统的编写采用前后台操作的方式。这个方式的应用程序是在放在mian主函数里面无限循环，调用相应的处理子函数。这称为后台程序。而前台程序指的就是中断程序处理异步触发事件的程序。故前台程序称为中断级程序，而后台程序称为任务级程序。因此有些固定周期执行的任务都要靠中断服务程序来完成，以保证时间的精确性。但是在中断处理程序中只标记事件的发生，不做任何处理，转而由后台系统调度处理，这是为了避免在中断程序执行时间过长影响后续和其他中断事件。
这种设计方法的优点：
1、实现简单，特别是对于笔者这样的编程渣，照着stm32的库函数写代码，也可简单实现；
2、类似单片机的编程，没有OS，因此对CPU的性能要求不算高，不太关注ROM/RAM；
3、如果设计得当，相较于带OS的飞控，系统运行更加稳定，听说很多工业级的飞控是不带OS的；
缺点：
由于是用在飞行控制系统中，对整个系统的实时性有着很高的要求，如果逻辑和时序出现偏差，将出现无法估计的严重后果。而在初始开发过程中，发现采用此前后台系统带来两大问题：
1、设计不当的话，比如某个周期的函数执行超时，后面所有的程序都会受到影响。如果飞控程序执行时间变得不够准确，不利于对飞行器的控制，严重时发生飞机失控的现象。
2、移植性和扩展性差，给整个程序后续改动和维护带来不便，由于各种任务都是相关的子函数，往往一个任务需要调用多个子函数。在程序改动或者维护的时候变得非常繁琐复杂。经常由于忽略某一细节而导致功能无法实现，最后导致程序的可读性降低，不利于他人做程序修改。
最近几年也接触了一些开源飞控，看了有关带OS的飞控设计。这种设计方法是在某一操作系统上进行二次开发，OS通过一个内核的调度来管理CPU，使得所有的模块也就是任务都能正常运行，达到相对意义的“并行”。同时采用基于优先级的可剥夺性调度算法来保证实时性。RTOS 将应用层软件分成多个任务，简化了应用软件的设计，同时使得飞行控制的实时性得到保证。
3 完整的飞控系统组成模块
当设计一个商业飞控的软件时，就不仅仅是让飞机飞起来那么简单了，也就是说软件模块除了基本要素外，还需有其他扩展，如下图所示。
4 飞控数据流分析
当熟悉了飞控系统中常用的软件模块，那这些模块互相之间又是怎样的关系？又是如何互相通信，各自需要什么数据来完成飞行任务？
以pixhawk飞控的原生固件为例，如下图所示。这图是很早期的代码记录下来的，与最新的代码相比，有些误差，不过差不多，能解释问题，所以笔者懒得更新，重新画图了。
5 示例分析
掌握了以上所描述的几个知识点，这样基本上在初次阅读一份飞控代码时，能有起码的认知了。下面简单介绍两款开源的飞控代码，都是网上找的代码，主要看下软件架构。
5.1 恒拓开源飞控
基于MDK的开发环境，使用C语言，基于STM32的官方库。
代码结构：
STARTUPCODE：stm32的启动文件；
StdPeriph_Driver：基于3.5版本的库函数的驱动文件；
USB-FS-Device_Driver：USB设备驱动文件；
usb_virture_com：USB的板级支持驱动；
Driver：板级驱动层，包含一些总线和外设的驱动程序；
Modules：传感器模块的驱动程序；
Algorithm：算法程序，包含滤波、数学库等；
Function：飞行应用层，关键模块，比如姿态估计、姿态控制等；
User：主程序和中断应用程序；
ANO_DT：支持匿名地面站协议；
Heigh：高度控制程序；
整个代码的模块化非常细致，比较清晰。
代码设计就是前面所讲的裸机代码的一般实现方法。
先看main文件：
非常简单，上电进行各种初始化，然后大循环，循环执行任务调度。
下面看下loop的函数内容。
将整个飞控代码分成了几个周期分别为5ms，10ms，25ms、50ms和100ms的任务。而每个任务的时间标志flag是由一个时间片函数进行管理的。设了一个tick节拍，2.5ms一次，所以比如计数达到2次，则5ms的定时任务即可执行。
而这个时间片函数是一个定时中断，每隔2.5ms执行一次。
这种程序设计方法如下图所示。定时中断的影响只在任务调度模块里起作用，依次让不同的任务按不同的周期进行执行。要注意的是所设计的每个任务运行时间不能超过设定的周期。
笔者也看了国内有名的匿名飞控，也是同样的调度设计方法，另开源ardupilot飞控，因历史原因，是继承APM飞控而来，也是采用这种类OS的伪调度器方式，代码全部顺序执行，根据定时的计数标志去分别安排飞控任务。
5.2 PX4飞控 - Pixhawk原生固件
开源PX4飞控相对复杂多了，很多软件的细节笔者也不甚了解，所以就简单描述下。
PX4飞控从软件架构上可以分为四层。在每一层里，各个驱动程序或上层的控制/估计算法都是一个独立模块，能够在运行期间互相通信。这种模块化的设计不仅有助于支持更多机型(因为不存在特定机型的主循环)，同时使得代码具有高度的可移植性。
1、 应用层：该层是整个飞控系统运行的核心。飞控日常飞行所用到的模块基本上都在这层，包括姿态控制，状态估计，导航模块等等来完成多旋翼和固定翼完全自主的航点飞行。应用层可以使用其他的控制软件，如APM:Plane、APM:Copter，但必须运行于中间层之上。
2、 中间层： 通讯的中间层运行于操作系统之上，提供设备驱动和一个微对象请求代理（micro object request broker ，uORB)用于飞控上运行的单个任务之间的异步通信。
3、 NuttX操作系统层：提供给用户操作环境，进行底层的任务调度。
4、 底层驱动层： 提供系统运行所需要的硬件驱动，如一些传感器、执行器等。
板载程序模块：
uorb通信功能描述
在飞控系统中，应用层所有的功能被独立以进程模块为单位进行实现并工作，而进程间的数据交互就由为重要。为了确保通讯符合实时、有序的特点，需要一种安全的通讯机制。Px4飞控系统采用uORB通讯方式。
uORB(Micro Object Request Broker，微对象请求代理器)是非常重要且关键的一个模块，它肩负了整个系统的数据传输任务，所有的传感器数据、GPS、PPM信号等都要从芯片获取后通过uORB进行传输到各个模块进行计算处理。实际上uORB是一套跨进程的IPC（Inter Process Communication）通讯模块。
uORB的发布-订阅设计模式（publish–subscribepattern）
飞控系统是基于NuttX实时ARM系统，而uORB对于NuttX而言，它仅仅是一个普通的文件设备对象，这个设备支持Open、Close、Read、Write、Ioctl以及Poll机制。通过这些接口的实现，uORB提供了一套“点对多”的跨进程广播通讯机制，“点”指的是通讯消息的“源”，“多”指的是一个源可以有多个用户来接收、处理。而“源”与“用户”的关系在于，源不需要去考虑用户是否可以收到某条被广播的消息或什么时候收到这条消息。它只需要单纯的把要广播的数据推送到uORB的消息“总线”上。对于用户而言，源推送了多少次的消息也不重要，重要的是取回最新的这条消息。
uORB实际上是多个进程打开同一个设备文件，进程间通过此文件节点进行数据交互和共享。
飞控中每个进程都可以通过Publish/Subscribe（发布/订阅）模式与其他进程以及驱动进行连接。以传感器应用程序发送传感器数据到姿态估计应用程序为例。进程（或者称节点）通过命名的总线交换的消息称之为“主题”，在系统中，一个主题仅包含一种消息类型，例如：vehicle_attitude 主题传输包含姿态角（滚转、俯仰和偏航）的消息。节点可以在总线、主题上发布一条消息(Send data) 或者订阅总线、主题（Receive Data）。通讯双方之间并不知道在与谁通讯，可以存在多个发布者或一条消息有多个订阅者，进程的发布和订阅可以在同一时间，主题对象在自身状态变化时，会通知所有订阅者对象，使它们能够自动更新自己的状态。这种设计模式可以防止锁定的问题，常用于机器人技术。为了保证高效，一条总线上始终只有一条消息，并且不使用队列存放。
Px4官网给出的几种常用通讯机制时间延迟对比：
uORB的实现位于固件源码的src/modules/uORB/uORB.cpp文件，它通过重载CDev基类来组织一个uORB的设备实例。并且完成Read/Write等功能的重载。uORB 的入口点是uorb_main函数，在这里它检查uORB的启动参数来完成对应的功能，uORB支持start/test/status这3条启动参数，在飞控系统的rcS启动脚本中，使用start参数来进行初始化，其他2个参数分别用来进行uORB功能的自检和列出uORB的当前状态。
在rcS中使用start参数启动uORB后，uORB会创建并初始化它的设备实例， 其中的实现大部分都在CDev基类完成。通过init调用完成设备的创建，节点注册以及派遣例程的设置等。
下图是一个设备订阅/发布uorb消息的流程图。
底层驱动中类的概念
PX4中不少程序是用C++写的，大量使用了类的概念。如MPU6000这个芯片，与STM32的SPI接口相连（几个传感器都是通过同一个SPI与STM32相连，只是用CS引脚来加一区分）。在src/drivers/device/spi.cpp中定义了SPI类，而在MPU6000的驱动源文件（src/drivers/mpu6000/mpu6000.cpp）中，使用了继承的方法创建了mpu6000类：
class MPU6000 :public device::SPI。其他几个与SPI相连的传感器也是如此进行初始化。
有关px4飞控的软件设计，可以看下官网给出的论文：
Lorenz Meier,Dominik Honegger and Marc Pollefeys. PX4: A Node-Based Multithreaded OpenSource Robotics Framework for Deeply Embedded Platforms, ICRA (Int. Conf. onRobotics and Automation) 2015. (to appear)
6 几个思考
飞控软件中哪些任务优先级高？
很显然就是前面所讲的基本模块，包括遥控输入、传感器数据读取、姿态估计/控制这一类。当然这也设计到飞控的各个控制回路所需的更新速率问题等，后续会详细阐述下飞控中的各个控制回路。关于优先级可以参看px4飞控：
1.（中断级）快速传感器驱动程序
2.看门狗/系统状态监控
3.驱动器输出（PWM输出驱动器线程，IOCOMMS发送命令线程）
4.姿态控制器
5.更新速率慢的传感器驱动程序（不能阻塞姿态控制器）
6.航路/位置控制器
7.默认优先级 - 通用用户代码，shell命令等
8.日志记录，参数同步程序
9.空闲进程
飞控软件的更新周期设计？
飞控中有两个基本的计时：更新周期和延迟。要想获得良好的系统性能，就必须减少延迟。一般情况下，延迟甚至比更新周期更重要—因为大延迟产生相移。延迟造成的相移会让你输出错误的控制量。
所以飞控中，除了设计一个固定的更新周期，还需关注延迟。一个固定频率的控制循环，有时会因为延迟，导致性能很差。举个例子：假设你以100Hz采样速率读取传感器，状态估计和控制器也都是100Hz。那每一步输入的最大延迟/延迟是10ms？
另外，如果分别设计3个循环，传感器读取，状态估计，控制或者将3者放在一个任务循环里，有什么区别？
500hz的读取数据，状态估计，再进行控制和500hz读取数据，状态估计，250hz控制两种方式有区别吗？这3个问题，目前笔者也没有确切的答案，因此就不写了，留作大家思考，可以互相交流看法。
测量函数运行时间？
在飞控算法中，需要用到更新周期这个变量，当然，如果算法运行是固定周期，则变量的值就是所设定的周期，还可以代码自行测量，如下所示。
当然，对算法而言，采用固定的采样率还是计算出来的采样率，个人觉得理论上来讲，计算出来的更准确，应该是更好，当然如果固定的周期不出错，应该也无大的影响。
测量固定周期？
笔者曾经在编写裸机飞控代码时，想确认下所设计的固定周期是否准确，采用了一个笨办法。在周期里，配置一个输出引脚，每执行一次，引脚的电平取反，然后用示波器观察波形，看显示的周期是否与设定的一致，是否会波动变化。如果波动变化很大，说明代码中有任务超时了，整个代码运行出错。
