浅谈飞控的软件设计

写在前面

开这个专栏的目的主要是深感自己对飞控软件、算法的知识点过于杂乱，很久没有进行系统的总结了，因此决定写几篇文章记录一些飞控开发过程的知识点。主要是针对一些软件、算法部分进行讨论，如内容有错误，欢迎指出。

---

2019.03.02更新

距离专栏的第一篇文章已经快2年半了，最近在回看文章的时候，发现有些地方写得不尽如人意，或亦是之前的技术水平不足，导致对一些问题的理解不深刻。由此，萌生了将已有文章进行更新的想法，同时也不忘初衷，自己能同时回顾一些知识，毕竟年纪大，工作中不常使用便会遗忘。

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：高度控制程序；

整个代码的模块化非常细致，比较清晰。

代码设计就是前面所讲的裸机代码的一般实现方法。

先看mian文件：

非常简单，上电进行各种初始化，然后大循环，循环执行任务调度。

下面看下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、 底层驱动层： 提供系统运行所需要的硬件驱动，如一些传感器、执行器等。

综上，飞控的裸机方案设计，常用的是分时调度机制。将整个系统时间分成若干时间片，用ID进行标识（通常用频率标志），每个时间片内执行相应的功能模块。这就需要事先合理的分配各个软件任务，保证在相应的时间片内不能超时。

飞行模式与状态机设计

以上描述了飞控设计必要的功能模块，以及互相之间的数据流是如何工作的。接下来就是有关飞行模式与状态机的设计。

飞行模式，按照功能需求，进行自定义设计。飞行模式与飞控内部的状态切换以及控制器切换具有密切联系，因此需要事先拟定好，有哪些飞行模式。进而，根据飞行模式，设计状态机，即各种飞行模式之间如何进行切换，包括由地面的加锁状态，到空中的状态如何进行切换。

有关飞行模式和状态机设计，有一个视频讲解

6 几个思考

飞控软件中哪些任务优先级高？

很显然就是前面所讲的基本模块，包括遥控输入、传感器数据读取、姿态估计/控制这一类。当然这也设计到飞控的各个控制回路所需的更新速率问题等，后续会详细阐述下飞控中的各个控制回路。关于优先级可以参看px4飞控：

1.（中断级）快速传感器驱动程序

2.看门狗/系统状态监控

3.驱动器输出（PWM输出驱动器线程，IO COMMS发送命令线程）

4.姿态控制器

5.更新速率慢的传感器驱动程序（不能阻塞姿态控制器）

6.航路/位置控制器

7.默认优先级 - 通用用户代码，shell命令等

8.日志记录，参数同步程序

9.空闲进程

综上，越底层的控制器和关键数据的获取部分，优先级最高，因为这个是飞行器安全的保障。

飞控软件的更新周期设计？

飞控中有两个基本的计时：更新周期和延迟。要想获得良好的系统性能，就必须减少延迟。一般情况下，延迟甚至比更新周期更重要—因为大延迟产生相移。延迟造成的相移会让你输出错误的控制量。

所以飞控中，除了设计一个固定的更新周期，还需关注延迟。一个固定频率的控制循环，有时会因为延迟，导致性能很差。举个例子：假设你以100Hz采样速率读取传感器，状态估计和控制器也都是100Hz。那每一步输入的最大延迟/延迟是10ms？

另外，如果分别设计3个循环，传感器读取，状态估计，控制或者将3者放在一个任务循环里，有什么区别？

500hz的读取数据，状态估计，再进行控制和500hz读取数据，状态估计，250hz控制两种方式有区别吗？这3个问题，目前笔者也没有确切的答案，因此就不写了，留作大家思考，可以互相交流看法。

有关更新周期，主要部分是数据融合算法和控制器的频率设计。基于以下原则。

一般来说，传感器的采样频率尽可能越高越好，比如gyro数据1000hz采样，实际在姿态估计算法中是200hz读取，这样多出来的数据点能用来做滤波处理，尽可能的保存真实信息。

控制器频率则一般根据系统的带宽决定，一般来说，控制频率是带宽的5倍左右（理论推导不详述了，请自行查阅书籍）。

在多旋翼飞行器中，一般包含了4个控制回路，角速度控制、角度控制、速度控制、位置控制。以角速度控制为例，前面gyro滤波器设置的截止频率为30hz，所以最好角速度控制达到150hz以上。当然，这些没有标准的设置，基本合理就可以，底层的角速度控制频率尽可能高些，200~400左右即可

测量函数运行时间？

在飞控算法中，需要用到更新周期这个变量，当然，如果算法运行是固定周期，则变量的值就是所设定的周期，还可以代码自行测量，如下所示。

当然，对算法而言，采用固定的采样率还是计算出来的采样率，个人觉得理论上来讲，计算出来的更准确，应该是更好，比如对控制器的积分计算等会有影响。当然如果固定的周期不出错，并无太大影响，因为时间即使有些许误差，也是比较小的数值。

测量固定周期？

笔者曾经在编写裸机飞控代码时，想确认下所设计的固定周期是否准确，采用了一个笨办法。在周期里，配置一个输出引脚，每执行一次，引脚的电平取反，然后用示波器观察波形，看显示的周期是否与设定的一致，是否会波动变化。如果波动变化很大，说明代码中有任务超时了，整个代码运行出错。

还有一个简单的办法，就是在任务中设置一个计数标志，然后计时，看两者数值是否相差很多。比如50hz的计数标志，计时10s，则计数标志应该是500左右，考虑人工的误差，相差不会很大，如果相差过大，则说明任务超时。