飞控FirmamentAutopilot介绍

Firmament特色

使用 RT-Thread 嵌入式操作系统，Fatfs 文件系统，完整的系统功能支持 (如跨进程通信，文件管理，参数系统等)
基于 Pixhawk 硬件平台开发，完美支持 Pixhawk 硬件
ADRC 控制和 PID (串级) 控制
支持 Mavlink 协议 (QGC 地面站)
支持 Gazebo 硬件在环 (HITL) 仿真
Msh 指令系统，提供丰富以及可扩展的系统指令
Log 日志系统，方便抓取飞行日志信息，并提供相应的日志查看工具。
基于最小二乘法的最优椭球拟合校准算法 (可校准零偏误差，缩放误差，非正交误差)，可用于磁力计和加速度计的校准。

整个系统除了Pixhawk之外，还有一些外接的电子设备，如无刷电机，GPS，电调，数传，RC接收机，Lidar-Lite激光雷达等。整体的系统框架图如下图所示：

【软件设计】

软件采用分层结构设计，如下图所示，从底层到上层分别是Driver层，RTOS(RTT + Fatfs)，HAL硬件虚拟层，Framework层和应用层。

一、Driver层

        Driver层实现了Pixhawk板载的所有硬件设备的驱动，如传感器设备(陀螺仪，加速度计，磁力计，气压计等)，总线设备(UART, I2C, SPI等)，USB，电机驱动，SD卡驱动，GPS驱动，LED灯以及一些板载设备的驱动。各个驱动以Device的形式向RTT进行注册，并且提供各自的init(), read(), write, ioctrl()函数供上层调用。这里仅以hmc5883磁力计的驱动举例说明驱动程序的一般设计步骤。

- 设备初始化函数

        如下为hmc5883的初始化函数。首先对设备的功能函数进行函数指针赋值，这里定义了init(), read()和ioctrl()三个函数。然后向RTT注册名为hmc5883的驱动设备，上层可以通过查找设备名的方式来获取该驱动设备。接下来就是给hmc5883设置对应的总线设备。这里 hmc5883使用的是i2c通信，所以初始化中会为其查找对应的i2c设备。

- init()函数

Init()函数主要做一些芯片的初始化的工作，包括寄存器配置等。

- read()函数

read()函数实现设备数据读取的功能,如原始磁场数据和标定后的磁场数据读取。

- ioctrl()函数

Ioctrl()函数主要实现一些设备的配置功能，如设置测量磁场的范围，采样频率等。

二、RTOS层

        RTOS采用RT-Thread嵌入式操作系统，它的功能强大，除了提供基本的线程调度，内存管理，同步等基本功能外，还提供了如msh的shell系统，device设备驱动系统，使得系统的整体架构更加清晰，功能也更加完善。

三、HAL层

        HAL层主要是对底层驱动设备进行进一步封装，将具体的设备抽象为一个类别的设备。以串口设备为例，不同的硬件平台，串口的底层驱动会有差异，通过HAL层，将串口设备封装为统一的接口，这样当系统移植到不同平台的时候，上层代码不需要做任何改变，而只需要将对应底层驱动进行简单替换就行。这将大大降低系统移植的复杂程度。

四、Framework层

        Framework层实现了飞控设备的主要功能，如姿态估计，位置估计，控制器，通信，日志管理等等。可以说，这一层是整个系统的核心，大部分的功能模块都在这一层来实现。

五、Application层

        应用层是将飞控的各个大的功能进行分割，变成一个个小的功能。每个功能在系统中为一个单独的线程，每个线程负责执行自己的任务，并且跟其他线程通过IPC进行通信。Vehicle线程具有最高优先级，执行周期为1ms。其负责实现无人机飞行相关的所有功能，如状态估计和电机的控制。

【启动流程】

        下面介绍一下系统的启动流程. 启动流程沿用RTT提供的原始结构。首先在rtthread_startup()函数中进行RTOS的一些初始化，然后进入到rt_application_init()中。在这个函数中会建立一个init的初始线程，在这个线程中，会对飞控各个模块进行初始化的工作。在初始化完成之后，会根据Application层的定义，建立各个线程，并唤醒，最后将init线程自己给销毁掉，避免不必要的内存消耗

FMT Model

        FMT Model 为基于 Matlab/Simulink平台编写的一个完整的仿真模型框架和算法模型库。支持模型在环仿真 (Model-in-the-loop Simulation, MIL)，软件在环仿真 (Software-in-the-loop Simulation, SIL) 和开环仿真 (Open-loop Simulation)。

        FMT Model 的模型库包括一套完整的无人机算法模型。可以分为导航系统，飞行管理系统，控制系统和被控对象模型四个部分。每个部分包含多种类的算法模型，以支持不同的算法和被控对象。

其架构如下图所示：



        其中算法库通过Github Submodule的方式进行组织，用户可以用来独立维护自己的算法模型，并添加到FMT Model的仿真框架中。FMT提供种类丰富而且功能完善的仿真模型，可以很容易的基于已有模型或者提供的模型模板进行二次开发。

FMT Model 算法模型库主要由四类模型组成：

· INS : 惯性导航系统。通过多传感器融合算法得到被控对象的状态信息（如姿态、速度和位置等）以及各个传感器的健康状况信息等。

· FMS : 飞行管理系统。主要负责飞行逻辑相关的控制，内部主要由状态机实现。包括飞行模式控制，自动起飞降落，轨迹跟踪控制，安全检测等功能。

· Controller : 控制器模型。基于 FMS 输出的指令信息和 INS 输出的状态信息进行速度环、姿态环和角速度环的控制。控制器的输出经由控制分配将控制信号转成作动器（电机）信号。

· Plant : 被控对象模型，如无人机，无人车/船，机器人等。对象模型包括了动力学模型，作动器模型，环境模型和传感器模型。通过对不同的被控对象进行科学的建模，以达到对不同被控对象进行闭环仿真的目的。

        可以看到由于引入了被控模型Plant，从而构成了一个完整的闭环。Plant模拟真实世界中的被控对象，如无人机，无人船等，它接收控制器的控制信号，然后更新状态信息（姿态，速度，位置等）并由其内部的传感器模型生成传感器数据供导航模型进行数据融合。

        Plant输出状态信息供Virtualization可视化模块进行3维显示。可视化模块可以使用Simulink自带的Simulink 3D，也可以使用外部的，如UE4，Webots，Gazebo等。

        通过外接遥控输入（Pilot Cmd）或者自行构造输入指令，我们就可以像操控真机一样来对整个系统进行完整功能的仿真。仿真完成后，我们可以通过Simulink Data Inspector来查看任意的数据。

仿真能力

        模型在环仿真和开环仿真，可以大大简化调试难度并优化算法，也更容易发现设计中的问题。特别是开环仿真，能提供“黑匣子”一样的功能，仅需记录少量的模型输入数据，即可通过开环仿真得到算法模型输出和所有内部信号的数据，方便问题定位和算法优化，大大提高测试效率。

代码生成

        基于Simulink开发的模型，其代码生成功能已经非常成熟，生成的代码特别清晰，且比一般的工程师写的代码更好。可以大大提高编码效率，并减少因人为编码而导致的错误。

文档自动化

        基于模型的设计可以帮助我们实现文档自动化，大部分文档可以让计算机帮助我们完成。

遥控输入

遥控信号通过Pilot Cmd总线将数据传输给FMS模型，再由FMS来进行处理。遥控信号的输入源包括：

- Joystick: 通过手柄输入。可以通过接入USB手柄（比如XBox游戏手柄）来产生遥控信号。

- Mavlink: FMT飞控通过Mavlink将飞控端的遥控信号传给Simulink。

- Fake: 通过Simulink滑块构建遥控信号。

- None：无遥控信号输出（使用GCS Cmd进行控制。因为当遥控信号无效时，地面站信号GCS才会有效）