基于FMT飞控MBD智能开发流程

前面三章我们围绕PX4讲了环境搭建、基本概念和地面站生态。PX4的飞控算法（姿态环、位置环、估计器）都是以C代码的形式"手写"在源码里的，想改控制算法就得直接啃C++。本章介绍另一条路线——基于模型设计（Model-Based Design，MBD）的开源飞控FMT（Firmament）：算法在Simulink里"画"出来，仿真验证通过后由Embedded Coder自动生成C代码，再部署到飞控板上运行。对搞控制出身的同学来说，这条路线要友好得多。

在写作本章时，MATLAB和Simulink都已经有了官方MCP（Model Context Protocol）服务器，AI编程助手（本文以GitHub Copilot为例）可以直接操作MATLAB会话、读写Simulink模型、跑仿真取数据。所以我们最后会把传统MBD流程和AI智能开发串起来，形成一条"对话式"的开发流水线。

考虑到文章成稿后长时间不会更新，本章参考资料以FMT官方文档与源码仓库为准：

• FMT官方文档：https://docs.sieon.net/fmt
• 固件仓库：https://github.com/Firmament-Autopilot/FMT-Firmware
• 模型仓库：https://github.com/Firmament-Autopilot/FMT-Model

FMT是什么

Firmament（FMT）是一个基于MBD方法论设计的开源飞控系统，可用于无人机、车、船、机器人等各类无人系统的控制系统开发。整个系统主要由两部分组成：

• FMT-Firmware：运行在飞控硬件上的嵌入式系统，C/C++编写，基于RT-Thread实时操作系统，负责传感器采集、通信、日志等"飞控底座"工作；
• FMT-Model：基于MATLAB/Simulink的仿真框架与算法库，INS（导航）、FMS（制导）、Controller（控制）、Plant（被控对象）四大核心算法全部以Simulink模型形式开发。

与PX4开发方式的对比

由于本系列前几章都基于PX4，这里先给一张对比表，方便大家建立映射关系：

可以看到，两者的"底座"设计思想高度一致（发布订阅消息总线 + 模块化任务），FMT相当于把PX4里"手写C++的算法部分"整体换成了"Simulink模型生成"。所以前面学过的uORB、QGC、MAVLink概念在FMT这里基本都能平移过来用。

目前FMT支持的常见硬件包括AMOV ICF5、CUAV V5+/V5 Nano/7-nano/X7+、Pixhawk 4（FMUv5）、Pixhawk 2.4.6（FMUv2）、Cube Orange以及QEMU虚拟目标等，完整列表见固件仓库的target目录。

FMT软件架构

固件侧：src/model 四大模型

FMT-Firmware的src/model目录下放着四类核心算法模型：

• INS（惯性导航系统）：融合IMU、GPS、气压计、磁力计等传感器数据，输出姿态、位置、速度等状态估计；
• FMS（飞行管理系统）：接收遥控器/地面站指令，完成模式切换、航点管理，输出位置/姿态期望；
• Controller（控制器）：根据FMS期望与INS状态估计，输出执行器指令；
• Plant（被控对象模型）：仅用于仿真（SIH模式），在飞控内部模拟飞行器动力学。

每个模型目录（如src/model/control/mc_controller）里有两个关键部分：

• lib/：Simulink自动生成的C代码（.c/.h），里面能看到Controller_init()、Controller_step()这类模型入口函数，文件头会标注"Code generated for Simulink model"；
• xxx_interface.c：模型接口层，手写的胶水代码，负责调用模型的Init/Step函数、通过uMCN订阅模型输入、发布模型输出、定义模型参数与日志。

数据流转与调度在src/task/vehicle/normal/task_vehicle.c里一目了然：vehicle任务按各模型声明的周期（INS 2ms、FMS 4ms、Controller 2ms等）依次执行ins_step→fms_step→control_step，最后把控制量发送给执行器。模块之间通过uMCN消息总线交换数据（如ins_output、fms_output、control_output话题），这套MCN_DEFINE/MCN_DECLARE的写法和PX4的uORB几乎一个思路。

编译哪些模型由每个板级支持包（BSP）的target/<厂商>/<板子>/config/model.py决定，以Pixhawk FMUv5为例：

# Modify this file to decide which model are compiled
from building import *

vehicle_type = GetOption('vehicle')
sim_mode = GetOption('sim')

if vehicle_type == 'Multicopter':
    if sim_mode == 'HIL':
        MODELS = [
            'plant/template_plant',
            'ins/external_ins',
            'fms/mc_fms',
            'control/mc_controller',
        ]
    else:
        MODELS = [
            'plant/multicopter',
            'ins/cf_ins',
            'fms/mc_fms',
            'control/mc_controller',
        ]
elif vehicle_type == 'Fixwing':
    ...

注意sim_mode == 'HIL'分支：HIL仿真时飞行器动力学由外部仿真器（如AirSim）提供，所以Plant换成空模板template_plant、INS换成external_ins（传感器数据来自仿真器注入），而INS/FMS/Controller照常运行——这个配置逻辑和PX4里SITL/HIL启动脚本的思路是相通的。

模型侧：FMT-Model

FMT-Model的目录结构如下：

FMT-Model/
|-- bus/          Simulink Bus对象定义脚本（与固件侧数据结构一一对应）
|-- lib/          FMT_Model_Lib.slx 模型库（FMT Toolbox）
|-- model/        四大算法模型源文件
|   |-- Controller/  Base/FW/MC/VTOL/Boat/Car/Submarine/ADRC等
|   |-- FMS/         Base/FW/MC/VTOL/Boat/Car/Submarine等
|   |-- INS/         Base(CF-INS)等
|   |-- Plant/       FW/MC/VTOL/Boat等
|-- script/       Matlab脚本（数据分析等）
|-- simulation/   MILSIM.slx、DataSIM.slx 仿真顶层模型
|-- utils/        工具脚本
|-- FMT_Model.prj      Simulink工程文件
|-- FMT_Model_Init.m   工程初始化脚本

每个算法模型都是一个独立文件夹，固定包含四样东西：

• xxx.slx：Simulink模型本体；
• xxx_model_init.m：模型初始化脚本；
• README.md：模型说明（环境、参数表、支持模式）；
• LICENSE：许可证。

初始化脚本是理解FMT模型的关键，它把模型的"运行周期、版本信息、可调参数"统一导出给固件。以控制器模板为例：

model_version = 'v0.0.1';   % model version
model_name = 'Base Controller'; % model name

%% load configuration
load('control_default_config.mat');

%% Constant Variable
CONTROL_CONST.dt = 0.002;   % model execution period（执行周期，单位秒）

%% Exported Value
CONTROL_EXPORT_VALUE.period = uint32(CONTROL_CONST.dt*1e3); % 固件侧按ms调度
CONTROL_EXPORT_VALUE.model_info = int8([model_name, ' ', model_version, 0]);

%% Paramaters
CONTROL_PARAM_VALUE.KP = single(1.0);
CONTROL_PARAM_VALUE.KI = single(0.1);
CONTROL_PARAM_VALUE.KD = single(0.1);

%% Export to firmware
CONTROL_EXPORT = Simulink.Parameter(CONTROL_EXPORT_VALUE);
CONTROL_EXPORT.CoderInfo.StorageClass = 'ExportedGlobal';
CONTROL_PARAM = Simulink.Parameter(CONTROL_PARAM_VALUE);
CONTROL_PARAM.CoderInfo.StorageClass = 'ExportedGlobal';

几个要点：

• model_name/model_version会显示在飞控开机banner里，方便确认板上跑的是哪个版本的模型；
• dt决定了固件vehicle任务的调度周期，改这里就行，不用动固件代码；
• 所有*_PARAM参数以ExportedGlobal存储类导出后，会成为固件参数系统里的一员，可以通过QGC/msh命令行在线查看修改——这一点比PX4的param set-default还要直观；
• bus/目录下的FMS_Out_Bus.m、Control_Out_Bus.m等脚本定义了Simulink Bus对象，字段与固件侧FMS_Out_Bus等C结构体严格一一对应（连注释里的枚举含义都标好了），这是"模型世界"与"嵌入式世界"的数据契约。

MBD开发流程总览

经典的MBD开发遵循V模型：左侧从需求到模型设计逐级细化，右侧从单元验证到系统验证逐级回溯。

落实到FMT上，一条完整的算法开发流程是：

1. 模型设计：在FMT-Model中开发/修改INS、FMS、Controller模型；
2. MIL仿真：整个飞控算法链（含Plant动力学）都在Simulink里跑，验证算法逻辑；
3. 代码生成：Embedded Coder把模型翻译成C代码；
4. 代码部署：把生成代码放进固件对应目录，接入模型接口层；
5. 编译固件：SCons交叉编译出bin/hex；
6. SIL仿真：编译出的固件跑在QEMU虚拟硬件上，接QGC验证；
7. HIL仿真：固件跑在真实飞控板上，动力学由外部仿真器提供；
8. 实机飞行：装机、试飞、参数整定。

其中2/6/7/8是四个不同等级的"环路"，每一级都比上一级更接近真实、成本和风险也更高：

环境搭建

FMT-Model（MATLAB/Simulink侧）

克隆模型仓库（含子模块）：

git clone https://github.com/Firmament-Autopilot/FMT-Model.git --recursive

FMT-Model基于MATLAB 2018b开发（推荐版本，更高版本一般也可用），所需工具箱如下：

搭建步骤：

1. 在MATLAB中把工作目录切换到FMT-Model根目录；
2. 双击FMT_Model.prj打开并初始化工程。初始化脚本FMT_Model_Init.m会自动完成：设置代码生成输出目录、注册代码替换库（CRL）、加载全部Bus对象、依次执行Plant/INS/Controller/FMS四个模型的init脚本；
3. 在SIMULINK PROJECT面板点击Project Path查看当前选用的模型（默认多旋翼全家桶：MC-Plant/CF-INS/MC-FMS/MC-Controller）；
4. 想换固定翼就把多旋翼模型路径移除、加入FW系列模型，关闭工程并清空workspace后重新运行FMT_Model.prj才能生效。

FMT-Firmware（固件侧）

克隆固件仓库（注意--recursive，RTOS等内容在子模块里）：

git clone https://github.com/Firmament-Autopilot/FMT-Firmware.git --recursive --shallow-submodules

固件基于SCons构建，Windows下推荐使用RT-Thread的env工具（env.bat打开一个带scons、arm-none-eabi-gcc工具链的环境），Linux下直接装SCons与ARM GCC工具链即可。固件目录速览：

FMT-Firmware/
|-- rtos/rt-thread      RT-Thread实时操作系统（子模块）
|-- src/                主源码：driver/hal/model/module/task/protocol等
|-- target/             板级支持包BSP（amov/cuav/pixhawk/cubepilot/qemu等）
|-- unit_test/          单元测试

标准开发流程：以新增一个控制器为例

下面用"给多旋翼做一个自己的控制器"把整条流程走一遍，其他模型（INS/FMS/Plant）的套路完全相同。

第一步：在FMT-Model中创建新模型

官方推荐从现有模型复制起步。把model/Controller/Base-Controller整个文件夹复制一份，改名为New-Controller，内部保持四个必备文件：

然后修改controller_model_init.m：改model_name/model_version、按需调整dt和参数初值。接着在Controller.slx里把你的算法画进去——FMT模型库（Library Browser中的FMT Toolbox）提供了PID、滤波、坐标变换、饱和限幅等常用模块，输入输出总线直接复用INS_Out_Bus、FMS_Out_Bus、Control_Out_Bus。

最后把新模型挂进工程：SIMULINK PROJECT → Project Path → 移除旧控制器路径（如model/Controller/Base-Controller）→ 添加model/Controller/New-Controller → 关闭并重开工程。

第二步：MIL仿真验证算法

打开simulation/MILSIM.slx，这是一个完整的"虚拟飞控"：INS+FMS+Controller+Plant全链路在Simulink内闭环。

几个实用模块：

• RC输入（MILSIM/FCS/RC）：Variant切换四种来源——Joystick（USB游戏手柄）、Mavlink（真飞控转发遥控信号）、Fake（滑块模拟）、None（无遥控，用地面站控制）；
• GCS输入：模拟地面站下发模式切换与指令。注意RC优先级高于GCS，想用GCS控模式需先把RC置None；
• Auto Command / Mission Data：Offboard外部指令与MAVLink任务航点注入；
• 可视化：默认MATLAB 3D（VR Sink），也可接Flightgear等。

跑一次Mission模式的操作顺序：RC置None → GCS模式选PilotMode.Mission → 点击Run → 把GCS cmd_1从FMS_Cmd.None切到FMS_Cmd.PreArm（该输入边沿触发）。仿真结束后用Simulation Data Inspector对比期望/实际曲线：

MIL阶段迭代最快、成本最低，务必在这里把算法调到满意再往下走，这也是MBD相对手写代码最大的红利。

第三步：代码生成

打开你的模型（如Controller.slx），点击工具栏Build Model按钮即可生成代码：

FMT的官方模型已经预配好了代码生成选项，几个关键配置在Model Configuration Parameters里：

• Target选择ert.tlc（Embedded Coder，面向嵌入式；桌面验证可临时换grt.tlc）；
• Language选C或C++；
• Hardware Implementation页设置Device Vendor/Type（如ARM Compatible → ARM Cortex），保证生成代码与目标芯片匹配。

生成结果落在工程初始化时设定的输出目录（FMT-Model/build）下。

第四步：部署到固件

把生成代码接入固件分两种情况：

情况一：替换现有模型。直接把生成的.c/.h拷贝到对应模型的lib目录覆盖旧代码即可，官方约定位置：

• FMS：FMT-Firmware/src/model/fms/base_fms/lib
• INS：FMT-Firmware/src/model/ins/base_ins/lib
• Controller：FMT-Firmware/src/model/control/base_controller/lib
• Plant：FMT-Firmware/src/model/plant/multicopter/lib

情况二：新建模型目录（推荐，不动官方模型）。以控制器为例：

1. 复制FMT-Firmware/src/model/control/template_controller改名为my_controller；
2. 把生成代码全部拷入my_controller/lib；
3. 修改control_interface.c：按需注册参数、日志，核对输入输出话题；
4. 在BSP的config/model.py里把control/mc_controller替换成control/my_controller。

模型接口层（xxx_interface.c）的职责如下：

• 调用模型的Init()和Step()入口函数；
• 通过uMCN订阅模型输入（fms_output、ins_output），发布模型输出（control_output）；
• 把controller_model_init.m里导出的参数绑定到固件参数系统（可选）；
• 定义模型日志数据供DataSIM回放分析（可选）。

顺带一提：如果你手上已有一份C/C++写的算法（比如PX4的ECL EKF），也可以不走Simulink直接集成进固件，官方给了现成范例src/model/ins/px4_ecl，套路与部署模型一致。

第五步：编译与烧录

进入目标板目录编译，以AMOV ICF5为例：

cd FMT-Firmware/target/amov/icf5
scons -j4

--vehicle指定机型（默认Multicopter）、--airframe指定机架编号、--sim指定HIL模式。编译产物在build目录。

烧录两种方式：

FMT-Firmware\target\amov\icf5> python .\uploader.py
Found board id: 50,0 bootloader version: 5 on COM7
Erase  : [====================] 100.0%
Program: [====================] 100.0%
Verify : [====================] 100.0%

需要pip3 install pyserial；识别不到串口可用--port COM3手动指定。有J-Link的话直接连调试口下载也行（小心别把bootloader刷掉）。

上电后通过串口终端或QGC的Mavlink Console（输入boot_log）能看到启动banner，核对模型版本是否为你刚部署的版本：

   _____                               __
  / __(_)_____ _  ___ ___ _  ___ ___  / /_
 / _// / __/  ' \/ _ `/  ' \/ -_) _ \/ __/
/_/ /_/_/ /_/_/\_,_/_/_/_/\_,_/_/_/_/\__/_//_/\__/_/
Firmware.....................FMT FW v0.5.5
Kernel....................RT-Thread v4.0.3
Target...........................Amov-ICF5
Vehicle........................Multicopter
INS Model..................Base INS v0.3.2
FMS Model..................Base FMS v0.4.0
Control Model.......Base Controller v0.2.4
Task Initialize:
  mavobc................................OK
  mavgcs................................OK
  vehicle...............................OK

第六步：SIL仿真（QEMU）

没有硬件也能跑真实固件——FMT提供了QEMU虚拟目标qemu-vexpress-a9。先安装QEMU（qemu-system-arm），然后：

cd FMT-Firmware/target/qemu/qemu-vexpress-a9
scons -j4
qemu.bat     （Linux为qemu.sh）

注意两点：

• Windows下首次运行需在FMT控制台执行mkfs sd0格式化虚拟SD卡；
• 先开QGC再启动qemu，地面站会自动连上。此时固件内部跑的是SIH Plant模型，可以完整体验解锁、起飞、切模式、走航点，与真机操作完全一致。

还支持多机仿真：每个QEMU实例在控制台里param set MAV_SYS_ID 2并param save后重启，QGC里就能看到多架飞机。

第七步：HIL仿真

HIL是"真板子+虚拟动力学"：飞控固件跑在真实飞控上，传感器数据由外部仿真器通过MAVLink HIL消息注入。开启方法：在BSP目录的fmtconfig.h中取消注释#define FMT_USING_HIL后重新编译烧录。

上电后在控制台输入mcn list，传感器话题发布频率全为0是正常现象（数据等仿真器喂进来）。FMT走标准MAVLink HIL协议，所以任何支持HIL消息的仿真器都能接，官方演示了AirSim：在settings.json里把串口指向飞控端口即可。

HIL通过后，就可以装机校准、实飞整定参数了。实飞相关（传感器校准、参数整定、日志分析）官方文档有专门章节，此处不再展开。

智能开发：Copilot + MATLAB/Simulink MCP

以上是传统MBD流程。接下来是本章的主题：把AI助手接进这条流水线。

MathWorks推出了MATLAB Agentic Toolkit，核心是一个MCP服务器（matlab-mcp-core-server），让AI助手可以直接调用MATLAB与Simulink。以往AI只能帮你写写文档、改改C代码，Simulink模型对它是个黑盒；接入MCP后，AI可以：

• 直接在MATLAB会话里执行命令、跑脚本、跑单元测试；
• 读取Simulink模型的层次结构、端口连接、参数取值；
• 直接增删改模型里的模块和连线；
• 对模型做结构检查和行为测试。

环境配置

到 https://github.com/matlab/matlab-mcp-core-server 下载对应平台的MCP服务器（Windows为matlab-mcp-core-server-win64.exe），按仓库的GETTING_STARTED完成安装后，配置会写入~/.matlab-agentic-toolkit/config.json。

本文以VS Code + GitHub Copilot为平台。在工程根目录新建.vscode/mcp.json注册MCP服务器（示意，路径与参数以官方GETTING_STARTED为准）：

{
    "servers": {
        "matlab": {
            "command": "C:\\Users\\<你的用户名>\\.local\\bin\\matlab-mcp-core-server.exe"
        }
    }
}

保存后在VS Code里启动该服务器，Copilot Chat切换到Agent模式即可看到一整套mcp_matlab_*工具。主要工具分为两类：

智能开发闭环

把前文的标准流程逐步"交给AI"，得到一条对话式开发流水线：

1. 读懂模型：让Copilot调用model_overview/model_read看模型结构，比如它会告诉你Controller.slx里y1 = @Gain(ROLL_P) * u1(...)这样的表达式以及参数当前取值——比人肉翻模型快得多，也适合接手别人留下的模型；
2. 修改模型：直接用自然语言下需求（"在角速率环前加一个50Hz二阶低通""把ROLL_P参数导出为固件参数"），Copilot通过model_edit完成结构改动，再用model_check确认没有悬空端口；
3. MIL仿真：Copilot用evaluate_matlab_code初始化工程、运行MILSIM仿真、提取logsout数据，并对曲线做定量分析（超调量、调节时间、稳态误差），给出参数修改建议后自动迭代——这正是MBD最费人力的调参环节；
4. 代码生成：一条slbuild('Controller')交给AI执行，产物直接落在build目录；
5. 部署与编译：改control_interface.c、更新model.py这类模板化修改Copilot很擅长；scons -j4编译报错直接把日志丢回给AI分析；
6. 回归测试：用model_test把关键行为写成Gherkin用例（"给定roll期望10°，姿态应在2s内收敛到±2°"），以后每次改模型自动回归；脚本类改动可用check_matlab_code+run_matlab_test_file把关；
7. SIL/HIL/实飞：这三步涉及QEMU、真板和仿真器，AI可以负责起进程、盯日志、读banner核对模型版本，但解锁试飞这类高风险操作请务必留在人的手里。

实践注意事项

结合前几章的经验，给几条务实建议：

• AI改完模型必须回归：model_check只能查结构，算法行为一定要跑MIL对比仿真确认，不要AI说"改好了"就直接生成代码上板；
• 生成代码是产物，别手改：lib/*.c/*.h每次代码生成都会被覆盖，任何定制都应写在xxx_interface.c接口层里；
• 参数表要同步：controller_model_init.m里新增的导出参数会出现在固件参数系统，同步更新模型的README参数表，否则QGC里一排不明参数没法维护；
• 模型版本号要递增：model_version是上电banner里核对固件版本的关键信息，每次改动顺手+1，避免"板上跑的到底是哪个版本"的哲学问题；
• MATLAB版本一致性：FMT-Model基于R2018b，团队协作时最好统一MATLAB版本，低版本打不开高版本保存的.slx；
• 本章的MATLAB/Simulink MCP生态还在快速迭代，工具名与配置方式以 https://github.com/matlab/matlab-mcp-core-server 最新文档为准。

参考资料

• FMT官方文档（MBD/仿真/构建各章节）：https://docs.sieon.net/fmt
• FMT-Firmware源码：https://github.com/Firmament-Autopilot/FMT-Firmware
• FMT-Model源码：https://github.com/Firmament-Autopilot/FMT-Model
• MATLAB MCP服务器：https://github.com/matlab/matlab-mcp-core-server
• MAVLink任务协议：https://mavlink.io/en/services/mission.html
• 本系列前三篇：《PX4学习记录之入门指南》《PX4学习记录之概念》《PX4学习记录之生态》