1. FMT飞控简介
FMT (Firmament) 是一款基于模型设计 (MBD, Model-Based Design) 的开源飞控系统，核心特点：
– **Simulink建模 + C代码自动生成**：飞控算法（FMS/INS/Controller）在Simulink中建模，自动生成C代码嵌入固件
– **RT-Thread RTOS**：基于国产实时操作系统，任务调度、IPC机制成熟
– **多机型支持**：多旋翼(Multicopter)、固定翼(Fixwing)、船(Boat)、车(Car)
– **多硬件平台**：Pixhawk v2/v5、CUAV V5+/V5Nano/7Nano、SIEON S1、AMOV ICF5、CubeOrange等
2. 整体分层架构
FMT采用**六层分层架构**，从底层硬件到顶层任务，职责清晰：
2.1 各层职责
| 层级 | 目录 | 职责 |
| BSP层 | target/<vendor>/<board>/ | 时钟配置、MPU配置、外设初始化、链接脚本 |
| Driver层 | src/driver/ | 传感器芯片驱动（BMI088/ICM42688P等），直接操作SPI/I2C寄存器 |
| HAL层 | src/hal/ | 设备抽象接口（gyro/accel/mag等），提供统一的`ops`函数表 |
| Module层 | src/module/ | 功能模块（参数系统、日志、MAVProxy通信、传感器管理等） |
| Model层 | src/model/ | Simulink自动生成的算法代码（FMS/INS/Controller/Plant） |
| Task层 | src/task/ | RT-Thread线程入口，组合各模块完成具体任务 |
2.2 源码目录结构
FMT-Firmware-master/
├── rtos/rt-thread/ # RT-Thread RTOS内核
├── src/
│ ├── driver/ # 芯片级驱动
│ │ ├── imu/ # BMI088, ICM42688P, MPU6000, BMI055…
│ │ ├── mag/ # BMM150, RM3100, QMC5883L, IST8310…
│ │ ├── barometer/ # SPL06, MS5611, BMP581
│ │ ├── gps/ # UBX协议, DroneCAN GPS
│ │ ├── mtd/ # GD25QXX, W25QXX, Ramtron, TF卡
│ │ ├── range_finder/ # TOFSense, TFxx激光测距
│ │ ├── airspeed/ # MS4525空速计
│ │ ├── rgb_led/ # AW2023, NCP5623C等LED驱动
│ │ ├── vision_flow/ # PMW3901光流, MTF-01
│ │ └── uwb/ # NLink LinkTrack UWB
│ ├── hal/ # 硬件抽象层
│ │ ├── accel/, gyro/, mag/, barometer/ # 传感器HAL
│ │ ├── airspeed/, gps/, rc/ # 外设HAL
│ │ ├── serial/, spi/, i2c/, can/ # 总线HAL
│ │ ├── actuator/, adc/ # 输出/采集HAL
│ │ ├── usb/, sd/, mtd/, pin/ # 存储/GPIO HAL
│ │ ├── fmtio_dev/ # FMT IO协处理器设备
│ │ └── systick/ # 系统滴答
│ ├── module/ # 功能模块
│ │ ├── task_manager/ # 任务管理器(核心!)
│ │ ├── ipc/ # uMCN发布订阅(核心!)
│ │ ├── sensor/ # 传感器融合中心
│ │ ├── param/ # 参数系统
│ │ ├── log/ # MLog/ULog日志
│ │ ├── mavproxy/ # MAVLink通信代理
│ │ ├── sysio/ # 系统I/O(cmd控制)
│ │ ├── syscmd/ # Shell命令系统
│ │ ├── console/ # 控制台
│ │ ├── pmu/ # 电源管理/电池SOC
│ │ ├── math/ # 四元数/旋转/坐标变换
│ │ ├── filter/ # Butterworth滤波器
│ │ ├── workqueue/ # 工作队列
│ │ ├── utils/ # 环形缓冲区等工具
│ │ ├── system/ # 系统时间/统计
│ │ ├── toml/ # TOML配置解析
│ │ └── dronecan/ # DroneCAN协议
│ ├── model/ # Simulink生成的算法模型
│ │ ├── fms/ # 飞行管理系统(FMS)
│ │ │ ├── mc_fms/ # 多旋翼FMS
│ │ │ ├── fw_fms/ # 固定翼FMS
│ │ │ ├── boat_fms/ # 船FMS
│ │ │ ├── car_fms/ # 车FMS
│ │ │ └── template_fms/# 模板(新增机型参考)
│ │ ├── ins/ # 惯性导航系统(INS)
│ │ │ ├── cf_ins/ # 互补滤波INS
│ │ │ ├── px4_ecl/ # PX4 EKF惯导(移植自PX4)
│ │ │ ├── external_ins/# 外部INS接口
│ │ │ └── template_ins/# 模板
│ │ ├── control/ # 控制律(Controller)
│ │ │ ├── mc_controller/ # 多旋翼控制律
│ │ │ ├── fw_controller/ # 固定翼控制律
│ │ │ ├── boat_controller/ # 船控制律
│ │ │ ├── car_controller/ # 车控制律
│ │ │ ├── adrc_controller/ # ADRC控制律
│ │ │ ├── submarine_controller/# 潜艇控制律
│ │ │ └── template_controller/# 模板
│ │ └── plant/ # 被控对象模型(SIH仿真)
│ │ ├── multicopter/ # 多旋翼动力学
│ │ ├── fixwing/ # 固定翼动力学
│ │ ├── boat/ # 船动力学
│ │ └── template_plant/# 模板
│ ├── task/ # 任务层
│ │ ├── vehicle/ # 核心控制任务(最高优先级)
│ │ ├── comm/ # MAVLink通信任务
│ │ ├── logger/ # 飞行日志任务
│ │ ├── status/ # 状态监控任务
│ │ ├── dronecan/ # DroneCAN任务
│ │ └── fmtio/ # FMT IO协处理器任务
│ ├── protocol/ # 协议解析(MSP/UBX/NLink)
│ ├── lib/ # 第三方库(MAVLink/DroneCAN/printf等)
│ ├── startup.c # 系统启动入口
│ ├── firmament.h # 全局头文件
│ └── fmt_def.h # 宏定义/线程优先级/工具宏
├── target/ # 硬件目标板
│ ├── pixhawk/ # Pixhawk系列
│ │ ├── fmu-v2/ # Pixhawk FMUv2 (STM32F427)
│ │ └── fmu-v5/ # Pixhawk FMUv5 (STM32F765)
│ ├── cuav/ # CUAV系列
│ │ ├── v5_plus/ # CUAV V5+ (STM32F765)
│ │ ├── v5_nano/ # CUAV V5 Nano (STM32F765)
│ │ └── 7_nano/ # CUAV 7 Nano (STM32H743)
│ ├── sieon/s1/ # SIEON S1 (STM32H743)
│ ├── cubepilot/cubeorange/# CubeOrange (STM32H757)
│ ├── amov/icf5/ # AMOV ICF5 (GD32F4)
│ ├── qemu/qemu-vexpress-a9/ # QEMU仿真(ARM Cortex-A9)
│ └── minimum/ # 最小系统示例
└── tools/ # 工具脚本
3. 启动流程详解
FMT的启动流程分为**三个阶段**，严格有序执行：
3.1 阶段一：BSP早期初始化 (`bsp_early_initialize`)
在RTOS启动**之前**执行，不在任务上下文中：
main() → rtthread_startup()
→ rt_hw_board_init()
→ bsp_early_initialize()
├── CPU_Config() // MPU配置 + I-Cache使能
├── rt_system_heap_init() // 堆内存初始化
├── HAL_Init() // STM32 HAL库初始化
├── SystemClock_Config() // 时钟树配置(480MHz)
├── drv_gpio_init() // GPIO驱动
├── drv_usart_init() // 串口驱动
├── console_init() // 控制台初始化(可输出日志)
├── drv_systick_init() // 系统滴答
├── drv_i2c_init() // I2C总线
├── drv_spi_init() // SPI总线
├── drv_pwm_init() // PWM输出
├── drv_fdcan_init() // FDCAN总线
├── drv_rc_init() // 遥控器接收
└── sys_stat_init() // 系统统计
此阶段只做最基本的硬件初始化，确保串口可用，后续阶段可以输出调试信息。
3.2 阶段二：BSP初始化 + 任务初始化 (`bsp_initialize` + `task_manager_init`)
RTOS启动**之后**，在init线程中执行：
// startup.c
static void rt_init_thread_entry(void* parameter)
{
bsp_initialize(); // BSP完整初始化
task_manager_init(); // 任务管理器初始化所有任务
bsp_post_initialize(); // BSP后初始化(配置解析)
task_manager_start(); // 启动所有auto_start=true的任务
}
`bsp_initialize()` 的详细流程：
bsp_initialize()
├── systime_init() // 系统时间模块
├── boot_log_init() // 启动日志记录
├── mcn_init() // uMCN发布订阅初始化
├── workqueue_manager_init() // 工作队列
├── drv_sdio_init() // SD卡驱动
├── drv_gd25qxx_init() // Flash驱动
├── file_manager_init() // 文件系统挂载
├── param_init() // 参数系统加载
├── drv_usb_cdc_init() // USB虚拟串口
├── drv_adc_init() // ADC驱动
├── lwip_system_init() // TCP/IP协议栈
├── drv_eth_init() // 以太网驱动
├── 传感器初始化:
│ ├── drv_bmi088_init() // 主IMU
│ ├── drv_icm42688_init() // 备份IMU
│ ├── drv_bmm150_init() // 磁力计
│ ├── drv_spl06_init() // 气压计
│ ├── gps_ubx_init() // GPS
│ └── register_sensor_xxx()// 注册到sensor_hub
├── finsh_system_init() // FinSH Shell
└── console_enable_input() // 使能控制台输入
3.3 阶段三：任务启动 (`task_manager_start`)
任务管理器遍历所有已初始化的任务，启动 `auto_start=true` 的任务：
task_manager_start()
├── mavgcs (priority=12) // MAVLink GCS通信
├── mavobc (priority=11) // MAVLink OBC通信
├── vehicle (priority=3) // 核心控制(最高优先级!)
├── logger (priority=13) // 飞行日志
├── status (priority=14) // 状态监控
├── dronecan (priority=4) // DroneCAN协议
└── fmtio (priority=7) // FMT IO协处理器
`vehicle` 任务优先级最高(数值最小)，确保控制回路的实时性。
4. 构建系统
FMT使用 **SCons** 构建系统，通过Python配置文件控制编译：
4.1 构建入口
# 在target目录下执行
scons –vehicle=Multicopter # 指定机型
scons –vehicle=Fixwing # 固定翼
scons –sim=SIH # 仿真模式
scons –airframe=1 # 机架编号
4.2 配置文件体系
每个target目录下有 `config/` 文件夹，通过Python列表控制编译内容：
# target/sieon/s1/config/driver.py – 选择编译哪些驱动
DRIVERS = [
'barometer/spl06.c',
'gps/gps_ubx.c',
'imu/bmi088.c',
'imu/icm42688p.c',
'mag/bmm150.c',
'mag/qmc5883l.c',
'mtd/gd25qxx.c',
'range_finder/tofsense.c',
]
# target/sieon/s1/config/model.py – 根据机型选择模型
if vehicle_type == 'Multicopter':
MODELS = ['plant/multicopter', 'ins/cf_ins', 'fms/mc_fms', 'control/mc_controller']
elif vehicle_type == 'Fixwing':
MODELS = ['plant/fixwing', 'ins/cf_ins', 'fms/fw_fms', 'control/fw_controller']
elif vehicle_type == 'Boat':
MODELS = ['plant/boat', 'ins/cf_ins', 'fms/boat_fms', 'control/boat_controller']
elif vehicle_type == 'Car':
MODELS = ['plant/fixwing', 'ins/cf_ins', 'fms/car_fms', 'control/car_controller']
FMT源码中支持多旋翼、固定翼、船、车等多种机型。在实际项目中，如果只需要固定翼，可以在二次开发时精简掉不需要的模型。
# target/sieon/s1/config/task.py – 选择编译哪些任务
TASKS = [
'comm/*.c',
'logger/*.c',
'status/*.c',
'vehicle/normal/*.c',
]
4.3 SConscript递归构建
每层目录有 `SConscript` 文件，声明本层的源文件和头文件路径，SCons递归收集所有目标文件：
SConstruct (target/sieon/s1/)
└── 遍历 src/ 下所有目录
└── 每个目录的 SConscript 声明源文件
└── 根据 config/*.py 过滤，决定编译哪些文件
5. 核心设计模式
5.1 Linker Section自动注册
FMT大量使用**链接器段(section)**实现模块自动注册，无需手动维护初始化列表：
// 链接脚本 link.lds
__fmt_task_start = .;
KEEP(*(TaskTab)) // 任务表段
__fmt_task_end = .;
__fmt_param_start = .;
KEEP(*(ParamTab)) // 参数表段
__fmt_param_end = .;
__fmt_mlog_start = .;
KEEP(*(MlogTab)) // 日志表段
__fmt_mlog_end = .;
// 使用宏将结构体放入指定section
#define TASK_EXPORT RT_USED static const struct fmt_task_desc SECTION("TaskTab")
// 注册任务 – 编译时自动放入TaskTab段
TASK_EXPORT __fmt_task_desc = {
.name = "vehicle",
.init = task_vehicle_init,
.entry = task_vehicle_entry,
.priority = VEHICLE_THREAD_PRIORITY,
.auto_start = true,
.stack_size = 3072,
};
**优势**：新增任务只需定义一个 `TASK_EXPORT` 结构体，无需修改任何注册代码。类似Linux内核的 `module_init` 机制。
5.2 发布-订阅IPC (uMCN)
模块间通信使用 uMCN (Micro Mission Computer Network) 发布-订阅机制：
// 定义话题
MCN_DEFINE(sensor_imu0, sizeof(INS_Out_Bus));
// 发布数据
mcn_publish(MCN_HUB(sensor_imu0), &imu_data);
// 订阅 + 等待数据
McnNode_t node = mcn_subscribe(MCN_HUB(sensor_imu0), NULL);
mcn_wait(node, RT_WAITING_FOREVER);
mcn_copy(MCN_HUB(sensor_imu0), node, &buffer);
5.3 面向对象C语言
HAL层使用**函数指针表**实现多态：
// 设备操作接口
struct gyro_ops {
rt_err_t (*gyro_config)(gyro_dev_t gyro, const struct gyro_configure* cfg);
rt_err_t (*gyro_control)(gyro_dev_t gyro, int cmd, void* arg);
rt_size_t (*gyro_read)(gyro_dev_t gyro, rt_off_t pos, void* data, rt_size_t size);
};
// 设备结构体 – 包含ops指针
struct gyro_device {
const struct gyro_ops* ops;
struct gyro_configure config;
// …
};
// 具体驱动实现ops
static const struct gyro_ops __gyro_ops = {
gyro_config,
gyro_control,
gyro_read,
};
6. 线程优先级设计
FMT的线程优先级经过精心设计，确保控制回路实时性：
// fmt_def.h
#define VEHICLE_THREAD_PRIORITY 3 // 控制任务 – 最高!
#define DRONECAN_THREAD_PRIORITY 4 // DroneCAN
#define FMTIO_THREAD_PRIORITY 7 // FMT IO
#define MAVLINK_RX_THREAD_PRIORITY 10 // MAVLink接收
#define MAVOBC_THREAD_PRIORITY 11 // OBC通信
#define MAVGCS_THREAD_PRIORITY 12 // GCS通信
#define LOGGER_THREAD_PRIORITY 13 // 日志
#define STATUS_THREAD_PRIORITY 14 // 状态(最低)
数值越小优先级越高。控制任务必须最先响应，通信和日志可以适当延迟。
7. TOML配置系统
FMT使用TOML格式的配置文件 (`sysconfig.toml`) 进行运行时配置，配置文件将从tf卡中读取，应被放置在/sys目录下：
# /sys/sysconfig.toml
target = "SIEON-S1"
[console]
device_name = "serial0"
baudrate = 57600
[mavproxy]
chan0_device = "serial1"
chan0_baudrate = 57600
[pilot-cmd]
stick_channel = [1, 2, 3, 4]
aux_channel = [5, 6, 7, 8]
配置在 `bsp_post_initialize()` 阶段解析，如果文件不存在则使用默认配置。
8. 与PX4/APM架构对比
| 特性 | FMT | PX4 | ArduPilot |
| RTOS | RT-Thread | NuttX | ChibiOS/NuttX |
| 建模方式 | Simulink MBD | C++手写 | C++手写 |
| 算法可移植性 | 高(模型独立) | 中 | 低 |
| 任务管理 | Linker Section | 命令模式 | 手动注册 |
| IPC机制 | uMCN pub-sub | uORB | 无统一机制 |
| 构建系统 | SCons + Python | CMake | WAF |
| 学习曲线 | 中等 | 较高 | 较高 |
FMT的核心优势：MBD设计使得算法开发与固件开发解耦，算法工程师可以在Simulink中调试，自动生成代码后直接嵌入固件，大幅提升开发效率。
9. 二次开发实践要点
在实际项目中基于FMT进行二次开发，需要关注以下几个关键点：
9.1 新增硬件支持
1. 在 `src/driver/` 下新增芯片驱动
2. 在 `src/hal/` 下注册HAL设备
3. 在 `target/<board>/board.c` 的 `bsp_initialize()` 中初始化
4. 在 `config/driver.py` 中添加编译项
9.2 新增MAVLink消息
1. 在 `src/module/mavproxy/` 中注册周期消息
2. 使用 `mavproxy_register_period_msg()` 注册回调
3. 优化消息队列大小防止丢包（详见后续MAVLink优化篇）
9.3 新增自定义任务
1. 在 `src/task/` 下创建任务文件
2. 使用 `TASK_EXPORT` 宏注册任务
3. 在 `config/task.py` 中添加编译项
## 10. 总结
FMT飞控的架构呈以下特点：
1. **分层解耦**：BSP/Driver/HAL/Module/Model/Task六层职责清晰
2. **自动注册**：Linker Section机制实现零侵入式模块注册
3. **发布订阅**：uMCN实现松耦合的模块间通信
4. **面向对象**：函数指针表实现C语言多态
5. **MBD设计**：Simulink模型与固件代码分离，算法可独立验证
6. **配置驱动**：Python构建配置 + TOML运行时配置
这套架构既保证了实时性（RTOS + 优先级设计），又保证了可扩展性（自动注册 + 发布订阅），是嵌入式飞控系统的优秀参考实现。
