PX4代码架构研究学习报告

PX4代码架构研究学习报告

一份写给"想真正搞懂 PX4 怎么跑起来"的人的研究学习报告

源码

---

目录

章	内容	定位
---	写在前面	版本基线、阅读方法
1	认识 PX4	入门：它是什么、不是什么
2	代码地图与构建系统	入门：东西都在哪、怎么编译
3	运行时全景：从上电到起飞	入门：系统怎么醒过来
4	中间件核心：uORB	地基：全系统的通信骨架
5	任务、线程与工作队列	地基：代码什么时候跑
6	参数系统	地基：怎么调、怎么存
7	驱动层与硬件抽象	地基：硬件怎么接进来
8	传感器处理与状态估计	主干：从原始数据到可靠状态
9	控制链路	主干：从杆量到电机
10	决策层：commander 与 navigator	主干：飞去哪、能不能飞
11	通信：和外面世界的接口	主干：MAVLink / DDS / CAN
12	日志与调试	实战：怎么看问题、怎么排
13	仿真：SITL 与 HIL	实战：不用真飞机做开发
14	端到端：一次自动起飞的全链路走查	串讲：把所有东西连起来
15	动手：写一个自己的模块	进阶：完整代码 + 逐行解释
16	动手：移植一块新板子	进阶：板级支持包
17	版本演进与常见误区	避坑：老资料对不上的地方
18	学习路线图	规划：四阶段成长路径
A--D	术语 / 命令 / 话题 / 资源 速查	备查

建议第一次阅读顺序：1 → 3 → 4 → 5 → 9 → 14，先把主脊梁立起来，再回头补细节。

---

写在前面

先说清楚这份报告是怎么写出来的，你需要知道的前提。

代码基线。 报告里的结论全部来自我实际读过的源码，不是二手转述。基线是 PX4-Autopilot 的 main 分支，commit 43c6d582adaa0b19090bdbdb0873da0babcbe619（2026-10-15）。同期的版本状况是：最新稳定版 v1.17.0 ，v1.18.0-rc1 于 2026-09-10 发布，main 上跑的是 v1.18 之后的开发内容。

这一点很重要，因为 PX4 在 2024--2026 这两年做了一轮不小的重构。你在网上看到的很多中文教程（尤其是 2019--2021 年那批）描述的架构，和今天的 main 已经对不上了。典型的几个差异：

老教程里的说法	今天 main 上的实际情况
机型几何靠 .mix 混控文件定义	v1.14 起改成 Control Allocator + 效能矩阵 （CA_* 参数 + QGC Actuators 界面）
控制器是 mc_pos_control → mc_att_control 两段式	多旋翼拆成了 mc_pos_control → mc_att_control → mc_rate_control 三段，角速率环独立成模块
固定翼是 fw_att_control + fw_pos_control_l1	变成 fw_lateral_longitudinal_control（制导）+ fw_att_control / fw_rate_control（控制）
模块继承 ModuleBase<T>（CRTP 模板）	改成 ModuleBase + ModuleBase::Descriptor 描述符，去掉了模板
消息文件名 vehicle_attitude.msg（snake_case）	文件名改成 CamelCase （VehicleAttitude.msg），topic 名仍是 snake_case
消息没有版本概念	v1.16 起引入消息版本化 ，出现 msg/versioned/、msg/px4_msgs_old/ 和 translation_node
与 ROS 2 桥接用 microRTPS / micrortps_client	用 uxrce_dds_client（uXRCE-DDS），另外新增了 zenoh 模块

所以，如果你手里的资料和上面的表格对不上，先怀疑资料过期，而不是怀疑代码。

这份报告怎么用。 前三章是"地图"，目的是让你知道东西都在哪；第 4--7 章是中间件和基础设施，这部分是全篇的地基，值得反复看；第 8--11 章是飞行栈主干（估计、控制、决策、通信），是真正决定"飞机为什么这么飞"的地方；第 12 章之后是调试、仿真和动手环节。

一个建议。 别把这份报告当字典背。PX4 的架构每隔几版就会动，能让你长期受益的不是"mc_att_control 在哪"，而是你排查问题时知道该去哪个目录、用哪条命令、看哪个 topic 。所以报告里凡是出现命令的地方，我都尽量给出可以直接粘进 pxh> 终端的版本；凡是出现结论的地方，我都标了来源文件，你可以自己去核对。

最后一句话给你打预防针：PX4 的源码不小（src/ 下 68 个库、80 多个模块、60 多个驱动、230 多个 uORB 消息定义），第一次读会晕。这很正常。晕的时候回到第 14 章那条端到端链路去对，那条线是整份报告的脊梁。

---

第 1 章 认识 PX4

1.1 PX4 到底是什么

一句话：PX4 是一个运行在飞控板上的实时飞控固件，同时它还是一套通用的机器人中间件。

官方文档把它拆成两层，这个拆法非常准确，建议刻在脑子里：

• 飞栈（Flight Stack）：导航、制导、控制、状态估计。也就是"让飞行器飞起来"的那部分算法。
• 中间件（Middleware）：uORB 消息总线、设备驱动、参数系统、对外通信、日志、仿真层。也就是"让上面那套算法能跑、能跟外界说话"的那部分。

这两层的边界不是画着好看的。它决定了你改东西时该往哪下手：调控制效果去飞栈，加个传感器去中间件。

源码布局也直接反映了这个分层：

src/modules/     ← 飞栈主体 + 一部分中间件模块
src/drivers/     ← 中间件：设备驱动
src/lib/         ← 中间件：算法库与通用工具
platforms/       ← 中间件：HAL、uORB、任务/工作队列、参数
ROMFS/           ← 启动脚本与文件系统镜像
msg/             ← uORB 消息定义（就是通信的"字典"）
boards/          ← 板级支持包

1.2 一套代码，所有机型

PX4 最值得称道、也最容易让新手困惑的一点是：多旋翼、固定翼、VTOL、无人车、无人船、水下潜航器、飞艇，用的是同一份代码。

这不是"复制粘贴然后改改"的那种同一份。是真正的同一份代码，靠三个东西区分行为：

1. 机型配置文件（airframe） ：在 ROMFS/px4fmu_common/init.d/airframes/ 下，一个文件对应一个机型 ID（即参数 SYS_AUTOSTART 的值）。它决定启动哪些模块、加载哪些默认参数。
2. VEHICLE_TYPE 与 vehicle_status.vehicle_type：运行时判断，多旋翼走多旋翼的控制器，固定翼走固定翼的。
3. 控制分配矩阵（effectiveness matrix）：把"要多少推力、多少力矩"翻译成"每个电机转多快"。这是机型无关的关键抽象，第 9.6 节细讲。

好处是：你在多旋翼上学到的 uORB、参数、日志、调试方法，换到固定翼上完全通用。这对我们这种"今天调飞机、明天调船"的人来说是巨大的效率优势。

代价也很明显：main 分支上任何一个改动都要考虑所有机型，PR 的 review 门槛很高；而且很多代码路径是"多机型共用 + 运行时分支"，读代码时经常要停下来想"我这会儿到底在哪条分支上"。

1.3 跟 ArduPilot 比，PX4 的路子不一样

这个话题容易引战，我只说我自己的使用体感，不讲谁更好。

PX4 更"分层清晰、可复用"。 uORB 发布订阅、模块化、lib/ 里那一堆能单独拿出来的算法库（matrix、mathlib、geo、control_allocation、tecs......），这些东西的设计目标就是"通用机器人中间件"。你要做的是"把算法接到车上"，PX4 的接口更规整。

ArduPilot 更"整机调优、参数密集"。 它内部状态传递大量依赖全局变量和 L1/L2 之类的历史结构，分层没那么干净，但它的机型适配成熟度和"拿来就用"的完成度很高。

还有一个很现实的区别：PX4 与 ROS 2 的对接路径（uxrce_dds_client + px4_msgs）是官方主线在推的，搞 CV、搞 Offboard 的话 PX4 会顺很多。反过来，如果你只想让一架老式固定翼稳稳飞完航线，两家都行，看你手上社区资源。

1.4 你迟早会碰到的几个仓库

PX4 不是一个仓库，是一个仓库群。分清楚它们，能省你很多时间：

仓库	内容	什么时候需要
PX4/PX4-Autopilot	飞控固件本体	读源码、改算法、编译刷固件
PX4/PX4-user_guide	官方文档源（docs.px4.io）	查配置项、查参数含义
PX4/PX4-Devguide	已合并进 user_guide 的开发者文档	同上
PX4/px4_msgs	ROS 2 消息包（从 msg/ 自动生成）	写 ROS 2 节点
PX4/px4_ros_com / PX4/px4-ros2-interface-lib	ROS 2 桥接与接口库	Offboard 开发
mavlink/mavlink	MAVLink 协议定义	改自定义消息、写地面站
mavlink/qgroundcontrol	QGC 地面站	地面站二次开发
PX4/flight_review	在线日志分析	分析飞行日志
PX4/pyulog	ULog 解析 Python 库	写日志分析脚本
PX4/PX4-Autopilot 的 Tools/	构建与工具脚本	几乎每天用

顺带说一句：文档站 docs.px4.io 的源码就在 PX4-Autopilot 仓库的 docs/ 目录里。这是个宝藏------别人问"PX4 里 X 是什么"，你直接 docs/en/ 下搜，比翻网页快得多。

1.5 版本号与发布节奏

PX4 的版本号是 v主.次.修订，主分支叫 main，稳定分支叫 release/1.x。

• 每年一个大版本，节奏大概是"年初 alpha → 年中 beta → 下半年 rc → 发布稳定版"。
• 稳定版发布后会有一个 release/1.x 分支专门接 bug fix。
• main 永远是最激进的，不建议直接拿去飞飞机。要用就用 release/1.x。

这里有个新手常踩的坑：从 main 编译出来的固件，可能包含正在开发中的、文档还没写的新参数和新行为。 你照着文档配参数，发现参数不存在，或者行为不一致，十有八九是因为代码比文档新。

我的建议是：学习阶段用 main（能看到最新设计），实际飞飞机用最近的 release/1.x。

---

第 2 章 代码地图与构建系统

2.1 顶层目录，一个一个说

这是我刚拿到仓库时会跑的第一条命令和得到的答案，我把每个目录的用途补上：

git clone --recursive https://github.com/PX4/PX4-Autopilot.git
cd PX4-Autopilot
ls

AGENTS.md          AI 协作说明（新加的，可忽略）
CMakeLists.txt     工程入口，版本号就从这里解析
Kconfig            配置系统的总入口
Makefile           一条命令走天下的封装
ROMFS/             启动脚本 + 文件系统镜像内容（会烧进固件）
Tools/             构建、烧写、日志分析、CI 脚本
boards/            板级支持包：每块板一个目录
cmake/             CMake 宏与模块
docs/              官方文档源码（中/英/韩/乌克兰语）
launch/            ROS 2 launch 文件
msg/               uORB 消息定义
platforms/         平台抽象：NuttX / POSIX / QuRT / ROS2
posix-configs/     POSIX 平台（SITL、Raspberry Pi）的启动配置
src/               主源码
srv/               ROS 2 service 消息定义
test/              集成测试
validation/        验证相关

几个重点展开：

platforms/ 是很多人忽略但极其重要的目录。 它装的是"跟操作系统和硬件相关的那一层"，也是 uORB、工作队列、任务管理、参数后端这些地基的实现位置：

platforms/
├── common/                    ← 跨平台公共层（最重要的地方）
│   ├── uORB/                  ← uORB 实现本体
│   ├── px4_work_queue/        ← 工作队列实现
│   ├── work_queue/            ← 老一代工作队列（drivers 还在用）
│   └── include/px4_platform_common/
│       ├── module.h           ← 所有模块的基类 ModuleBase
│       ├── module_params.h    ← 参数基类
│       ├── tasks.h            ← 优先级定义、px4_task_spawn
│       ├── param.h            ← 参数 API
│       └── px4_work_queue/    ← WorkItem / ScheduledWorkItem
├── nuttx/                     ← NuttX 适配（含 px4/ 下的板级代码）
├── posix/                     ← Linux/macOS 适配（SITL 走这里）
├── qurt/                      ← 高通 QuRT 适配
└── ros2/                      ← ROS 2 平台的模块基类变体

src/ 是飞栈和大部分中间件模块：

src/
├── drivers/       ← 硬件驱动
├── examples/      ← 示例模块（学写模块从这里抄）
├── include/       ← 对外暴露的头文件
├── lib/           ← 算法库（68 个）
├── modules/       ← 功能模块（80+ 个）
├── systemcmds/    ← 系统命令（就是 pxh 里那些命令的实现）
└── templates/     ← 新模块模板

2.2 src/lib/：被低估的宝库

src/lib/ 下有 68 个库。这些库很多都是可以脱离 PX4 单独使用的通用实现，我列几个你一定会用到的：

库	干什么	备注
matrix	矩阵/向量/四元数库	PX4 全篇的数学基础，Header-only，可以直接抄走
mathlib	数学工具（限幅、低通、死区、积分器）	LowPassFilter2p、AlphaFilter 都在这
geo	地理坐标计算	经纬度转 NED、航向距离计算
controllib	控制器基类	Block/SuperBlock 那套，老代码里的控制块
pid	PID 实现	RateControl、PositionControl 都基于它
control_allocation	控制分配算法	伪逆 + 序贯解饱和
parameters	参数系统实现	参数存储、MAVLink 同步
mixer_module	混控基类	输出驱动的公共骨架
sensor_calibration	传感器校准模型	校准矩阵、温度补偿
ecl（历史上）	现在拆成 ekf2 + lat_lon_alt	EKF 相关
hysteresis	滞回比较器	状态机抖动抑制神器
hysteresis / circuit_breaker	熔断器	故障隔离
slew_rate	变化率限制	输出平滑
tecs / npfg / pure_pursuit / dijkstra / motion_planning	固定翼/路径跟随/避障算法	固定翼制导相关
tunes	蜂鸣器音调定义	听声辨位排故
perf	性能计数	perf 命令背后就是这个
dataman	键值存储（存在 Flash）	存任务、安全点
heatshrink / tinybson	压缩 / 序列化	日志与参数存储用
rl_tools / tensorflow_lite_micro / pid_design / system_identification	神经网络与系统辨识	新东西，v1.16+ 才有

实用建议 ：想知道某个算法 PX4 到底怎么实现的，先在 src/lib/ 里搜关键词，十有八九已经有人写好了，不用你重新推公式。

2.3 两套配置系统：Kconfig + CMake

这是 PX4 构建系统里最容易让人迷糊的地方。它同时用了两套东西，各管一摊：

Kconfig 管"编译什么" 。就是 Linux 内核那套 Kconfig，make px4_fmu-v6x_default menuconfig 能起一个图形界面让你勾选驱动和模块。所有 CONFIG_* 宏都是从这里来的。

boards/px4/fmu-v6x/default.px4board   ← 具体板子的勾选结果（纯文本）
src/drivers/Kconfig                    ← 驱动的开关定义
src/modules/Kconfig                    ← 模块的开关定义
platforms/common/px4_work_queue/Kconfig ← 连工作队列的栈大小和优先级都是 Kconfig 配的

default.px4board 就长这样，你完全可以手改：

CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
CONFIG_BOARD_ARCHITECTURE="cortex-m7"
CONFIG_BOARD_ETHERNET=y
CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0"
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
CONFIG_DRIVERS_BAROMETER_BMP388=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_PWM_OUT=y
...

看到最后一行没有？连默认的 GPS 串口是哪个、哪个 IMU 驱动要编进去，都是在这个文件里定的。 移植新板子，本质上就是在写这个文件和它同目录下的 nuttx-config/。

CMake 管"怎么编译" 。每个源码目录下的 CMakeLists.txt 决定把哪些 .cpp 编进 px4 这个可执行文件，以及为哪些 msg/*.msg 生成代码。

# src/modules/mc_att_control/CMakeLists.txt 大概长这样
px4_add_module(
	MODULE modules__mc_att_control
	MAIN mc_att_control
	SRCS
		mc_att_control_main.cpp
	DEPENDS
		px4_work_queue
)

px4_add_module 是 PX4 自定义的宏，它干了三件事：编译源文件、把模块注册进模块表（这样 pxh 里才能 xxx start）、处理依赖。

生成的消息代码放在 build/<target>/uORB/topics/ 下，你在代码里 #include <uORB/topics/xxx.h> 引的就是它。

2.4 目标名怎么读

PX4 的构建目标名格式是 <board>_<config>：

• px4_fmu-v6x_default：Pixhawk 6X 的默认配置
• px4_fmu-v6c_bootloader：6C 的 bootloader
• px4_sitl_default：软件在环仿真
• px4_sitl_test：仿真 + 单元测试
• px4_fmu-v6xrt_default：6X 的实时（RT）配置

板子对应的目录在 boards/px4/<board>/。当前 main 上支持的官方板子有：

fmu-v2  fmu-v3  fmu-v4  fmu-v4pro  fmu-v5  fmu-v5x
fmu-v6c fmu-v6u fmu-v6x fmu-v6xrt
io-v2   raspberrypi   ros2   sitl

外加 boards/ 下还有一大堆厂商板子（ARK、CUAV、Holybro、ModalAI、NXP、SIYI......），通常都是从头文件里 include 官方板子的配置再改几行。

2.5 常用构建命令

这些命令我几乎每天都在用，直接抄：

# ---- 一次性准备 ----
git clone --recursive https://github.com/PX4/PX4-Autopilot.git
cd PX4-Autopilot
bash ./Tools/setup/ubuntu.sh          # Ubuntu 依赖
# 或者用 Docker，跨平台最省事：
#   ./Tools/docker_run.sh

# ---- 编译固件 ----
make px4_fmu-v6x_default              # 编译 Pixhawk 6X 固件
make px4_fmu-v6x_default upload       # 编译并烧写（板子插着 USB）
make px4_fmu-v6x_default -j8          # 8 线程编译

# ---- 图形化配置 ----
make px4_fmu-v6x_default boardconfig  # 板级 Kconfig
make px4_fmu-v6x_default menuconfig   # 模块/驱动开关
make px4_fmu-v6x_default guiconfig    # 带界面的 menuconfig

# ---- 仿真 ----
make px4_sitl_default                 # 只编译 SITL
make px4_sitl gz_x500                 # 编译并启动 Gazebo 里的 X500
make px4_sitl jmavsim                 # 编译并启动 jMAVSim
make px4_sitl gazebo-classic_iris      # Gazebo Classic（老）

# ---- 测试与检查 ----
make tests                            # 单元测试
make check                            # 全套检查
make format                           # 代码格式化（astyle）
make check_format                     # 只检查格式

# ---- 元数据与文档 ----
make parameters_metadata              # 生成参数元数据（给 QGC 用）
make airframe_metadata
make uorb_graphs                      # 生成 uORB 话题图
make module_documentation             # 生成模块文档

有几个坑我替你踩过了：

1. 一定要 --recursive 。子模块（src/lib/matrix、mavlink、NuttX、Micro-XRCE-DDS-Client 等）不拉下来编不过。忘了加就 git submodule update --init --recursive。
2. 首次编译慢。NuttX 要编一遍内核，十分钟起步。之后增量编译就快了。
3. 换了 default.px4board 或 Kconfig 后，CMake 会重新跑一遍配置，第一次会明显变慢。
4. make px4_sitl 不带后缀 会直接启动仿真，不是只编译。想只编译用 make px4_sitl_default。
5. Windows 上原生编译官方不支持，老老实实用 WSL2 或者 Docker，我见过太多人在 Cygwin/MinGW 上折腾三天最后放弃了。

编译产物在 build/<target>/ 下：

build/px4_fmu-v6x_default/
├── px4_fmu-v6x_default.elf     ← 带调试信息的可执行文件
├── px4_fmu-v6x_default.px4     ← 最终固件（就是一个 zip）
├── uORB/topics/                ← 生成的消息头文件
├── parameters/                  ← 生成的参数定义
└── ...

2.6 关于 Docker：真香

我不想在环境上浪费生命，所以我的做法是：

# 一行进入官方容器，里面啥都装好了
./Tools/docker_run.sh

# 容器内
make px4_fmu-v6x_default

容器里 Tools/、build/ 都能跟宿主机共享（docker_run.sh 会挂载当前目录）。唯一的麻烦是 USB 烧写要额外传设备节点，所以我的习惯是：容器里编译，宿主机上烧写 ------把 build/xxx/xxx.px4 拿出来，用 QGC 或者 Tools/px4_uploader.py 刷。

2.7 怎么快速建立"心理地图"

最后给个方法论，比记住具体目录有用得多：

# 1. 先看版本和板子
git log -1 --oneline
ls boards/px4/

# 2. 看总入口
head -60 CMakeLists.txt

# 3. 数一数规模，心里有数
ls src/modules | wc -l      # 模块数
ls src/drivers | wc -l      # 驱动数
ls msg/*.msg | wc -l        # 消息数

# 4. 找启动脚本，这是运行时的地图
cat ROMFS/px4fmu_common/init.d/rcS | less
cat ROMFS/px4fmu_common/init.d/rc.mc_apps

# 5. 挑一个你最想搞懂的模块，从 CMakeLists.txt 和头文件读起

这套流程我在接触任何一个稍大的嵌入式项目时都会走一遍，半小时就能知道"东西在哪、从哪进、谁调谁"。

---

第 3 章 运行时全景：从上电到起飞

这一章讲的是"系统是怎么醒过来的"。搞懂启动链，你以后遇到"某个模块没起来""参数没生效"这类问题，五分钟就能定位。

3.1 启动链：三级跳

从上电到 PX4 主程序跑起来，要过三级：

第一级：Bootloader。 芯片复位后先跑的是 PX4 自己写的 bootloader（在 src/drivers/bootloaders/ 和 boards/px4/<board>/ 里）。它做的事很朴素：看是不是要升级固件，不是就把主固件搬到内存执行。

这一级的产物是独立的 *.bin（不是 *.px4）。所以 PX4 的固件包其实包含两个东西：bootloader 和主程序。

第二级：OS 启动。 NuttX 内核初始化，挂载文件系统，然后执行 /etc/init.d/rcS------注意，这是 NuttX 自己的 rcS，不是 PX4 的。

第三级：PX4 主程序。 NuttX 的 rcS 启动 px4 可执行文件（platforms/nuttx/src/px4/... 里的入口）。PX4 的 main() 会：

1. 初始化平台（时钟、串口、内存）
2. 挂载 ROMFS（PX4 的启动脚本和机型配置就在里面）
3. 执行 PX4 的 rcS（ROMFS/px4fmu_common/init.d/rcS）
4. 之后整个系统就由 rcS 里的命令驱动了

关键认知：PX4 的启动过程就是"读脚本、执行命令"。 rcS 里写的每一行，跟你手动在 pxh> 里敲的命令是同一回事，只是它开机自动跑。这解释了两个常见疑问：

• 为什么 rcS 里 xxx start 失败，模块就没起来？因为那就是唯一的启动手段。
• 为什么改了 rcS 要用 make 重新编译烧写？因为它被打包进 ROMFS 了。

3.2 rcS：整个系统的总指挥

ROMFS/px4fmu_common/init.d/rcS 有 797 行，是 PX4 最重要的一个文件。它的开头定义了一堆全局变量：

set R /
set FCONFIG ${BOARD_ROOT_PATH}/etc/config.txt    # 用户配置文件
set FEXTRAS ${BOARD_ROOT_PATH}/etc/extras.txt    # 用户追加的启动命令
set FRC ${BOARD_ROOT_PATH}/etc/rc.txt            # 用户完全接管启动
set STORAGE_AVAILABLE no
set VEHICLE_TYPE none
set USE_HARDFAULT_LOG no
set USE_TASK_WATCHDOG no
set PARAM_DEFAULTS_VER 1

这三行（config.txt / extras.txt / rc.txt）很有讲究，它们都在 SD 卡上（/fs/microsd/etc/），意思是你不用改代码、不用重新编译就能改启动行为：

文件	位置	作用
config.txt	SD 卡 /etc/	覆盖板级变量、串口映射、是否格式化 SD 卡
extras.txt	SD 卡 /etc/	在标准启动流程之后追加命令（最常见，用它启动自定义模块）
rc.txt	SD 卡 /etc/	完全接管启动，PX4 默认流程一行都不跑
rcS	固件 ROMFS	只能重新编译才能改

我个人的经验：90% 的自定义启动需求用 extras.txt 就够了 。比如你想开机自动跑一个自己的模块，插上 SD 卡建一个 /etc/extras.txt，里面写：

my_custom_module start
mavlink start -d /dev/ttyS3

rcS 的主流程大致是这样（我按执行顺序梳理）：

# 1) 挂载 SD 卡
#    成功 → STORAGE_AVAILABLE=yes；失败 → 蜂鸣器报警 + 错误音

# 2) 检查供电、启动 hardfault_log（如果有硬件故障日志功能）
if [ "$USE_HARDFAULT_LOG" = yes ]; then
	hardfault_log check
	...
fi

# 3) 启动任务看门狗（可选）
if [ "$USE_TASK_WATCHDOG" = yes ]; then
	task_watchdog start
fi

# 4) 检查 autostart 开关
#    如果 SD 卡上有 .autostart 文件 → 禁用自动启动（调试用）

# 5) 参数处理
#    - 如果 SYS_AUTOCONFIG=1 → 恢复出厂参数（保留机型/校准/飞行时间）
#    - 检查 SYS_PARAM_VER，版本不匹配就重置
param reset_all SYS_AUTOSTART SYS_PARAM_VER SYS_AUTOCFG_CAL CAL_* ...

# 6) 加载板级默认参数与板级启动脚本
. ${R}etc/init.d/rc.board_defaults      ← 板级默认参数
. ${R}etc/init.d/rc.board_arch_defaults ← 架构级默认参数

# 7) 根据 SYS_AUTOSTART 加载机型配置（这一步会设置 VEHICLE_TYPE）
. ${R}etc/init.d/rc.autostart

# 8) 启动蜂鸣器、dataman 等基础设施
tone_alarm start
dataman start

# 9) 启动核心服务
...

# 10) 根据 VEHICLE_TYPE 启动对应的应用集
#     多旋翼 → rc.mc_apps；固定翼 → rc.fw_apps；...

这里有两个设计值得点出来：

SYS_AUTOCONFIG 和 SYS_PARAM_VER 是参数系统的"自愈机制"。 前者是经典的"按错配置后强行恢复"，后者是"固件升级时自动重置参数"。很多人升级固件后发现参数全变了，就是 SYS_PARAM_VER 在起作用。

参数重置是"选择性"的。 看那行 param reset_all ... CAL_* LND_FLIGHT*------传感器校准、飞行时间、飞行 UUID、MAVLink 系统 ID 这些都被排除在外。这是个工程上的细节考虑：参数可以乱，但校准数据不能丢。

3.3 机型配置与 rc.*_apps

rcS 走到第 7 步会去 source 机型文件。机型文件长这样（简化）：

#!/bin/sh
. ${R}etc/init.d/rc.mc_defaults      # 多旋翼通用默认参数

set VEHICLE_TYPE mc                  # 关键！决定了后面走哪条分支

# 参数设置
param set-default MC_PITCH_P 6.5
param set-default MC_ROLL_P 6.5
param set-default MPC_XY_VEL_MAX 12

# 机型元数据（给 QGC 显示用）
param set-default SYS_AUTOSTART 4001
...

然后 rcS 根据 VEHICLE_TYPE 去 source 对应的应用脚本。多旋翼的是 rc.mc_apps，全文不长，我完整贴出来------这就是"一架多旋翼到底跑了哪些模块"的答案：

# 控制分配
control_allocator start

# 角速率环
mc_rate_control start

# 姿态环
mc_att_control start

# 自动调参（条件启动）
if param greater -s MC_AT_EN 0
then
	mc_autotune_attitude_control start
fi

# 视觉目标估计（条件启动）
if param greater -s VTE_EN 0
then
	vision_target_estimator start
fi

# 位置环 + 悬停推力估计 + 飞行模式管理
mc_hover_thrust_estimator start
flight_mode_manager start
mc_pos_control start

# 着陆检测
land_detector start multicopter

# 神经网络控制（条件启动）
if param compare -s MC_NN_EN 1
then
	mc_nn_control start
fi

看这个顺序是有讲究的：先起控制分配，再起控制环，最后起决策层。 反过来的话，控制环起来时发现没有分配器，会报错。

其他几个机型脚本同理：rc.fw_apps（固定翼）、rc.vtol_apps（垂直起降）、rc.rover_*. / rc.uuv_apps（无人车/潜航器）、rc.airship_apps（飞艇）。

而传感器驱动不在这些文件里，它们在 rc.sensors 里，跟机型无关，所有机型共用一个差不多。rc.sensors 里全是这种模式：

if param compare -s SENS_EN_BATT 1
then
	batt_smbus start -X
fi

if param compare -s SENS_EN_LL40LS 2
then
	ll40ls start -X
fi

-X 的意思是"用外部 I2C 总线"。这类逻辑很清楚：每个可选传感器一个参数开关，rc.sensors 负责按开关启动驱动。

补充一句，现在很多板子上，板载传感器的驱动不是靠 rcS 启动的 ，而是编译时就固定了，靠 rc.board_sensors 或者驱动自己的自动探测（start -a）起来。这块跟板子强相关，读 boards/px4/<board>/init/rc.board_* 最准。

3.4 控制台与命令体系

PX4 有三条进入系统的路：

1. NSH / pxh 串口控制台。 接 USB，用 screen/picocom/Tools/px4.py 打开：

# 最省事的方式，官方脚本
python3 Tools/px4.py /dev/ttyACM0

# 或者直接
screen /dev/ttyACM0 57600

看到 nsh> 是 NuttX shell，敲 pxh>（或者有时候直接就是）进 PX4 shell。两者的关系：nsh 是操作系统的 shell，pxh 是 PX4 自己的 shell，pxh 里的命令才是那些模块和系统命令。

2. MAVLink Shell。 不用连线，通过网络或数传进：

python3 Tools/mavlink_shell.py /dev/ttyACM0
# 或者指定 UDP
python3 Tools/mavlink_shell.py udp:0.0.0.0:14550

这个特别有用，因为它能穿过数传链路。飞机在天上、你在车里，也能敲命令。前提是 MAVLink 实例上开了 shell（mavlink start 带 -m config 里要有 shell 功能）。

3. QGroundControl 的 MAVLink Console。 图形界面里的一个终端，本质和第 2 条一样。

在 pxh> 里能敲的命令分三类：

# 系统命令（src/systemcmds/ 下）
top                     # 看任务占用（NuttX）
work_queue status       # 看所有工作队列情况
param show MC_ROLL_P    # 看参数
param set MC_ROLL_P 6.0 # 改参数
listener sensor_combined  # 实时打印一个 topic
uorb top                # 看 topic 频率
dmesg                   # 内核日志
free                    # 内存
perf                    # 性能计数
mavlink status          # MAVLink 实例状态
ver all                 # 版本信息

# 模块命令（每个模块自己的）
mc_att_control start
mc_att_control status
mc_att_control stop
ekf2 status
sensors status
commander arm
commander disarm
navigator status
logger status
control_allocator status

# 调试命令
hardfault_log info      # 看有没有 hardfault
task_watchdog status
ekf2 check              # 跑 EKF 自检

两个保命命令，记住它们：

# 1. 看所有模块和主题的实时频率 ------ 排查"数据不动了"的第一选择
uorb top

# 2. 看工作队列负载 ------ 排查"控制卡顿 / 延迟大"的第一选择
work_queue status

3.5 模块的生命周期

PX4 里每个模块都遵循同一套生命周期约定，这是通过 ModuleBase 基类强制的：

// platforms/common/include/px4_platform_common/module.h
class ModuleBase
{
public:
	struct Descriptor {
		int (*task_spawn)(int argc, char *argv[]);
		int (*custom_command)(int argc, char *argv[]);
		int (*print_usage)(const char *reason);
		px4::atomic<ModuleBase *> object{nullptr};
		int task_id{-1};
	};

	static int main(Descriptor &desc, int argc, char *argv[]);
	static int start_command(Descriptor &desc, int argc, char *argv[]);
	static int stop_command(Descriptor &desc);
	static int status_command(Descriptor &desc);
	...
};

注意最上面那段注释，它明确说了这是用描述符替换 CRTP ModuleBase<T> 模板的设计------这是 v1.18 之后的重构。老的写法是：

// 老写法（v1.17 及以前）
class MyModule : public ModuleBase<MyModule> {
public:
	static int task_spawn(int argc, char *argv[]);
	static MyModule *instantiate(int argc, char *argv[]);
	...
};

新写法变成了：

// 新写法（main 分支）
ModuleBase::Descriptor MyModule::desc{task_spawn, custom_command, print_usage};

class MyModule : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem {
	...
};

改的理由很实在：CRTP 会给每个模块生成一份 start/stop/status 的代码副本，几十个模块加起来很占 Flash。改成描述符之后这些静态方法只存一份。对 1MB Flash 的飞控来说，这是实打实的收益。

但对你写代码几乎没有影响。 用户接口还是那三件套：

mymodule start    # 启动
mymodule stop     # 停止
mymodule status   # 状态

start 的返回值是有意义的：

• 0：启动成功
• -1：启动失败
• 1：（在 PX4 模块约定里）表示已经启动 / 不需要重复启动

模块内部的标准结构是这样（以工作队列型模块为例）：

class MyModule : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem
{
public:
	MyModule();
	~MyModule() override;

	static int task_spawn(int argc, char *argv[]);   // 创建实例
	static int custom_command(int argc, char *argv[]); // 自定义子命令
	static int print_usage(const char *reason = nullptr);

	bool init();                                     // 订阅、初始化
	void Run() override;                             // 主循环，被工作队列调用
	int print_status() override;                     // status 命令打印什么
};

ModuleBase::Descriptor MyModule::desc{task_spawn, custom_command, print_usage};

extern "C" __EXPORT int mymodule_main(int argc, char *argv[])
{
	return ModuleBase::main(desc, argc, argv);
}

最后那个 mymodule_main 就是模块的入口点，pxh 里敲 mymodule 时就调它，然后 ModuleBase::main 负责解析 start / stop / status 参数并分发。

3.6 一个小实战：从零理解参数生效过程

这个练习我强烈建议你在真机或 SITL 上做一遍，五分钟就能把前面这些串起来。

# 1. 打开 shell
python3 Tools/px4.py /dev/ttyACM0

# 2. 看当前姿态环的 P 增益
pxh> param show MC_ROLL_P
MC_ROLL_P                   6.500000

# 3. 看这个参数被谁用着 ------ 先看模块在不在
pxh> mc_att_control status
mc_att_control: running

# 4. 改它
pxh> param set MC_ROLL_P 5.0
# 注意：PX4 的参数是写入即生效的，控制模块会收到 parameter_update 通知

# 5. 确认它真的变了
pxh> param show MC_ROLL_P
MC_ROLL_P                   5.000000

# 6. 重启一下，确认参数持久化了
pxh> reboot
# 重新连上后
pxh> param show MC_ROLL_P
MC_ROLL_P                   5.000000     ← 参数被存到了存储介质上

为什么改参数当场就生效？ 因为这是一个发布订阅系统：

1. param set 把新值写进参数存储
2. 参数系统发布一个 parameter_update topic
3. 所有订阅了 parameter_update 的模块（几乎所有模块都订）会在下一次 Run() 里看到它
4. 模块读取新参数、重算增益

你在 mc_rate_control 的代码里能看到这个模式的原文：

void MulticopterRateControl::Run()
{
	...
	// Check if parameters have changed
	if (_parameter_update_sub.updated()) {
		parameter_update_s param_update;
		_parameter_update_sub.copy(&param_update);
		updateParams();          // 批量拉取新参数
		parameters_updated();    // 重算派生的系数
	}
	...
}

这个"parameter_update → updateParams() → parameters_updated()"的三段式，你在任何一个 PX4 模块里都能看到。记住它。

---

第 4 章 中间件核心：uORB

如果只能让你记住 PX4 的一个东西，我会选 uORB。整个系统的"松耦合"就靠它撑着。

4.1 为什么是发布订阅

官方架构文档给的理由是三条：

• 系统是响应式的：异步，有新数据立刻更新
• 完全并行：所有操作和通信互不阻塞
• 线程安全的数据消费：任何模块可以在任何地方消费数据

翻译成人话，好处是：

模块之间没有直接依赖。 mc_att_control 不需要知道 mc_pos_control 的类名、不需要 include 它的头文件、不需要链接它。它只订阅 vehicle_attitude_setpoint 这个 topic。你可以把 mc_pos_control 整个换成你自己的位置控制器，只要它还发 vehicle_attitude_setpoint，姿态环一行都不用改。这就是为什么"PX4 里的模块可以在运行时被替换"------因为它本来就没有硬链接。

代价是什么？ 三个：

1. 调试变难 。传统的"函数调用栈"在这里不存在了。你看到 mc_att_control 拿到了一个奇怪的东西，得反推是谁发的、什么时候发的。
2. 时序不直观。同一个时刻，A 模块可能已经用上了新数据，B 模块还在用上一帧。
3. 数据新鲜度要自己管 。订阅者拿到的是"最后一次发布的值"，可能是 1ms 前的，也可能是 1 秒前的。判断数据是否过期是订阅者自己的责任，这是新手最常犯的错------拿着一个 5 秒前的 GPS 数据当实时数据用。

所以在 PX4 里你会看到大量这种检查：

if (!_vehicle_local_position.xy_valid || (hrt_elapsed_time(&_vehicle_local_position.timestamp) > 500_ms)) {
	// 数据过期，走降级逻辑
}

这个习惯你必须养成：拿到任何 topic，先想"它多久没更新了"。

4.2 消息定义与代码生成

msg/ 目录下有 233 个消息定义文件（不含 versioned/ 子目录），全部是 .msg 格式。

命名规则（v1.16 之后统一了）：

• 文件名 CamelCase ：VehicleAttitude.msg
• topic 名 snake_case ：vehicle_attitude
• 代码里 include snake_case 头文件 ：#include <uORB/topics/vehicle_attitude.h>
• 引用 topic 用 ORB_ID() ：ORB_ID(vehicle_attitude)

一个最简单的消息定义：

# Velocity and yaw rate limits for a multicopter position slow mode only

uint64 timestamp                           # [us] Time since system start.

float32 horizontal_velocity                # [m/s] Horizontal velocity.
float32 vertical_velocity                  # [m/s] Vertical velocity.
float32 yaw_rate                           # [rad/s] Yaw rate.

硬性规定 ：任何消息都必须 有 uint64 timestamp 字段。日志系统靠它对齐时间轴，没有它 logger 直接不收。

几个进阶特性：

多 topic 消息。 一份定义可以生成多个 topic：

# TOPICS actuator_outputs actuator_outputs_sim actuator_outputs_debug

嵌套消息。 消息可以嵌套，构建复杂结构：

PositionSetpoint previous
PositionSetpoint current
PositionSetpoint next

缓冲区长度。 默认每个 topic 只有一个消息的缓冲区 。发布太快、订阅者读太慢，数据就被覆盖丢掉了。对传感器数据无所谓（我们只要最新的），但对 vehicle_command 这种"命令"就不行------丢一条命令可能就少了一次指令。

uint8 ORB_QUEUE_LENGTH = 4   # 必须是 2 的幂，2/4/8...

vehicle_command 就是这么干的，所以命令不会丢。

废弃约定。 你要删字段删消息，得按规矩来，不然会破坏外部工具（Flight Review 等）：

• 删消息 → 加到 msg/CMakeLists.txt 的 deprecated_msgs 列表
• 删字段 → 注释掉并标 # DEPRECATED: xxx，防止后人再加回来
• 改语义（比如单位从度变成弧度）→ 必须改名 + 老字段标记废弃

编译生成什么？ make 时，Tools/msg/ 下的 Python 脚本（generate_uorb_topic_files.py 之类）会把 .msg 生成 C++ 头文件，放到 build/<target>/uORB/topics/。生成的结构体就是消息名加 _s 后缀（vehicle_attitude_s），字段名和 .msg 里一致。

4.3 运行时结构：三个关键类

uORB 的实现全在 platforms/common/uORB/。核心是三个东西：

uORBManager      单例，全局管理者。维护 topic 名 ↔ ORB_ID 的映射
uORBDeviceMaster 管理所有 topic 的"元信息"，持有 uORBDeviceNode 列表
uORBDeviceNode   一个 topic = 一个 DeviceNode。它是真正的数据容器

关系是这样的：

                    uORBManager (单例)
                         │
                         │ getDeviceNode(meta)
                         ▼
                 uORBDeviceMaster
                         │
        ┌────────────────┼────────────────┐
        ▼                ▼                ▼
  uORBDeviceNode   uORBDeviceNode   uORBDeviceNode
  sensor_combined  vehicle_attitude  vehicle_command
     [0] [1] [2]        [0]              [0]

uORBDeviceNode 就是一个带锁的环形缓冲区 ，加上元数据（发布者数量、订阅者计数、最后发布时间等）。多实例（sensor_gyro[0]、[1]、[2]）对应多个 DeviceNode。

为什么在嵌入式上也能很快？ 因为整个系统跑在单一地址空间里 。uORB 传数据本质上是 memcpy 到共享内存，没有序列化、没有系统调用、没有内核态切换。这就是官方文档说的"uORB 基于共享内存"。

一个性能数字给你个概念：一次 Publication::publish() 加上一次 Subscription::copy()，在 H7 上量级是几微秒。所以 PX4 里 1kHz 的循环里发几个 topic 完全不心疼。

官方文档里还提了一句有意思的设计目标：整个中间件跑在单地址空间，但设计上留了"最小代价改成多地址空间"的口子------需要改的只有 uORB、参数接口、dataman 和 perf 这四块。这说明 PX4 从一开始就是奔着"通用机器人中间件"去的，而不是"只给飞控用"。

4.4 订阅的五种姿势

这是 uORB 最容易用错的地方。五种订阅方式，各有适用场景，选错了性能或者实时性就出问题。

1. uORB::Subscription ------ 最基础

uORB::Subscription _sensor_combined_sub{ORB_ID(sensor_combined)};

void Run()
{
	sensor_combined_s data;
	if (_sensor_combined_sub.update(&data)) {   // 有新数据才返回 true
		// 处理
	}
}

update() 内部会检查时间戳，有新数据才返回 true。这是你自己的任务里最常用的方式。

2. uORB::SubscriptionBlocking ------ 阻塞等待

uORB::SubscriptionBlocking<sensor_combined_s> _sub{ORB_ID(sensor_combined)};
sensor_combined_s data;
while (!should_exit()) {
	_sub.copy(&data);       // 会阻塞，直到有新数据
	...
}

独占任务（有自己线程的模块）用这个。没有新数据时线程睡眠，不浪费 CPU。

注意：工作队列里的模块绝对不能用它。 工作队列里的模块是协作式的，阻塞会把整个队列上所有模块卡死。

3. uORB::SubscriptionCallback ------ 回调驱动（重点）

// init() 里注册
_vehicle_angular_velocity_sub.registerCallback();

// 之后每次有新数据，工作队列就会自动调 Run()
void MulticopterRateControl::init()
{
	if (!_vehicle_angular_velocity_sub.registerCallback()) {
		PX4_ERR("callback registration failed");
		return false;
	}
	return true;
}

工作队列型模块的标准做法，也是最优雅的一种：

• 零延迟 ：数据一到就触发 Run()
• 零空转：没数据就完全不跑

mc_rate_control 就是这么实现的，Run() 里的第一行就是判空：

void MulticopterRateControl::Run()
{
	...
	vehicle_angular_velocity_s angular_velocity;
	if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
		// 真的拿到新数据才干活
	}
}

4. uORB::SubscriptionInterval ------ 限频

uORB::SubscriptionInterval _sub{ORB_ID(vehicle_local_position), 100_ms};

// 最多每 100ms 处理一次，不管发布多快
if (_sub.update(&data)) { ... }

日志模块、需要降频处理的地方用。给 logger 最喜欢。

5. uORB::SubscriptionMultiArray ------ 多实例

uORB::SubscriptionMultiArray<sensor_gyro_s, 3> _gyro_subs{ORB_ID::sensor_gyro};

for (auto &sub : _gyro_subs) {
	sensor_gyro_s gyro;
	if (sub.update(&gyro)) { ... }
}

选型速查表（这个表我建议你贴在显示器边上）：

场景	用什么
工作队列模块，按数据驱动	SubscriptionCallback + registerCallback()
工作队列模块，顺便收点别的数据	Subscription
独立任务，等数据干活	SubscriptionBlocking
需要降频	SubscriptionInterval
多传感器/多实例	SubscriptionMultiArray
只需要"有没有更新"	Subscription + updated()

4.5 多实例

这个坑我见过太多次了，单独说。

多实例（Multi-instance） 指同一个 topic 有多个独立的数据容器，索引区分：sensor_gyro[0]、sensor_gyro[1]、sensor_gyro[2]。

多 topic（Multi-topic） 指一份 .msg 定义生成几个不同的 topic 名。这是两码事，别混。

// 发布多实例
uORB::PublicationMulti<sensor_gyro_s> _pub{ORB_ID(sensor_gyro)};
int instance = _pub.get_instance();   // 拿到自己的索引

// 订阅指定实例（不指定默认是 0）
uORB::Subscription _sub{ORB_ID(sensor_gyro), 1};   // 订第 1 个

几条铁律：

1. 不要对同一个 topic 同时用 orb_advertise 和 orb_advertise_multi。混用会导致实例索引错乱，症状是"订阅者永远收不到数据"。这个 bug 很隐蔽，因为两边代码都没错，就是约定不一致。
2. PublicationMulti 析构会释放实例 ，Publication 析构不会（因为可能有别人也在发）。
3. sensor_gyro、sensor_accel、sensor_mag、sensor_baro 都是多实例 topic 。你在 listener 里看到 sensor_gyro[0]、[1] 就是这个意思。

4.6 队列、丢包与"数据从哪来"

默认单缓冲意味着什么？

假设 sensor_combined 以 250Hz 发布，你的订阅者以 10Hz 读取。那么你每次读到的都是"最近那一帧"，中间 24 帧被覆盖了。这在传感器场景下是完全正确的行为------我们只关心最新值。

但如果你的逻辑是"每一条命令都必须处理"，单缓冲就会丢数据。这时候用 ORB_QUEUE_LENGTH。

一个实用的诊断命令：

pxh> listener sensor_combined
# 会打印出当前值和它的 timestamp
# 连续看几次，如果 timestamp 一直不变 → 发布者挂了

排查"数据从哪来"的标准套路：

# 1. 先看 topic 有没有在跑
pxh> uorb top vehicle_attitude

# 2. 看内容合不合理
pxh> listener vehicle_attitude

# 3. 反查谁在发 ------ 去代码里搜
grep -rn "ORB_ID(vehicle_attitude)" src/ | grep -i "publish\|Publication"

第 3 步是最靠谱的。PX4 里几乎不用间接发布，谁发的一搜就知道。

4.7 消息版本化（v1.16 引入的新东西）

这个是新东西，老教程里没有，但你必须知道，否则你会很困惑"为什么 msg/ 下面还有个 versioned/"。

问题背景 ：PX4 和 ROS 2 通过 DDS 通信，两边的消息定义必须完全一致。但 PX4 和 ROS 2 应用的升级节奏不同步------PX4 升到 v1.16 改了 VehicleAttitude，你那个编译于 v1.14 时代的 ROS 2 节点就崩了（字段对不上）。

解决方案：给消息加版本号，并配一个"翻译节点"。

msg/
├── *.msg                      ← 非版本化消息（内部用，随便改）
├── versioned/*.msg            ← 版本化消息的当前版本（对 ROS 2 暴露）
├── px4_msgs_old/msg/*.msg     ← 历史版本（如 VehicleStatusV2.msg）
└── translation_node/          ← 翻译节点：自动在新旧版本间转换

版本化消息多一个字段：

uint32 MESSAGE_VERSION = 3

msg/versioned/ 里现在有 40 个消息，都是要暴露给 ROS 2 的关键接口：

ActuatorMotors  ActuatorServos  AirspeedValidated  ArmingCheckReply
ArmingCheckRequest  AuxGlobalPosition  BatteryStatus  ConfigOverrides
Event  FixedWingLateralSetpoint  FixedWingLongitudinalSetpoint
GotoSetpoint  HomePosition  LateralControlConfiguration
LongitudinalControlConfiguration  ManualControlSetpoint  ModeCompleted
RaptorInput  RaptorStatus  RegisterExtComponentReply  ...
TrajectorySetpoint  VehicleAngularVelocity  VehicleAttitude
VehicleAttitudeSetpoint  VehicleCommand  VehicleCommandAck
VehicleGlobalPosition  VehicleLandDetected  VehicleLocalPosition
VehicleOdometry  VehicleRatesSetpoint  VehicleStatus  VtolVehicleStatus  Wind

注意：同一个 topic 名，版本化和非版本化是两个不同的消息定义。 别去 msg/versioned/VehicleAttitude.msg 里找 VehicleAttitude.msg------它只是恰好被归到了 versioned 目录。

translation_node 是一个 ROS 2 侧运行的东西。当 PX4 和 ROS 2 应用的版本不一致时，它负责在中间做字段映射。对你写 ROS 2 节点的直接好处是：升级 PX4 之后，你的老节点不会立刻炸。

顺便说：px4_msgs 这个 ROS 2 消息包就是从 msg/ 自动生成的。改消息定义 → 重新生成 px4_msgs → ROS 2 侧重新编译。

4.8 性能与踩坑清单

我在 PX4 上跟 uORB 相关的坑，按遇到频率排序：

坑 1：在中断上下文里第一次发布 topic。

文档明确说了：topic 必须在非中断上下文里至少发布过一次 （advertise 过），才能在中断里发布。否则会访问未初始化内存。传感器驱动的 SPI 中断里发数据是常见场景，一定要注意在 init() 里先发一次。

坑 2：忘了检查数据新鲜度。

前面说过了，再强调一遍。订阅者拿到的是缓存值，不是实时值。做法：

if (hrt_elapsed_time(&data.timestamp) > 100_ms) {
	// 过期，降级
}

坑 3：在回调里做重活。

SubscriptionCallback 触发的是工作队列里的 Run()。你在这儿做个 5ms 的复杂运算，整个队列上所有模块都要等。工作队列是协作式的，没有抢占。

坑 4：以为自己订阅的是最新的，其实是上一帧。

发布时间戳和消费时间戳不一样。涉及多传感器融合时（比如相机 + IMU 时间同步），必须用 timestamp_sample 而不是 timestamp。

坑 5：listener 不显示刚改的 topic。

listener 是编译时决定要不要包含的（CONFIG_SYSTEMCMDS_TOPIC_LISTENER）。不是所有板子都有。

关于 timestamp 和 timestamp_sample 的区别（很重要）：

• timestamp：这条消息被发布的时刻
• timestamp_sample：数据被采样的时刻

对传感器来说，这俩差着驱动内部的积分/滤波时间，可能相差几百微秒。做融合和控制的算法必须用 timestamp_sample，否则会引入不可忽略的延迟误差。

4.9 uORB 相关的命令行

# 列出所有 topic（包括多实例）
pxh> uorb status
pxh> uorb top              # 实时显示所有 topic 的发布频率
pxh> uorb top -1           # 只显示一次
pxh> uorb top vehicle_attitude   # 只看某个

# 查看 topic 内容
pxh> listener sensor_combined
pxh> listener vehicle_attitude 5    # 打 5 条

# 直接用命令行发布 topic（调试神器！）
pxh> uorb_... # 一般用 listener 看，发布用下面的方式
pxh> actuator_test 0 0.5    # 比如直接测试电机

uorb top 的输出长这样，这是我判断系统"健不健康"的最快手段：

TOPIC NAME                      INST #SUB #MSG #LOST #QSIZE
sensor_combined                    0    6  2456     0 1
vehicle_attitude                   0    8  1230     0 1
vehicle_local_position             0    5   100     0 1
vehicle_command                    0    3      2     0 4

怎么看：

• #MSG 是频率的近似值 （每秒采样数）。sensor_combined 2456 说明 IMU 在以约 250Hz 发布（PX4 的 uorb top 会做平均）。
• #LOST 不为 0 就要注意了。持续丢数据说明订阅者跟不上，或者队列长度不够。
• #SUB = 0 表示没人订阅，那这个 topic 就白发了（除了 logger）。
• #QSIZE 是队列长度，1 表示单缓冲。

一个很实用的技巧：飞完下来先跑 uorb top，看有没有 topic 的 #MSG 明显低于预期 。比如 vehicle_local_position 正常应该 100Hz 左右（EKF 输出），如果只有 20Hz，说明估计器出问题了。

---

第 5 章 任务、线程与工作队列

这一章讲的是"代码什么时候跑"。搞不清这个，你看 PX4 的代码会有种"这些函数到底谁在调"的漂浮感。

5.1 两种执行模型

官方文档的原文说得很清楚，模块有两种跑法：

方式一：独立任务（Task）。 模块有自己的栈、自己的优先级、自己的线程/任务。NuttX 上是一个 task，POSIX 上是一个 thread。

方式二：工作队列（Work Queue）。 模块挂在共享的工作队列线程上，和其他模块共用栈和优先级。

官方给的优劣对比我直接引述并补充我的理解：

	独立任务	工作队列
内存	每个任务独占栈（2--4KB）	多个模块共享一个栈
切换开销	每次调度都要切上下文	少（同一线程内顺序执行）
阻塞能力	可以 sleep/poll/阻塞 IO	不可以，会卡死整条队列
抢占	高优先级可抢占	队列内模块不能互相打断
长耗时计算	可以	会拖累同队列所有模块

判断原则，我总结成一句话：

周期性、需要阻塞、或者计算量大的 → 独立任务。

数据驱动、执行快（微秒级）、不需要睡的 → 工作队列。

按这个原则，PX4 里的分工是：

• 独立任务 ：commander、navigator、logger、mavlink（每个实例一个）、ekf2、dataman、mavlink 的各个串口实例
• 工作队列 ：所有传感器驱动、所有控制器（mc_* / fw_*）、sensors、control_allocator、rc_update、land_detector、flight_mode_manager

这个划分有道理：控制器是数据驱动的，每次来一帧 IMU 就跑一次，跑完就睡，天然适合工作队列。

一个非常实用的排查点 ：工作队列上的模块不会出现在 top 命令的输出里 。top 只能看到队列自己（比如 wq:lp_default）。要看队列里的模块，得用：

pxh> work_queue status

5.2 独立任务：px4_task_spawn_cmd

创建一个独立任务的标准写法（这段是官方文档的示例，commander 就是这么起的）：

independent_task = px4_task_spawn_cmd(
	"commander",                    // 任务名
	SCHED_DEFAULT,                  // 调度策略（RR 或 FIFO）
	SCHED_PRIORITY_DEFAULT + 40,    // 优先级
	3600,                           // 栈大小（字节）
	commander_thread_main,          // 入口函数
	(char * const *)&argv[0]        // 参数
);

优先级体系 （platforms/common/include/px4_platform_common/tasks.h）------这张表你得记住，出问题时要靠它判断"谁该先跑"：

#define SCHED_PRIORITY_MAX 255
#define SCHED_PRIORITY_MIN 0
#define SCHED_PRIORITY_DEFAULT 20

#define PX4_WQ_HP_BASE                      (SCHED_PRIORITY_MAX - 15)   // 240

#define SCHED_PRIORITY_FAST_DRIVER          (SCHED_PRIORITY_MAX - 0)    // 255
#define SCHED_PRIORITY_ACTUATOR_OUTPUTS     (PX4_WQ_HP_BASE - 3)        // 237
#define SCHED_PRIORITY_ATTITUDE_CONTROL     (PX4_WQ_HP_BASE - 4)        // 236
#define SCHED_PRIORITY_ESTIMATOR            (PX4_WQ_HP_BASE - 5)        // 235
#define SCHED_PRIORITY_LOG_WATCHDOG         (PX4_WQ_HP_BASE - 6)        // 234
#define SCHED_PRIORITY_POSITION_CONTROL     (PX4_WQ_HP_BASE - 7)        // 233
#define SCHED_PRIORITY_LOG_CAPTURE          (PX4_WQ_HP_BASE - 10)       // 230
#define SCHED_PRIORITY_SLOW_DRIVER          (PX4_WQ_HP_BASE - 35)       // 205
#define SCHED_PRIORITY_NAVIGATION           (SCHED_PRIORITY_DEFAULT + 5) // 25
#define SCHED_PRIORITY_LOG_WRITER           (SCHED_PRIORITY_DEFAULT - 10) // 10
#define SCHED_PRIORITY_PARAMS               (SCHED_PRIORITY_DEFAULT - 15) // 5

这张表的顺序就是 PX4 的"优先级政治"，直接决定了系统的实时性：

1. 最快的驱动（255） ------ IMU 的 SPI 中断/读取
2. 执行器输出（237） ------ 电机指令输出，比姿态控制还高一档
3. 姿态控制（236）
4. 估计器（235）
5. 位置控制（233）
6. 慢速驱动（205） ------ I2C 传感器之类
7. 导航（25） ------ 注意！导航的优先级远低于控制，因为它慢（几 Hz）

为什么执行器输出比姿态控制优先级高？ 因为如果姿态控制被中断打断、输出延迟了，飞机就抖；而输出本身很轻量，让它先跑完最安全。这是个很典型的嵌入式实时设计取舍。

注意这里有个历史遗留：SCHED_PRIORITY_* 这些宏现在基本上是给老代码看的参考值。新代码（工作队列时代）用的是 Kconfig 里的"相对优先级"，见下一节。

5.3 工作队列体系（这是现在的主流）

工作队列的实现分两块：

platforms/common/px4_work_queue/           ← 新一代（模块用这个）
├── WorkQueue.cpp            队列本体
├── WorkQueueManager.cpp     队列管理器，负责创建和调度
├── WorkItem.cpp             工作项基类
├── ScheduledWorkItem.cpp    定时/回调调度的工作项
└── WorkItemSingleShot.cpp   一次性工作项

platforms/common/work_queue/                ← 上一代（部分驱动还在用）
└── work_queue.c

所有可用的队列定义在 WorkQueueManager.hpp ，我完整列出来，连同它们的默认规则（来自 px4_work_queue/Kconfig）：

namespace wq_configurations
{
// 角速率控制环，最高优先级
static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", CONFIG_WQ_RATE_CTRL_STACKSIZE, CONFIG_WQ_RATE_CTRL_PRIORITY};

// 总线驱动的队列（每个 SPI/I2C 总线一个）
static constexpr wq_config_t SPI0{"wq:SPI0", ...};
static constexpr wq_config_t SPI1{"wq:SPI1", ...};
... SPI2 ~ SPI6

static constexpr wq_config_t I2C0{"wq:I2C0", ...};
... I2C1 ~ I2C4

// 导航 + 控制器
static constexpr wq_config_t nav_and_controllers{"wq:nav_and_controllers", ...};

// 视觉目标估计
static constexpr wq_config_t vte{"wq:vte", ...};

// 惯导（每个 INS 一个）
static constexpr wq_config_t INS0{"wq:INS0", ...};
... INS1 ~ INS3

// 高通/低通默认队列
static constexpr wq_config_t hp_default{"wq:hp_default", ...};
static constexpr wq_config_t lp_default{"wq:lp_default", ...};

// CAN
static constexpr wq_config_t uavcan{"wq:uavcan", ...};

// 每个串口一个队列
static constexpr wq_config_t ttyS0{"wq:ttyS0", ...};
... ttyS1 ~ ttyS9, ttyACM0, ttyUnknown
}

默认优先级和栈大小（负数越小优先级越低），这是我在 Kconfig 里抠出来的实际值：

队列	相对优先级	栈大小	备注
wq:rate_ctrl	0（最高）	3150 B	角速率环，唯一一个 0
wq:SPI0 ~ SPI6	-1	---	SPI 传感器
wq:I2C0 ~ I2C4	-8	---	I2C 传感器
wq:nav_and_controllers	-13	2240 B	导航/位置控制
wq:INS0 ~ INS3	-14	---	惯导
wq:hp_default	-18	2800 B	高速默认
wq:lp_default	-50	3500 B	低速默认（最低）
wq:ttyS*	---	---	串口

关键实现细节 （WorkQueueManager.cpp 第 297 行）：

int sched_priority = sched_get_priority_max(SCHED_FIFO) + wq->relative_priority;

所以相对优先级是相对 SCHED_FIFO 的最大优先级（255）来算的 。wq:rate_ctrl 是 0 → 实际 255；wq:lp_default 是 -50 → 实际 205。

看清楚这个公式，你就明白为什么 PX4 的控制能这么稳：角速率环跑在 NuttX 的最高优先级上（255），仅次于硬件中断。

队列的管理器在启动时创建队列 。WorkQueueManagerStart() 会在 rcS 早期被调用，然后所有 wq:* 队列作为任务被创建出来。之后每个模块用 px4::wq_configurations::xxx 声明自己属于哪个队列，WorkQueueFindOrCreate() 负责找或者建。

一个必须知道的映射规则：传感器驱动的队列是按总线自动决定的。

// 从设备 ID 反推该用哪个队列
WorkQueue *WorkQueueFromDeviceId(uint32_t device_id);
// I2C 设备 → wq:I2Cn
// SPI 设备 → wq:SPIn
// 其他 → wq:hp_default

所以你把一个 IMU 从 SPI1 挪到 SPI2，它的驱动自动换到 wq:SPI2，代码不用改。这个设计非常舒服。

5.4 ScheduledWorkItem：uORB 回调的承载者

工作队列模块的基类选择有三种，我按使用频率排：

1. ScheduledWorkItem（最常用）

class MyModule : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem
{
public:
	MyModule() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) {}

	void Run() override;   // 被调用的入口
};

它提供的能力：

ScheduleNow();                    // 立刻排一次
ScheduleDelayed(10_ms);           // 10ms 后跑一次
ScheduleOnInterval(1_ms);         // 每 1ms 跑一次（周期）
ScheduleClear();                  // 取消

2. px4::WorkItem（不带自动调度）

只有一个 Run()，调度完全靠 uORB 回调。mc_rate_control 用的就是它：

MulticopterRateControl::MulticopterRateControl(bool vtol) :
	ModuleParams(nullptr),
	WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
	...

然后 init() 里注册回调，数据来了才跑：

bool MulticopterRateControl::init()
{
	if (!_vehicle_angular_velocity_sub.registerCallback()) {
		PX4_ERR("callback registration failed");
		return false;
	}
	return true;
}

这是 PX4 现在最优雅的实时模式：不在固定的时间点空转，而是"IMU 数据一到就开始算"。既没有调度抖动引入的延迟，也不浪费 CPU。

3. WorkItemSingleShot：跑一次就结束，用在一次性任务上。

perf：模块性能的标准写法

每个控制器模块里都有这几行，你也应该照抄：

MulticopterRateControl::MulticopterRateControl(bool vtol) :
	...
	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME ": cycle"))
{}

void MulticopterRateControl::Run()
{
	perf_begin(_loop_perf);
	// ...干活...
	perf_end(_loop_perf);
}

然后：

pxh> mc_rate_control status
# 或者
pxh> perf

输出类似：

mc_rate_control: cycle: 6144 events, 71833us elapsed, 11.69us avg, min 8us max 421us, 0.000us stddev

avg 和 max 是我最关心的两个数。 avg 11.69us 说明这个模块平均每次跑 11.69 微秒，很轻。但如果 max 421us------哪怕只有一次------那就麻烦了，说明有一次跑了 421 微秒，这会直接造成一次控制延迟。实时系统里 max 比 avg 重要得多。

5.5 高精度定时器 hrt

除了 uORB 回调，另一种触发方式是高精度定时器：

#include <drivers/drv_hrt.h>

hrt_call_every(&_call, 1000, 1000, timer_callback, this);  // 1ms 周期
hrt_cancel(&_call);

hrt（high-resolution timer）的粒度在 NuttX 上是微秒级。老一代的驱动和模块大量用它。新代码里，能用 uORB 回调就用回调 ------因为回调天然和数据对齐，而 hrt_call_every 是独立的时间基准，你会有"定时器触发了但数据还没到"的竞态。

5.6 实时性、栈溢出与观察手段

栈：最容易踩的坑。

工作队列的栈是共享的，所以栈大小要看队列里最"深"的那个模块。Kconfig 里能配：

pxh> work_queue status

输出会显示每个队列的栈使用情况（stack used）。看到超过 70% 就要警觉了。

NuttX 上还有一个"栈涂色（stack coloring）"机制：系统会往栈里填特定的值，定期检查还剩多少没被覆盖，从而估算栈使用量。栈溢出在嵌入式上是灾难性的------它会静默地破坏相邻内存，症状是各种莫名其妙的随机崩溃。

优先级反转。 工作队列里的模块是协作式的（不能互相打断），所以队列内部不存在优先级反转。但队列之间 会：如果 wq:lp_default 上的某个模块长时间不放手，wq:rate_ctrl 是抢得到 CPU 的（优先级更高），这个没问题。真正的问题是共享资源竞争------比如两个队列都要访问同一个 I2C 总线，低优先级队列持着总线锁，高优先级队列就得等。

PX4 里解决这类问题的方式是"一个总线一个队列"。这也是为什么会有 wq:I2C0 到 wq:I2C4 这么多队列，而不是统一一个 wq:i2c。

task_watchdog：看门狗。

pxh> task_watchdog status
pxh> task_watchdog start

它监控关键任务有没有卡死。在 rcS 里通过 USE_TASK_WATCHDOG 控制是否启用。

hardfault_log：崩溃日志。

pxh> hardfault_log info     # 看有没有崩溃记录
pxh> hardfault_log reset    # 清掉
pxh> hardfault_log commit   # 提交（写到 SD 卡）

芯片硬件异常（HardFault）后，PX4 会把现场信息存到 RTC 备份域（掉电不丢），下次启动 rcS 会检查并报告。如果你的飞机"莫名其妙重启了"，第一件事就是查这个。

观察工具速查：

pxh> top                    # NuttX 任务列表：CPU 占用、栈、优先级
pxh> work_queue status      # 所有队列 + 队列里的模块 + 栈使用
pxh> perf                   # 所有 perf 计数器
pxh> free                   # 内存使用
pxh> dmesg                  # 内核日志
pxh> load_mon status        # 系统负载

top 的输出解读（NuttX 上）：

PID PRIO  USER STACK USED  CPU
  0  255  nsh  1000   424   0.0%
  5  255  px4  2800  1504   2.3%   ← wq:rate_ctrl
 12   20  commander 3600 2100 0.5%

CPU 那一列加起来如果超过 100%，说明系统过载了。 这是我判断"这块片子够不够用"的第一个指标。

---

第 6 章 参数系统

参数是 PX4 里你摸得最多、但最容易被当成黑盒的东西。这章把它拆开。

6.1 参数存在哪

PX4 的参数不只是内存里的一堆变量，它有持久化：

Flash (FRAM/EEPROM/内部 Flash)  ← 主存储，掉电不丢
        ↕
内存中的参数表（全局唯一）
        ↕
SD 卡 /fs/microsd/params.txt    ← 纯文本备份

几个关键的文件和命令：

pxh> param show          # 显示所有参数
pxh> param show MC_*     # 通配符
pxh> param show -c       # 只看被改过的（非默认值）------ 极其有用！
pxh> param set MC_ROLL_P 6.0
pxh> param get MC_ROLL_P
pxh> param reset MC_ROLL_P
pxh> param reset_all     # 重置所有
pxh> param save          # 强制保存到 Flash
pxh> param load          # 从 Flash 重新加载
pxh> param import        # 从 SD 卡的 params.txt 导入
pxh> param export /fs/microsd/params.txt

param show -c 是我最常用的参数命令。 调试别人的飞机时，第一件事就是跑它，看看这架飞机的参数跟默认值差在哪------往往一眼就能看出问题（比如 MPC_XY_VEL_MAX 被改成了 20）。

6.2 参数怎么定义：YAML → 代码

这是 v1.14 之后的重要变化。参数不再写在 C++ 源码里的 PARAM_DEFINE_FLOAT 宏里了 ，而是定义在 *.yaml 文件里，编译时自动生成代码。

看 src/modules/fw_rate_control/fw_rate_control_params.yaml：

module_name: fw_rate_control
parameters:
- group: FW Rate Control
  definitions:
    FW_USE_AIRSPD:
      description:
        short: Use airspeed for control
        long: |-
          If set to 1, the airspeed measurement data, if valid, is used in the
          following controllers:
          - Rate controller: output scaling
          - Attitude controller: coordinated turn controller
          - Position controller: airspeed setpoint tracking, takeoff logic
          - VTOL: transition logic
      type: boolean
      default: 1

    FW_PR_P:
      description:
        short: Pitch rate proportional gain
      type: float
      default: 0.08
      unit: '%/rad/s'
      min: 0.0
      max: 10
      decimal: 3
      increment: 0.005

每个字段什么意思：

字段	作用
group	在 QGC 参数界面里归到哪个分组
short	短描述（列表里显示的）
long	长描述（鼠标悬停/详情显示的）
type	float / int32 / boolean
default	默认值
unit	单位（QGC 会显示）
min / max	取值范围（QGC 会做输入校验）
decimal	小数位数（UI 显示用）
increment	调节步长（UI 滑条用）

为什么要从 C++ 迁到 YAML？ 因为这样参数元数据可以自动生成成 JSON（make parameters_metadata），给 QGC、MAVSDK、ROS 2 用。以前是要手动维护文档和代码两份，经常对不上。

生成出来什么？ 编译后你会得到：

// src 里这样访问
(ParamFloat<px4::params::FW_PR_P>) _param_fw_pr_p

用法：

_param_fw_pr_p.get()        // 读
_param_fw_pr_p.set(0.1f);   // 写
_param_fw_pr_p.get_default() // 读默认值

一个重要约定 ：模块只在自己处理 parameter_update 时重新读取参数，不要每帧都 get() 。虽然 get() 很便宜，但每帧读几十个参数累积起来也是钱，而且容易造成"参数改了但行为没变"的困惑。

规范写法（所有 PX4 模块都这样）：

void Run()
{
	if (_parameter_update_sub.updated()) {
		parameter_update_s param_update;
		_parameter_update_sub.copy(&param_update);
		updateParams();        // 这一步批量刷新所有 ParamXXX 成员
		parameters_updated();  // 这一步把参数换算成派生系数（如 K*P）
	}
	...
}

mc_rate_control 里那段就是范本，注意它做了"参数 → 派生系数"的换算：

void MulticopterRateControl::parameters_updated()
{
	// The controller gain K is used to convert the parallel (P + I/s + sD) form
	// to the ideal (K * [1 + 1/sTi + sTd]) form
	const Vector3f rate_k = Vector3f(_param_mc_rollrate_k.get(),
	                                 _param_mc_pitchrate_k.get(),
	                                 _param_mc_yawrate_k.get());
	_rate_control.setPidGains(
		rate_k.emult(Vector3f(_param_mc_rollrate_p.get(), ...)),
		rate_k.emult(Vector3f(_param_mc_rollrate_i.get(), ...)),
		rate_k.emult(Vector3f(_param_mc_rollrate_d.get(), ...)));
	...
}

这段注释值得单独说 ：PX4 的多旋翼角速率环用的是"并联形式"参数（独立的 P/I/D），但内部是"理想形式"（K·1 + 1/(sTi) + sTd）。K 是归一化增益，MC_ROLLRATE_K 影响整体响应速度，而 _P/_I/_D 影响各分量比例。这个设计意味着：调整整体响应速度只需要动 K，不用同时调三个参数。 搞明白这一点，你对多旋翼调参的理解会上一个台阶。

6.3 参数与机型（airframe）

参数和机型的关系是双向的：

机型 → 参数 ：机型文件里那一堆 param set-default XXX YYY 就是机型的"出厂参数"。

param set-default MC_PITCH_P 6.5
param set-default MC_ROLL_P 6.5

注意是 set-default，不是 set。set-default 只在参数不存在或为默认值时生效------这保证了用户调过的参数不会被机型文件覆盖。

参数 → 机型 ：SYS_AUTOSTART 这个参数就是机型 ID。改了它，重启后飞机就换了机型和对应的参数集。

pxh> param set SYS_AUTOSTART 4001    # 改成 4001 号机型
pxh> param set SYS_AUTOCONFIG 1      # 同时要求恢复出厂设置
pxh> reboot

机型的元数据 在 ROMFS/px4fmu_common/init.d/airframes/ 里，每个文件顶部的注释就是元数据：

#!/bin/sh
#
# @name Generic Quadcopter
#
# @type Quadrotor
#
# @maintainer ...
#

. ${R}etc/init.d/rc.mc_defaults
set VEHICLE_TYPE mc
...

make airframe_metadata 会把这些注释提取成 JSON，QGC 就是靠它显示机型列表和对应参数的。

6.4 参数元数据与 QGC 显示

QGC/MAVSDK 那套参数界面的信息从哪来？从参数元数据 JSON：

make parameters_metadata
# 生成 build/px4_sitl_default/parameters.json（类似路径）

这个 JSON 里包含了每个参数的描述、单位、范围、分组、是否重启生效等等。QGC 拉到 MAVLink 参数表之后，用这个 JSON 来渲染界面。

所以：如果你自己加了个参数，但在 QGC 里显示不出描述，八成是忘了：

1. 在 yaml 里写描述
2. 重新 make parameters_metadata
3. 把生成的 JSON 喂给 QGC

6.5 一些实战经验和坑

坑 1：参数改了不生效。

90% 是模块没订阅 parameter_update，或者订阅了但没重新读。10% 是参数需要重启（有些参数标了 @reboot_required）。

# 判断是否需要重启
pxh> param show -c
# 元数据里带 "reboot_required": true 的就需要

坑 2：param show 看不到新参数。

参数是编译时生成的。你改了 yaml 不重新编译，代码里就没有这个参数。

坑 3：参数被自动重置。

SYS_PARAM_VER 的作用。固件升级后如果 SYS_PARAM_VER 提高了，rcS 会自动重置参数（但保留校准）。升级固件前一定要导出参数备份：

pxh> param export /fs/microsd/params_backup.txt
# 升级后
pxh> param import /fs/microsd/params_backup.txt

坑 4：param reset_all 之后飞机完全不能飞。

因为校准数据也被清了。但 PX4 的 rcS 里那个 param reset_all 是带排除列表的：

param reset_all SYS_AUTOSTART SYS_PARAM_VER SYS_AUTOCFG_CAL CAL_* LND_FLIGHT* TC_* COM_FLIGHT* MAV_SYS_ID

意思是"重置除了这些之外的所有参数"。你手动敲的时候如果忘了带排除项，校准就没了。手动重置参数请用带排除项的完整命令。

坑 5：SD 卡上的 params.txt 优先级。

启动时如果有 /fs/microsd/params.txt，PX4 会导入它。这会导致"我明明重置了参数，重启又变回来了"------查 SD 卡。

---

第 7 章 驱动层与硬件抽象

7.1 硬件抽象的分层

PX4 的硬件适配分三层，从上往下：

┌─────────────────────────────────────────────┐
│ 第一层：通用驱动（src/drivers/）              │
│   如：imu/invensense/icm42688p/             │
│   只依赖抽象接口，不关心具体是哪块板子          │
├─────────────────────────────────────────────┤
│ 第二层：平台抽象（platforms/common/）          │
│   I2C/SPI/UART/GPIO 的通用 API               │
│   px4_platform_common/i2c.h, spi.h, ...     │
├─────────────────────────────────────────────┤
│ 第三层：芯片与板级实现（platforms/nuttx/）      │
│   platforms/nuttx/src/px4/stm/stm32h7/       │
│   platforms/nuttx/src/px4/common/            │
│   实际操作寄存器的地方                         │
└─────────────────────────────────────────────┘

这个分层的价值：同一个 ICM42688P 驱动，在 STM32H7 上跑、在 SITL 上跑、在 Raspberry Pi 上跑，代码是一份。换平台只改第三层。

看 platforms/nuttx/src/px4/ 的结构你就能明白：

platforms/nuttx/src/px4/
├── common/            ← 跨芯片的 NuttX 适配
├── stm/
│   ├── stm32h7/       ← H7 特有实现
│   ├── stm32f7/       ← F7
│   └── ...
├── imxrt/             ← NXP i.MX RT
└── ...

7.2 驱动目录一览

src/drivers/ 下有 60 多个子目录，按类别分：

类别	目录	说明
IMU	imu/	按厂商再分（invensense / bosch / analog_devices / st / memsensing...）
磁力计	magnetometer/	同上
气压计	barometer/
GNSS	gps/（u-blox、Trimble...）、gnss/（Septentrio、Unicore、Femtom...）、cyphal/	注意 GPS 和 GNSS 是分开的目录
空速	differential_pressure/
测距	distance_sensor/	激光雷达、超声波、雷达
光流	optical_flow/
输出	pwm_out/、dshot/、uavcan/、pca9685_pwm_out/、tap_esc/	PWM/DShot/CAN
遥控	rc_input/、rc/、rpi_rc_in/	SBUS/CRSF/DSM/PPM
电源	power_monitor/、batt_smbus/、smart_battery/
协处理器	px4io/、voxl2_io/	单独的 IO 芯片
其他	camera_trigger/、tone_alarm/、heater/、rgbled/...
系统	actuators/、bootloaders/、serialpassthrough/

7.3 总线抽象与设备探测

设备 ID 与总线。 每个设备有一个 32 位的 device ID，编码了总线类型、总线号、地址：

// 从 device_id 反推总线
#include <px4_platform_common/i2c_spi_buses.h>

uint8_t bus = I2C_SPI_BUS_FROM_DEVID(device_id);

驱动的自动探测。 现代驱动都支持 start -a：

pxh> icm42688p start -a     # 自动扫描所有 SPI 总线找这个芯片

实现上，驱动会调用：

#define PX4_I2C_BUS_EXPANSION   ...
static constexpr px4_i2c_bus_t px4_i2c_buses[] = {
	{...}, {...},
	{}
};

int ICM42688P::start_bus_scan()
{
	// 遍历所有可能的总线，尝试 probe
	for (auto bus_option : px4_i2c_buses) { ... }
}

SPI 类似，通过 SPI 的 probe() 读 WHO_AM_I 寄存器判断。

这对移植的意义 ：你不用在 rcS 里硬编码"IMU 在 SPI1 的 CS0"，只要芯片在支持的总线上，start -a 自己会找到。这也解释了为什么 rc.board_sensors 越来越短了。

7.4 传感器驱动输出什么

这是新手容易搞混的地方：IMU 驱动不直接发 vehicle_attitude，它发的是原始数据。

┌──────────────────────────────────────────────────────────────┐
│ icm42688p 驱动（工作队列 wq:SPI1）                             │
│   SPI 读寄存器 → 原始 ADC 值 → 乘 scale → 物理量               │
│   积分/降采样（1kHz 采样 → 250Hz 发布）                        │
│   ↓                                                          │
│  sensor_gyro[0] / sensor_accel[0]  ← 原始传感器数据，多实例     │
└──────────────────────────────────────────────────────────────┘
                    ↓
┌──────────────────────────────────────────────────────────────┐
│ sensors 模块（负责"整合"）                                     │
│   轮询所有实例、投票、失效检测、温度补偿、坐标系校正             │
│   ↓                                                          │
│  sensor_combined        ← 给老代码用（已逐步废弃）              │
│  vehicle_angular_velocity  ← 角速度（IMU 数据 + 滤波 + 导数）    │
│  vehicle_acceleration      ← 加速度                            │
│  vehicle_imu[0..n]         ← 每个 IMU 的整合结果                │
│  vehicle_magnetometer / vehicle_air_data / ...                │
└──────────────────────────────────────────────────────────────┘
                    ↓
┌──────────────────────────────────────────────────────────────┐
│ ekf2（状态估计）                                               │
│   融合 IMU + 磁力计 + 气压计 + GPS + 光流 + 测距 + 视觉          │
│   ↓                                                          │
│  vehicle_attitude / vehicle_local_position / vehicle_global_position │
└──────────────────────────────────────────────────────────────┘

关键理解：驱动做"采样"，sensors 模块做"整合"，ekf2 做"融合"。 这三层分工明确，很多人以为驱动直接出 vehicle_attitude，那是错的（那是 ArduPilot 的老结构）。

为什么要 sensors 这一层？ 因为一架飞机可能有 3 个 IMU、2 个磁力计、3 个气压计。sensors 模块负责：

1. 收集所有实例的数据
2. 投票选出最可信的（比如 3 个 IMU 里 2 个接近、1 个偏离，就用多数派）
3. 失效检测（数据长时间不变、数值超范围、噪声过大 → 标记为坏）
4. 温度补偿 （用 TC_* 参数里的多项式系数，这些是靠 temperature_compensation 模块离线算出来的）
5. 坐标系转换 （把传感器本体系转成机体系，用 CAL_* 参数里的旋转矩阵）
6. 发布给下游用的"整车视角"的 topic

对应的源码结构：

src/modules/sensors/
├── sensors.cpp / sensors.hpp      ← 主模块
├── voted_sensors_update.cpp/.h    ← 投票逻辑
├── data_validator/                ← 数据有效性检查
├── vehicle_angular_velocity/      ← 角速度整合子模块
├── vehicle_acceleration/          ← 加速度整合
├── vehicle_air_data/              ← 气压/空速
├── vehicle_airspeed/
├── vehicle_gps_position/          ← GPS 整合
├── vehicle_imu/                   ← 单个 IMU 的整合
├── vehicle_magnetometer/
├── vehicle_optical_flow/
├── Integrator.hpp                 ← 积分器（采集与降采样）
└── sensor_params*.yaml            ← 参数定义

这个目录结构本身就是一张架构图 ，我建议你把这个 ls 的输出背下来，比看图有用。

7.5 输出驱动

输出侧的驱动比输入侧复杂，因为涉及"哪个电机接在哪个通道"这种机型相关的东西。

┌───────────────────────────────────────────────────────────────┐
│ control_allocator                                              │
│   输出 vehicle_torque_setpoint / vehicle_thrust_setpoint         │
│   （机型无关的"要多少力矩/推力"）                                 │
└───────────────────────────────────────────────────────────────┘
                    ↓
┌───────────────────────────────────────────────────────────────┐
│ 输出驱动（pwm_out / dshot / uavcan）                            │
│   内部用 mixer_module 基类                                      │
│   按机型配置做矩阵换算 → 每个通道的归一化值 [-1, 1]                │
│   ↓                                                            │
│  actuator_outputs + ActuatorMotors / ActuatorServos            │
│   ↓                                                            │
│  硬件寄存器 / DMA / CAN 帧                                       │
└───────────────────────────────────────────────────────────────┘

mixer_module 是所有输出驱动的公共基类 （src/lib/mixer_module/）。它负责：

• 管理输出通道的映射（PWM_MAIN_FUNC1、PWM_AUX_FUNC1 这些参数）
• 处理 actuator_test（地面测试电机）
• 处理 failsafe（失控时的输出行为）
• 处理 ActuatorTest / ActuatorArmed 等安全逻辑
• 提供统一的 status 输出

pxh> pwm_out status
INFO  [pwm_out] Running, 8 outputs
INFO  [pwm_out]     0: 4000us (armed: 1000us) [Motor 1]
INFO  [pwm_out]     1: 4000us (armed: 1000us) [Motor 2]
...

pwm_out status 是排查"电机不转"的神器，它能告诉你每个通道当前输出什么、映射到哪个功能。

7.6 加一个新驱动要动什么

给你一个清单，照着做就不会漏：

1. 建目录 ：src/drivers/<类别>/<厂商>/<型号>/
2. 写驱动类 ：继承 I2C/SPI 基类 + px4::ScheduledWorkItem，实现 probe()、init()、Run()
3. 写 CMakeLists.txt ：用 px4_add_module

px4_add_module(
	MODULE drivers__imu__invensense__mynewimu
	MAIN mynewimu
	COMPILE_FLAGS
	SRCS
		mynewimu.cpp
	DEPENDS
		drivers_imu
		px4_work_queue
	)

4. 加 Kconfig 开关 ：在 src/drivers/<类别>/Kconfig 里加

   menuconfig DRIVERS_IMU_MYNEWIMU
   bool "MyNewIMU"
   default n
   ---help---
   Enable support for MyNewIMU

5. 在板子配置里打开 ：boards/px4/fmu-v6x/default.px4board 加 CONFIG_DRIVERS_IMU_MYNEWIMU=y

6. 注册到传感器的元数据 ：在 src/lib/drivers/device/Device.hpp 附近加设备类型枚举（这一步经常被漏，漏了会导致 listener 看不到）

7. 在 rc.sensors 或 rc.board_sensors 里启动

最省事的做法 ：cp -r 一个结构最接近的现有驱动，改名改寄存器地址。PX4 里几乎所有驱动都是这么来的------你去看 git 历史，新驱动通常是照着邻居抄的。

7.7 px4io 与协处理器

老一点的 Pixhawk（FMUv2/3/4/5）有两个芯片：FMU （主控，跑 PX4）和 IO （辅助，跑 px4iofirmware）。

FMU (STM32H7/F7)  ──串口+共享内存──  IO (STM32F1)
  跑 PX4 全部逻辑                     跑 px4iofirmware
                                      负责：PWM 输出、遥控输入、
                                           安全开关、失控保护

为什么要 IO 芯片？ 两个原因：

1. 失效安全：即使 FMU 死机，IO 还能保持/切断输出
2. 实时性：PWM 输出由 IO 独立负责，不受 FMU 负载影响

源码在 src/modules/px4iofirmware/。它是个完全独立的固件，有自己的 rcS（ROMFS/px4fmu_common/init.d/rcS 里会 px4io start，然后上传 IO 固件）。

新板子（FMUv6X 及以后）很多不再有独立 IO 芯片 ，而是把 IO 功能合并进了主控（fmu-v6x 里叫 "FMU-v6X + IO"，但 fmu-v6c/fmu-v6u 就是单芯片）。这是成本和集成度驱动的演进。

对你调试的影响 ：在双芯片的板子上，"电机不转"可能是 FMU 的问题、可能是 IO 的问题、也可能是两者之间的串口通信问题。排查顺序是先看 px4io status，再看 pwm_out status。

---

第 8 章 传感器处理与状态估计

8.1 从 IMU 到 vehicle_angular_velocity

这条链路值得一步一步走，因为它是整个系统的源头，出了问题飞机就完全不能飞。

① 硬件：ICM42688P，SPI 接在 SPI1 上，CS 是 0 号

② 驱动 icm42688p（跑在 wq:SPI1，优先级 -1）
   - probe：读 WHO_AM_I（0x75），确认是 0x47
   - 配置：量程 ±16g / ±2000dps，ODR 1kHz
   - 数据就绪中断 或 定时读取
   - 采样：读 6 轴原始值 → 乘 scale → 转成 rad/s 和 m/s²
   - 每帧发：sensor_gyro[instance] / sensor_accel[instance]
     * timestamp_sample = 采样时刻（关键！）
     * timestamp        = 发布时刻
     * 还带 error_count、clip_count、temperature

③ Integrator（src/modules/sensors/Integrator.hpp）
   - 按驱动给的 raw data 做累积和降采样
   - 1kHz 采样 → 250Hz 发布（4 帧平均/抽取）
   - 保证时间戳对齐（timestamp_sample 是区间起点）

④ sensors 模块（跑在 hp_default 工作队列）
   - 收集所有 sensor_gyro[i] 实例
   - 失效检测：值突变？长时间不变？超出量程？
   - 投票：多数派一致 → 采信；不一致 → 用优先级
   - 温度补偿：用 TC_G0_* / TC_G1_* 等参数做多项式补偿
   - 坐标系校正：应用 CAL_GYRO0_XROT 等旋转参数
   - 发布 vehicle_angular_velocity（整合后的角速度 + 角加速度）

⑤ ekf2（跑在自己的任务里，优先级 235）
   - 用 vehicle_angular_velocity 做预测（IMU 积分）

这里有几个非常重要的概念：

timestamp_sample 与 timestamp。 前面提过，这里再具体化。IMU 的 timestamp_sample 是"这个积分窗口的起点"，timestamp 是"发布时刻"。EKF2 做预测时用的是 timestamp_sample，如果驱动把这两个搞混了，会引入固定的时间偏差 → 位置估计整体偏移 → 悬停时飞机缓慢漂移。这是那种"代码看起来全对但飞机就是不准"的典型病因。

error_count 与 clip_count。 传感器消息里带这两个字段。

• clip_count：原始 ADC 值触到量程边界的次数。如果这个数一直在涨，说明你的振动太大或者量程设小了。
• error_count：通信错误次数。涨说明 SPI/I2C 有问题。

pxh> listener sensor_accel 5
# 看 clip_count 是不是 0

这是判断"机械振动是否过大"的第一手数据。 振动大 → IMU 饱和（clip）→ 姿态估计发散 → 飞机抖。这条因果链在 PX4 上非常常见。

8.2 sensors 模块：投票与失效检测

sensors 模块做的事，用一个词概括是仲裁。

投票机制（voted sensors）。 源码在 voted_sensors_update.cpp。核心思想很简单：

• 有 N 个同类型传感器（比如 3 个 IMU）
• 计算它们的"一致性"
• 多数一致的被认为是正确的
• 落单的被标记为坏

实现上不是简单的"少数服从多数"，而是有一套加权和滞回（hysteresis 库在这里派上用场）：避免传感器在临界值附近反复跳来跳去。

失效检测的几种典型情况：

症状	检测方式	后果
数据长时间不变	时间戳超时	标记失效，切到备用传感器
数值突然跳变	与其它传感器/模型偏差过大	标记失效
噪声异常增大	方差超阈值	降权或失效
长时间触量程	clip_count 激增	记录事件，可能降权
通信错误激增	error_count	记录事件

这些信息最后汇总到 SensorsStatus / SensorsStatusImu / SensorsStatusGnss 这些 topic 里 ，然后 commander 的健康检查会读它们决定"能不能解锁"。

pxh> listener sensors_status_imu
# 看每个 IMU 的 healthy、priority、是否 enable

事件系统（events）。 PX4 有个 events 模块，负责把"发生了什么事"用统一的 ID 广播出去，然后由 MAVLink 转成 STATUSTEXT 报给地面站、或者记进日志。

pxh> events

这个命令输出的东西，是排查现场问题最有价值的信息之一。 它有历史记录，飞机飞完下来，你先跑 events，往往就能看到"其实早就报警了"。

事件的定义在 src/lib/events/ 里的 .json 文件，是结构化定义的------所以 QGC 能把它渲染成多语言的可读文本，而不是纯字符串。这是 PX4 一个挺讲究的地方。

8.3 EKF2：PX4 的心脏

EKF2 是 PX4 里最复杂、也最值钱的模块。

它是什么： 一个误差状态（error-state）扩展卡尔曼滤波器，融合 IMU、磁力计、气压计、GPS、光流、测距、视觉等所有可用信息，输出姿态、位置、速度的估计。

状态量（24 个误差状态）。 这个结构是自动生成的，定义在 src/modules/ekf2/EKF/python/ekf_derivation/generated/state.h，我把原文搬过来：

struct StateSample {
	matrix::Quaternion<float> quat_nominal{};   // 姿态四元数（4 个存储元素，3 个自由度）
	matrix::Vector3<float>    vel{};            // NED 速度
	matrix::Vector3<float>    pos{};            // NED 位置
	matrix::Vector3<float>    gyro_bias{};      // 陀螺零偏
	matrix::Vector3<float>    accel_bias{};     // 加速度计零偏
	matrix::Vector3<float>    mag_I{};          // 地磁（惯性系）
	matrix::Vector3<float>    mag_B{};          // 磁力计零偏（机体系）
	matrix::Vector2<float>    wind_vel{};       // 风速（北/东）
	float                     terrain{};        // 地形高度
};

namespace State {
	static constexpr IdxDof quat_nominal{0,  3};
	static constexpr IdxDof vel         {3,  3};
	static constexpr IdxDof pos         {6,  3};
	static constexpr IdxDof gyro_bias   {9,  3};
	static constexpr IdxDof accel_bias  {12, 3};
	static constexpr IdxDof mag_I       {15, 3};
	static constexpr IdxDof mag_B       {18, 3};
	static constexpr IdxDof wind_vel    {21, 2};
	static constexpr IdxDof terrain     {23, 1};
	static constexpr uint8_t size{24};          // ← 24 个误差状态
};

注意两点：

1. 姿态是四元数，但只有 3 个自由度 （IdxDof{0, 3}）。这是误差状态滤波的正确做法------姿态的不确定性用旋转向量（李代数）表示，而不是直接对四元数的 4 个分量求协方差。v1.15 从"全状态"改成"误差状态"推导，主要收益就在这里，另外数值上也更稳定、计算更省。
2. wind_vel 和 terrain 是编译期可选的 （源码里有 #if defined(CONFIG_EKF2_WIND) / CONFIG_EKF2_TERRAIN）。关掉它们，状态维度会减小。多旋翼通常不需要风估计，固定翼需要。

顺便说，v1.18 又加了一个 EKF2_SENS_EN 位掩码，用来在运行时选择启用哪些融合源，还能通过 MAVLink 的 VEHICLE_CMD_ESTIMATOR_SENSOR_ENABLE 动态开关。这个能力对"某个传感器临时出问题想手动屏蔽"的场景很有用。

源码结构 （src/modules/ekf2/EKF/）：

EKF/
├── ekf.cpp / ekf.h              ← 滤波器主体
├── control.cpp                  ← 主循环与状态机
├── covariance.cpp               ← 协方差传播
├── ekf_helper.cpp               ← 辅助函数（重置、对齐、融合）
├── estimator_interface.cpp/.h   ← 与外部交互的接口
├── position_fusion.cpp          ← 位置融合（GPS/视觉/测距）
├── velocity_fusion.cpp          ← 速度融合
├── yaw_fusion.cpp               ← 偏航融合（磁力计/双天线）
├── height_control.cpp           ← 高度源切换逻辑
├── terrain_control.cpp          ← 地形估计（测距 + 地形图）
├── wind.cpp                     ← 风估计
├── aid_sources/                 ← 【关键】各个传感器融合源
│   ├── baro_height_control.cpp
│   ├── ev_pos_control.cpp       ← 外部视觉位置
│   ├── ev_vel_control.cpp
│   ├── ev_yaw_control.cpp
│   ├── fake_pos_control.cpp
│   ├── gnss_yaw_control.cpp
│   ├── gravity_control.cpp
│   ├── mag_control.cpp
│   ├── optical_flow_control.cpp
│   ├── range_finder_control.cpp
│   ├── sideslip_control.cpp
│   └── zero_innovation_control.cpp
├── bias_estimator/              ← 零偏估计
├── imu_down_sampler/            ← IMU 降采样
├── output_predictor/            ← 输出预测（提高输出频率）
├── yaw_estimator/               ← 偏航估计器
├── documentation/               ← 文档
└── python/                      ← Python 参考实现（教学用！）

python/ 目录是个宝贝。 EKF2 有一套等价的 Python 实现，专门用来教学和验证。你想搞懂卡尔曼滤波的某个细节，读 Python 版比读 C++ 版轻松十倍。

aid_sources/ 目录体现了 EKF2 最漂亮的设计：每个传感器融合源是一个独立的控制器。 它们的基类 AidSource 提供了统一接口，核心思想是：

每个融合源是一个"控制器"，它的目标是把新息（innovation，即测量值与预测值之差） 驱动到零。

这意味着：

• 加一个新的传感器融合逻辑 = 写一个新的 XxxControl 类
• 每个源的融合是独立可测的（python/ 里有对应的测试）
• 融合源可以互相钳制（比如测距和气压计同时可用时，用高度一致性做交叉检查）

融合时序（fusion order） ：EKF2 对传感器融合有严格的顺序，在 control.cpp 里能看到。大致是先做时间对齐（把传感器数据推到统一的"IMU 时间轴"），然后按固定顺序融合。

为什么顺序重要？ 因为卡尔曼滤波里"先融合哪个"会影响结果。PX4 采用的顺序是经过大量实飞验证的，不要随便改。

8.4 EKF2Selector：多实例

从 v1.12 起，PX4 支持同时运行多个 EKF2 实例 （默认 2--3 个），由 EKF2Selector 选出最好的那个作为主输出。

# 在 rcS / 机型里配置
ekf2 start -i 0     # 实例 0
ekf2 start -i 1     # 实例 1
ekf2 start -i 2     # 实例 2

选择器怎么选？ 看 EstimatorSelectorStatus：

• 每个实例报告自己的 healthy、time_slip、filter_fault_flags、solution_status_flags
• 选择器给每个实例打分
• 选分数最高的，但不轻易切换（有滞回，避免来回跳）

pxh> listener estimator_selector_status
# 看哪个实例是 selected，每个的健康状况

实际意义 ：一架装了 3 个 IMU 的飞机，如果某个 IMU 出问题，EKF2 实例 2 可能发散，但实例 0 和 1 还是好的，选择器会切到好的那个。这是 PX4 在硬件冗余上的主要保障机制。

参数 （params_selector.yaml）：EKF2_SEL_ERR_RED（误差阈值）、EKF2_SEL_IMU_ACC（哪个 IMU 喂给选择器）等。

EKF2 常用的几个关键参数（这些你迟早要调）：

参数	作用	什么时候改
EKF2_PREDICT_US	预测周期（微秒）	默认按 IMU 速率，一般不动
EKF2_IMU_CTRL	IMU 辅助数据的使用方式	振动大时可以调
EKF2_GYR_NOISE / EKF2_ACC_NOISE	陀螺/加速度噪声	基本不该动，默认值是基于器件手册的
EKF2_MAG_NOISE	磁力计噪声	磁干扰严重时考虑
EKF2_GPS_CTRL	用 GPS 的哪些信息	多天线、RTK 时设
EKF2_OF_CTRL	光流的使用方式	室内飞行
EKF2_RNG_CTRL	测距的使用方式	室内/降落
EKF2_HGT_REF	高度基准（气压/GPS/测距）	按任务定
EKF2_EV_CTRL	外部视觉的使用方式（VIO/动捕）	装 VIO 必调
EKF2_AID_MASK（老版本）	辅助源总开关	老版本用，新版拆成上面几个

给新手的强烈建议：不要动 EKF2_GYR_NOISE、EKF2_ACC_NOISE。 这两个是滤波器的"信任度"设定，默认值是按 MEMS 器件的实际噪声水平给的。你调它等于骗滤波器，短期可能看起来"稳了"，长期一定出事。

8.5 其他估计器

除了 EKF2，PX4 里还有几个估计器，各有用途：

模块	用途	现状
ekf2	主力估计器（姿态+位置+速度）	生产用
attitude_estimator_q	四元数互补滤波（姿态）	备用/对比
local_position_estimator	老的位置估计（LPE）	已弃用
mc_hover_thrust_estimator	悬停推力估计	多旋翼用
landing_target_estimator	降落目标（视觉标记）	精准降落
vision_target_estimator	视觉目标	新模块
airspeed_selector	空速源选择	固定翼
mag_bias_estimator	磁偏置在线估计
wind_estimator	风估计	固定翼/VTOL
flight_phase_estimation	飞行阶段（起飞/巡航/降落）
gyro_fft	陀螺频谱分析	调 PID 的神器，见下
terrain_estimation	地形（src/lib/）

gyro_fft 值得单独说一下。 它做实时 FFT 分析陀螺数据，找出机体的共振频率，然后 PX4 可以自动设置陷波滤波器（notch filter）。

pxh> gyro_fft status
# 或者看 topic
pxh> listener sensor_gyro_fft

为什么这个重要？ 机架共振是 PX4 上最常见的"抖动/飞不稳"原因。传统做法是靠试错调陷波滤波器的频率，有了 FFT 就变成"读数据、填参数"。如果你的飞机抖，先看这个：

1. logging 记录 sensor_gyro_fft
2. 用 PlotJuggler 或 Flight Review 看频谱
3. 找到峰值频率 → 填 IMU_GYRO_NF0_FRQ / IMU_GYRO_NF1_FRQ
4. 设置 IMU_GYRO_NF0_BW（带宽）

这套流程下来，抖动问题通常能解决 80%。 剩下 20% 是机械问题，得去拧螺丝、换减震垫。

8.6 估计器怎么调、怎么看

第一步：确认数据源是好的。

# 检查传感器健康度
pxh> listener sensors_status_imu
pxh> listener estimator_status      # PX4 里的估计器健康报告

第二步：看 EKF2 的创新（innovation）。 这是判断"滤波器是不是在硬撑"的核心指标。

pxh> listener estimator_innovations

或者更直观的办法------看日志里的 estimator_innovations 和 estimator_status_flags（用 Flight Review 或 PlotJuggler）。

新息应该是什么样的？ 白噪声，均值为 0，幅度在 3σ 以内。如果新息：

• 持续偏一个方向 → 有个传感器的零偏没估计准，或者时间同步有问题
• 幅度超 3σ 且持续 → 该传感器数据不可信
• 剧烈震荡 → 噪声参数设小了（滤波器太"自信"）

第三步：看状态标志。

pxh> listener estimator_status_flags

estimator_status_flags 里的每个 bit 都对应一个具体问题，比如 cs_yaw_align、fs_bad_mag_x、reject_hor_vel 等等。这个 topic 是诊断 EKF 问题的字典，值得花半小时把字段名都看一遍。

第四步：看方差。

pxh> listener estimator_states

里面的 covariances 数组是状态协方差的对角线。协方差持续增长 说明没有足够的观测来约束那个状态（比如没有 GPS 时水平位置协方差会一直涨）。PX4 用这个来决定"当前位置估计是否可信"（vehicle_local_position.xy_valid）。

PX4 官方有一个专门的教程 tuning_the_ecl_ekf.md，标题叫 "Using PX4's Navigation Filter"。如果你真的要把 EKF 搞透，那是必读的。

---

第 9 章 控制链路

这是大家最关心的部分。我按"信号怎么流动"的顺序讲，而不是按模块列表讲------因为前者才是你调试时需要的思维模型。

9.1 总览：从杆量到电机

先看全景。一架多旋翼从你推杆到电机响应，大概经过这些环节：

① 遥控器 ──RF──→ 接收机 ──SBUS/CRSF──→ 飞控
                                          ↓
② rc_input 驱动（或 rc 驱动）
   ↓
   input_rc（原始通道值）
   ↓
③ rc_update 模块
   - 解析通道映射（RC_MAP_*）
   - 计算均值、检查信号质量
   - 生成手动控制输入
   ↓
   manual_control_setpoint（归一化的 -1~1 或 0~1）
   ↓
④ flight_mode_manager（飞行模式管理）
   - 根据 vehicle_status.nav_state 决定当前是什么模式
   - 手动模式：直接透传 manual_control_setpoint
   - 自动模式：用 navigator 给的 setpoint
   ↓
   trajectory_setpoint / 各种 setpoint
   ↓
⑤ mc_pos_control（位置环，~50-100Hz）
   - 输入：位置 setpoint + vehicle_local_position
   - 输出：姿态 setpoint + 推力 setpoint
   ↓
   vehicle_attitude_setpoint（期望姿态四元数 + 归一化推力）
   ↓
⑥ mc_att_control（姿态环，~250Hz）
   - 输入：姿态 setpoint + vehicle_attitude
   - 输出：角速率 setpoint
   ↓
   vehicle_rates_setpoint
   ↓
⑦ mc_rate_control（角速率环，~1kHz，最高优先级）
   - 输入：角速率 setpoint + vehicle_angular_velocity
   - 输出：力矩 setpoint + 推力 setpoint
   ↓
   vehicle_torque_setpoint / vehicle_thrust_setpoint
   ↓
⑧ control_allocator
   - 用 effectiveness matrix 把"力矩+推力"翻译成"每个执行器出多少力"
   ↓
   actuator_motors / actuator_servos
   ↓
⑨ pwm_out / dshot / uavcan 驱动
   ↓
⑩ ESC → 电机

这条链上有三个"换挡点"，理解它们就理解了 PX4 的控制设计哲学：

1. 位置环 → 姿态环：把"我想要什么位置"变成"我需要什么姿态"。这是从笛卡尔空间到姿态空间的转换。
2. 姿态环 → 角速率环：把"我想要什么姿态"变成"我需要多大角速度"。姿态是角速度的积分，分层控制能处理大角度机动。
3. 控制 → 分配：把"需要多少力矩"变成"每个电机多少转速"。这一步是机型无关的关键。

每一层的输出都是下一层的输入，物理量逐级"降维"。 这就是级联控制（cascade control），也是为什么 PX4 的 PID 调参要"从内到外"调------内环不稳，外环调什么都没用。

9.2 遥控输入链路

接收机协议。 PX4 支持的输入协议都有对应的驱动（src/drivers/rc_input/ 和 src/drivers/rc/）：

协议	特点	参数
PPM	老，一根线传所有通道	RC_INPUT_PROTO
SBUS	Futaba，反相串口，16 通道	自动探测
DSM/DSMX	Spektrum
CRSF	TBS，双向遥测	需 RC_CRSF_TEL_EN
GHST	ImmersionRC
SBUS 反相/非反相	常见的坑	RC_INVERT_...
MAVLink RC	通过 MAVLink 转发	for 数传遥控

rc_update 模块做什么：

// src/modules/rc_update/rc_update.cpp
- 读取 input_rc
- 用 RC_MAP_ROLL / RC_MAP_PITCH / RC_MAP_THROTTLE / RC_MAP_YAW 做通道映射
- 用 RC1_MIN / RC1_MAX / RC1_TRIM 做标定（归一化）
- 用 RC1_REV 做反向
- 用 RC1_DZ（死区）和 RC1_EXPO（指数）做输入整形
- 生成 manual_control_setpoint

几个容易踩的坑：

1. RC_MAP_* 错了，飞机一动杆就翻。 装机时一定要在地面站里逐通道确认。
2. 失控保护（RC loss）的检测阈值是 COM_RC_LOSS_T（默认 0.5s）。 太短会误触发，太长飞机会飞很远才发现。
3. manual_control_setpoint 是归一化的。 老版本是 0~1（油门），新版本统一成 -1~1（带 VALID 标志表示油门是否要单独处理）。看代码时注意 manual_control_setpoint_s 的 valid 字段和 sticks_moving。

9.3 多旋翼控制链路（重点）

这是 PX4 最成熟的一条链路，我详细讲。

⑤ mc_pos_control（位置环）

源码目录 src/modules/mc_pos_control/：

mc_pos_control/
├── MulticopterPositionControl.cpp/.hpp   ← 主模块
├── PositionControl/                      ← 位置控制核心
├── GotoControl/                          ← 指点飞行（go-to）
├── Takeoff/                              ← 起飞逻辑
├── multicopter_*_params.yaml             ← 一堆参数文件（按功能分组）
└── MulticopterPositionControlTest.cpp

注意那些参数文件的名字，它们本身就是功能地图：

multicopter_altitude_mode_params.yaml        高度模式
multicopter_autonomous_params.yaml           自动模式
multicopter_nudging_params.yaml              微调（打杆叠加）
multicopter_position_control_gain_params.yaml  增益
multicopter_position_control_limits_params.yaml 限幅
multicopter_position_control_params.yaml     通用
multicopter_position_mode_params.yaml        位置模式
multicopter_responsiveness_params.yaml       响应性
multicopter_stabilized_mode_params.yaml      增稳模式
multicopter_takeoff_land_params.yaml         起降

这个"参数按功能拆分到不同 yaml"的做法是 v1.15 之后的新风格 ，以前所有参数挤在一个文件里。好处是明显了：你想调"响应性"就打开 responsiveness_params.yaml。

mc_pos_control 的核心输入输出：

输入：
  trajectory_setpoint          ← 来自 navigator / flight_mode_manager
  vehicle_local_position       ← 来自 EKF2
  vehicle_attitude             ← 来自 EKF2
  manual_control_setpoint      ← 来自 rc_update

输出：
  vehicle_attitude_setpoint    ← 期望姿态 + 推力
  position_controller_status   ← 状态反馈（给地面站显示）
  trajectory_setpoint（回写）   ← 处理后的实际 setpoint

位置环的关键参数：

参数	含义	默认	调的方式
MPC_XY_P	水平位置 P	0.95	太大振荡，太小迟缓
MPC_XY_VEL_P_ACC	速度环 P	1.8
MPC_XY_VEL_I_ACC	速度环 I	0.4	抗风主要靠这个
MPC_XY_VEL_D_ACC	速度环 D	0.2
MPC_Z_P	垂直位置 P	1.0
MPC_Z_VEL_P_ACC	垂直速度 P	4.0
MPC_Z_VEL_I_ACC	垂直速度 I	2.0
MPC_XY_VEL_MAX	水平最大速度	12	按机型调
MPC_Z_VEL_MAX_UP	最大上升速度	3
MPC_ACC_HOR_MAX	最大水平加速度	5	影响飞的"猛不猛"
MPC_JERK_MAX	最大加加速度	8	影响柔顺度
MPC_TILTMAX_AIR	最大倾角	45°	影响最大水平速度

一个重要理解 ：PX4 的位置环是串级结构 （位置 → 速度 → 加速度），内部用 P 位置 + PID 速度。这也是为什么你看到的参数里有 MPC_XY_P（位置）和 MPC_XY_VEL_P_ACC（速度）两组。

MPC_JERK_MAX（加加速度限制）是 PX4 的一个亮点。 传统 PID 输出的加速度会突变，导致姿态突变。限制 jerk（加速度的变化率）能让飞行更平滑，对挂载相机的多旋翼特别重要。

⑥ mc_att_control（姿态环）

输入：
  vehicle_attitude_setpoint    ← 来自 mc_pos_control
  vehicle_attitude             ← 来自 EKF2
  vehicle_angular_velocity     ← 来自 sensors

输出：
  vehicle_rates_setpoint       ← 期望角速率

姿态环的一个关键技术：四元数控制。 新一代的 AttitudeControl（在 mc_att_control/AttitudeControl/）用的是"四元数误差 → 角速率 setpoint"的方式，比老的欧拉角方式在大角度下更稳。

参数主要在 mc_att_control_params.yaml：

参数	含义
MC_ROLL_P / MC_PITCH_P / MC_YAW_P	姿态 P 增益
MC_YAW_WEIGHT	偏航权重（协调转弯）
MC_ROLLRATE_MAX / MC_PITCHRATE_MAX / MC_YAWRATE_MAX	各轴最大角速率

⑦ mc_rate_control（角速率环）------ 这是最新的变化

这几年多旋翼控制链最大的结构变化，就是角速率环从 mc_att_control 里拆出来，独立成了 mc_rate_control 。它跑在 wq:rate_ctrl 工作队列上，相对优先级 0（换算后是 255，全系统最高，仅次于硬件中断） 。管理上它跟 mc_att_control 是平级的两个模块，rc.mc_apps 里能看到它们各自 start。

拆出来的好处是什么？ 三点：

1. 频率解耦。角速率环跟 IMU 走（250Hz+），姿态环只需要 250Hz，位置环 50--100Hz。三个环的频率需求本来就不一样，硬塞在一个模块里只能按最快的来，浪费算力。
2. 优先级解耦 。角速率环放最高优先级队列，位置环放 nav_and_controllers，互不干扰。
3. 可替换性。你想换掉角速率控制算法（比如换成 LQR、MPC），只需要动这一个模块，不用碰姿态环。

ModuleBase::Descriptor MulticopterRateControl::desc{task_spawn, custom_command, print_usage};

MulticopterRateControl::MulticopterRateControl(bool vtol) :
	ModuleParams(nullptr),
	WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
	_vehicle_thrust_setpoint_pub(vtol ? ORB_ID(vehicle_thrust_setpoint_virtual_mc)
	                                  : ORB_ID(vehicle_thrust_setpoint)),
	_vehicle_torque_setpoint_pub(vtol ? ORB_ID(vehicle_torque_setpoint_virtual_mc)
	                                  : ORB_ID(vehicle_torque_setpoint)),
	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME ": cycle"))
{
	_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
	parameters_updated();
	_controller_status_pub.advertise();
}

bool MulticopterRateControl::init()
{
	if (!_vehicle_angular_velocity_sub.registerCallback()) {   // ← 数据驱动
		PX4_ERR("callback registration failed");
		return false;
	}
	return true;
}

注意 vtol 那个分支 ：VTOL 机型用的是 vehicle_thrust_setpoint_virtual_mc / vehicle_torque_setpoint_virtual_mc 这两个虚拟 topic。原因是有多个控制器（多旋翼的、固定翼的）同时发 setpoint，得有个中间层把它们的输出按当前飞行阶段混合起来。这是 VTOL 特有的复杂度，第 9.5 节讲。

Run() 里的处理：

void MulticopterRateControl::Run()
{
	if (should_exit()) { ... }

	perf_begin(_loop_perf);

	// 参数更新检查
	if (_parameter_update_sub.updated()) {
		parameter_update_s param_update;
		_parameter_update_sub.copy(&param_update);
		updateParams();
		parameters_updated();
	}

	// 用角速度驱动
	vehicle_angular_velocity_s angular_velocity;
	if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {

		const hrt_abstime now = angular_velocity.timestamp_sample;
		// Guard against too small (< 0.125ms) and too large (> 20ms) dt's.
		const float dt = math::constrain(((now - _last_run) * 1e-6f), 0.000125f, 0.02f);
		_last_run = now;

		const Vector3f rates{angular_velocity.xyz};
		const Vector3f angular_accel{angular_velocity.xyz_derivative};
		...
	}

	perf_end(_loop_perf);
}

那段 dt 限幅的注释一定要看懂：

const float dt = math::constrain(((now - _last_run) * 1e-6f), 0.000125f, 0.02f);

• 下限 0.000125f = 125μs = 8kHz。防止 dt 太小导致微分项爆炸。
• 上限 0.02f = 20ms = 50Hz。防止 dt 太大导致积分项跳跃、微分项归零。

这个"dt 限幅"是所有数字控制器的基本卫生习惯。 没有它，一次调度抖动就能让你的 D 项输出一个巨大的尖峰，直接导致飞机抽一下。你在自己写控制器时必须照抄。

参数变化（注意跟姿态环的区别）：

const Vector3f rate_k = Vector3f(_param_mc_rollrate_k.get(),
                                 _param_mc_pitchrate_k.get(),
                                 _param_mc_yawrate_k.get());

_rate_control.setPidGains(
	rate_k.emult(Vector3f(_param_mc_rollrate_p.get(), ...)),   // K * P
	rate_k.emult(Vector3f(_param_mc_rollrate_i.get(), ...)),   // K * I
	rate_k.emult(Vector3f(_param_mc_rollrate_d.get(), ...)));  // K * D

MC_ROLLRATE_K 等参数的含义：它是"整体增益"，和 P/I/D 相乘后才是实际增益。所以：

• 想让整体响应快一点 → 只调 K
• 想改 P/I/D 的相对比例 → 调 _P / _I / _D

这个设计让你不用在调 K 的时候重新配平 P/I/D 的比例。

GainCompression（增益压缩） 也是新东西：

_gain_compression.setEnabled(_param_mc_gc_en.get());
_gain_compression.setCompressionGainMin(_param_mc_gc_gain_min.get());

它检测控制器的输出是否在饱和（饱和时高增益会导致振荡甚至发散），然后自动降低增益。这是自适应控制的一个简化版本。

⑧ 角加速度前馈 ：angular_velocity.xyz_derivative 是角加速度，被用作前馈（feed-forward）。有前馈之后，控制的响应会明显更快------因为它不再需要"等误差出现再反应"。

9.4 固定翼控制链路

固定翼的链路比多旋翼复杂，因为它多了"气动"这个非线性环节。

新版结构（v1.16+）：

navigator / flight_mode_manager
   ↓
trajectory_setpoint
   ↓
fw_lateral_longitudinal_control  ← 制导层（横向 + 纵向）
   - 横向：航向/侧偏 → roll setpoint 或 航向 setpoint
   - 纵向：高度/速度 → pitch setpoint + 油门 setpoint
   ↓
fixed_wing_lateral_setpoint / fixed_wing_longitudinal_setpoint
   ↓
fw_att_control（姿态环 + 协调转弯）
   ↓
vehicle_rates_setpoint
   ↓
fw_rate_control（角速率环）
   ↓
vehicle_torque_setpoint / vehicle_thrust_setpoint
   ↓
control_allocator → 舵机 + 电机

老版本 是 fw_pos_control_l1（横向用 L1 制导）+ fw_att_control（含纵向 TECS）。新版本拆开了：

• fw_lateral_longitudinal_control（src/modules/fw_lateral_longitudinal_control/）：横向制导 + 纵向制导
• fw_att_control（src/modules/fw_att_control/）：姿态控制 + 协调转弯 + 前轮控制
• fw_rate_control（src/modules/fw_rate_control/）：角速率环

关键概念：TECS（Total Energy Control System）。 这是固定翼纵向控制的核心算法，在 src/lib/tecs/。核心思想是：总能量 = 动能 + 势能，用油门控制总能量、用俯仰控制能量的分配（动能↔势能）。这样高度和速度就不会互相干扰。

TECS 的参数：

TECS_CLMB_MAX         最大爬升率
TECS_SINK_MIN         最小下降率
TECS_TIME_CONST       时间常数
TECS_THR_DAMP         油门阻尼
TECS_INTEG_GAIN       积分增益
TECS_VERT_ACC         垂直加速度限制
TECS_SPDWEIGHT         速度权重（0=纯高度，2=纯速度）
TECS_PITCH_MAX/MIN    俯仰限制

TECS_SPDWEIGHT 是个关键的"性格参数"：它决定飞机在扰动时优先保高度还是保速度。默认 1.0（均衡）。

FW_* 里的 FW_USE_AIRSPD 值得单独说。看它 yaml 里的描述：

FW_USE_AIRSPD:
  description:
    short: Use airspeed for control
    long: |-
      If set to 1, the airspeed measurement data, if valid, is used in the following controllers:
      - Rate controller: output scaling
      - Attitude controller: coordinated turn controller
      - Position controller: airspeed setpoint tracking, takeoff logic
      - VTOL: transition logic

空速是固定翼的命根子。 没有空速管（或者空速管堵了/结冰了），固定翼的失速保护、转弯协调、油门控制全都退化。这个参数的描述把空速用在哪四个地方说得清清楚楚，值得一读。

固定翼的角速率环参数 （fw_rate_control_params.yaml）：

FW_PR_P:   0.08   # 俯仰角速率 P
FW_PR_I:   0.1    # 俯仰角速率 I
FW_PR_D:   0.0    # 俯仰角速率 D
FW_PR_IMAX: 0.4   # 俯仰积分限幅
FW_RR_P:   0.05   # 滚转角速率 P
...

注意单位是 %/rad/s，也就是"百分之一的舵量每弧度每秒"。这个单位本身就说明了固定翼的角速率环输出是舵面偏转百分比。

9.5 VTOL

VTOL 是固定翼 + 多旋翼的叠加，复杂度来自"什么时候用哪套控制器"。

vtol_att_control（src/modules/vtol_att_control/）
  - 管理 VTOL 状态机：多旋翼模式 ↔ 过渡 ↔ 固定翼模式
  - 输出"控制模式"给控制分配器
   ↓
  两个控制器同时跑，输出到"虚拟"setpoint：
    mc_* → vehicle_torque_setpoint_virtual_mc / vehicle_thrust_setpoint_virtual_mc
    fw_* → vehicle_torque_setpoint_virtual_fw  / vehicle_thrust_setpoint_virtual_fw
   ↓
  vtol_att_control 按当前阶段混合两者
   ↓
  vehicle_torque_setpoint / vehicle_thrust_setpoint（真实）
   ↓
  control_allocator

为什么要"虚拟 setpoint"？ 因为过渡阶段需要两个控制器同时工作、按比例混合。比如从多旋翼过渡到固定翼时，前面几秒还在用多旋翼的升力，同时固定翼的舵面开始生效。虚拟 setpoint 就是这个混合的接口。

pxh> listener vtol_vehicle_status
# 看当前是 MC / transition / FW，过渡进度是多少

VTOL 的参数前缀是 VT_*：

VT_TYPE              VTOL 类型（标准/尾座式/倾转旋翼/倾转涵道）
VT_FW_PERM_STAB      固定翼模式下的增稳
VT_F_TRANS_DUR       前过渡时长
VT_B_TRANS_DUR       后过渡时长
VT_ARSP_TRANS        过渡空速阈值
VT_FW_MIN_ALT        固定翼模式最低高度
VT_FW_QC_P / VT_FW_QC_R  固定翼模式下的多旋翼姿态/角速率增益

VT_ARSP_TRANS（过渡空速）是最关键的参数：达到这个空速才认为固定翼能靠机翼维持升力。设太小 → 过渡时掉高；设太大 → 过渡时间过长。

9.6 Control Allocator 与控制分配

这是 PX4 架构里最漂亮的一块。

问题： 控制器输出的是"我需要在机体系上产生 τx, τy, τz, T"------三个力矩和推力。但飞机实际有的是"N 个电机 + M 个舵面"。怎么从 4 个量映射到 N+M 个执行器？

答案：effectiveness matrix（效能矩阵）。 定义如下：

[τx]     [ e1x e2x ... eNx ]   [u1]
[τy]  =  [ e1y e2y ... eNy ] · [u2]
[τz]     [ e1z e2z ... eNz ]   [...]
[T ]     [ e1T e2T ... eNT ]   [uN]

eij 表示"第 j 个执行器出单位力时，在第 i 个方向上产生多少效果"。也就是每个执行器对每个控制轴的效能。

这个矩阵就是机型的"基因"。 一个四旋翼的矩阵、一个六旋翼的矩阵、一个带副翼升降舵的固定翼的矩阵，完全是不同的东西。控制分配算法（求这个方程的解）与机型无关。

求解方法有两种 （src/lib/control_allocation/）：

ControlAllocationPseudoInverse     // 伪逆法
ControlAllocationSequentialDesaturation  // 序贯解饱和法（默认）

伪逆法 ：直接求矩阵的伪逆，一步解出来。数学上"最小二乘最优"，但不考虑执行器饱和------如果解出来的某个电机转速超过上限，就直接截断，会破坏控制效果。

序贯解饱和法 ：迭代求解。某一轴达到饱和时，把它固定，用剩余的执行器去满足其余的控制需求。结果是次优的，但可执行。 对一个真实飞行器来说，"能执行的次优解"远胜于"不能执行的最优解"。

这就是为什么 PX4 默认用序贯解饱和。 这是工程上的正确选择。

ActuatorEffectiveness 怎么定义？ 看 src/modules/control_allocator/VehicleActuatorEffectiveness/：

VehicleActuatorEffectiveness/
├── ActuatorEffectivenessMultirotor.cpp     多旋翼（4/6/8 旋翼几何）
├── ActuatorEffectivenessFixedWing.cpp      固定翼（副翼/升降舵/方向舵/油门）
├── ActuatorEffectivenessVTOL.cpp           VTOL（混合）
├── ActuatorEffectivenessHelicopter.cpp     直升机（斜盘）
├── ActuatorEffectivenessRover*.cpp          无人车
├── ActuatorEffectivenessTiltrotor.cpp      倾转旋翼
├── ActuatorEffectivenessUUV.cpp            水下
├── ActuatorEffectivenessSpacecraft.cpp     航天
└── ActuatorEffectivenessCustom.cpp         自定义

每个文件就是一个机型族的效能矩阵定义。

分配器的相关参数 （CA_*）：

CA_AIRFRAME          机型类型（自动从 airframe 设置）
CA_ROTOR0_PX/PY/PZ   第 0 个桨的位置（相对重心）
CA_ROTOR0_KM         第 0 个桨的反扭矩系数
CA_ROTOR_COUNT       桨的数量
CA_SV0_FUNC          第 0 个舵面的功能
...

看 CA_ROTOR0_PX/PY/PZ 没有？ 这些参数描述的是每个电机在机体上的物理位置 。分配器用这些位置和 KM（反扭矩系数）自动构建效能矩阵。

这意味着：改装一架飞机的电机布局，你不需要改代码，只要改这些参数。 这是 PX4 相对其他飞控的一个很大优势。

pxh> listener control_allocator_status

这个 topic 会告诉你每个执行器的分配结果、是否饱和、以及矩阵条件数。

9.7 频率与延迟表

这张表我按源码里的实际值整理，是判断"RT 是否够快"的依据：

环节	频率	触发方式	参考来源
IMU 采样	1 kHz（可配 8kHz 采样 + 8kHz 滤波）	硬件中断	驱动 ODR 配置
传感器发布（sensor_*）	250 Hz	驱动 → 工作队列	Integrator 降采样
vehicle_angular_velocity	250 Hz	sensors 模块
vehicle_attitude（EKF 输出）	250 Hz（可配）	EKF2 output predictor	EKF2_PREDICT_US
角速率环 mc_rate_control	跟随 vehicle_angular_velocity（250 Hz+）	uORB 回调	registerCallback()
姿态环 mc_att_control	250 Hz	uORB 回调
位置环 mc_pos_control	50--100 Hz	uORB 回调
control_allocator	跟随 setpoint 更新	uORB 回调
navigator	10 Hz	独立任务	SCHED_PRIORITY_NAVIGATION
commander	事件驱动 + 低频轮询	独立任务
logger	按 topic 配置	独立任务
MAVLink	按 stream 速率	独立任务

关于"为什么角速率环是 250Hz 而不是 1kHz" ：因为 vehicle_angular_velocity 就是 250Hz 发布的。虽然 IMU 底层是 1kHz 采样，但积分降采样后是 250Hz。要真跑 1kHz 角速率环，得改 IMU 的发布配置。

实际上，250Hz 对多旋翼角速率环是够的 （4ms 周期）。真正需要 1kHz 的是那些大型或者超轻的机型。PX4 提供的 MC_ROLLRATE_... 参数和支持更高 ODR 的 IMU 驱动（如 ICM42688P 支持 8kHz）留了这个口子。

延迟预算（粗略，H7 平台）：

环节	延迟
IMU 采样 → 驱动发布	~0.5 ms
驱动 → sensors 整合 → vehicle_angular_velocity	~1 ms
EKF 预测 → vehicle_attitude	~1 ms
控制链（率→姿态→位置）	~4--8 ms
控制 → 分配 → PWM 输出	~0.5 ms
PWM 帧 → ESC 响应	~1--4 ms
合计（不含执行器机械延迟）	~8--15 ms

这个数字决定了你最多能飞多"猛"。 如果你的飞机悬停时抖，而 PID 又怎么调都不行，想想这条链上的延迟------4ms 的采样周期加上 10ms 的总延迟，意味着你的相位裕度是有限的。

9.8 这条链上最容易出的问题

我把这些年在控制链上踩过的坑列一遍，你可以当检查清单：

1. "飞机往一个方向慢慢飘"。

位置环积分不够（MPC_XY_VEL_I_ACC 太小）或者 EKF 有常值偏差。先确认 EKF 的新息没有持续偏置，再考虑调积分。

2. "飞机高频抖动"。

三个可能：振动传到 IMU（先看 clip_count 和 sensor_gyro_fft）、角速率环 D 项太大、或者电机/桨的动力不平衡。排查顺序必须是从机械到电子到参数，反过来会浪费时间。

3. "推杆后飞机反应迟钝但最后能到位"。

整体增益偏小。多旋翼调 MC_ROLLRATE_K / MC_PITCHRATE_K，或者 MPC_XY_VEL_P_ACC。

4. "悬停时高度上下浮动"。

垂直位置环和高度估计耦合。检查 MPC_Z_VEL_P_ACC、MPC_Z_P，以及 EKF 的气压计是否有气流干扰（机架风扰动）。

5. "急停后飞机过冲震荡"。

MPC_JERK_MAX 设太大，或者速度环 D 项过大。jerk 限制就是为这个场景设计的。

6. "转换模式时飞机掉高"。

VTOL 的 VT_ARSP_TRANS 设置不合理；或者固定翼模式的最低高度保护不够。

7. "电机响应不对称"。

检查 control_allocator_status 里的饱和度，确认 CA_ROTOR*_PX/PY/PZ 的位置配置跟实际机架一致。这是装机时最容易漏的一步。

统一建议：所有这些问题，都先看日志再动参数。 PX4 的 logger 默认就记录了控制链上所有 topic。用 PlotJuggler 把 setpoint 和实际值画在一起，你能直接看到"哪里跟不上"。

---

第 10 章 决策层：commander 与 navigator

控制链解决"怎么飞"，决策层解决"飞去哪、能不能飞、什么时候不飞"。

10.1 commander：整个系统的守门员

commander 是 PX4 里权限最大的模块。它做四件核心的事：

1. 解锁 / 上锁（Arming）
2. 健康检查（Health and Arming Checks）
3. 飞行模式管理（Mode Management）
4. 安全逻辑（Safety：失控保护、kill switch、地理位置熔断）

目录结构就反映了这四件事：

src/modules/commander/
├── Commander.cpp/.hpp              主模块
├── Arming/                         解锁逻辑
│   └── ArmAuthorization/           外部授权（如从地面站请求解锁）
├── HealthAndArmingChecks/          健康检查
│   ├── HealthAndArmingChecks.cpp
│   └── checks/                     各个检查项
│       ├── batteryCheck.cpp
│       ├── escCheck.cpp
│       ├── estimatorCheck.cpp
│       ├── gnssRedundancy.cpp
│       ├── imuCheck.cpp
│       ├── manualControlCheck.cpp
│       ├── modeCheck.cpp
│       ├── powerCheck.cpp
│       ├── rcCalibrationCheck.cpp
│       ├── sensorsCheck.cpp
│       ├── homePositionCheck.cpp
│       ├── trafficAvoidanceCheck.cpp
│       ├── airspeedCheck.cpp
│       ├── externalChecks.cpp      ← 给外部组件用的接口
│       └── ...
├── ModeManagement.cpp/.hpp         模式管理
├── ModeUtil/                       模式工具
│   └── setpoint_types.hpp
├── UserModeIntention.cpp/.hpp      用户"想做什么"
├── Safety.cpp/.hpp                 安全逻辑
├── HomePosition.cpp/.hpp           Home 点管理
├── MulticopterThrowLaunch/         抛飞检测（多旋翼）
├── calibrations...                 各种校准
└── HealthAndArmingChecks/checks/   健康检查项

解锁状态机。 VehicleStatus.msg 里定义得很简单：

uint8 ARMING_STATE_DISARMED = 1
uint8 ARMING_STATE_ARMED    = 2

就两个状态。但进入和退出这两个状态的条件很复杂 ，全在 HealthAndArmingChecks/ 里。

健康检查机制。 每次解锁请求，commander 会跑一遍所有 checks/ 下的检查项。每个检查项返回：

class EstimatorChecks : public HealthAndArmingCheckBase
{
public:
	void checkAndReport(const Context &context, Report &reporter) override
	{
		// 检查各种条件
		if (条件不满足) {
			reporter.armingCheckFailure(模式组, 事件ID, "错误描述");
		}
	}
};

输出分三个等级：

等级	后果
OK	没问题
WARNING	能解锁，但会提示（可以通过 COM_ARM_... 相关参数忽略）
ERROR	不能解锁

pxh> commander check

这个命令必须在装机后、每次首飞前跑。 它会列出所有不通过的检查项和原因。这比反复试解锁然后看 QGC 弹窗快得多。

HealthReport topic 是这些检查结果的结构化输出：

pxh> listener health_report

解锁条件里几个关键的参数：

参数	含义
COM_ARM_CHK_ESCS	是否检查 ESC 状态
COM_ARM_WO_GPS	允许无 GPS 解锁（室内/光流）
COM_ARM_MAG_STR	磁力计强度检查的松紧
COM_ARM_IMU_ACC / COM_ARM_IMU_GYR	IMU 一致性阈值
COM_ARM_EKF_HGT / COM_ARM_EKF_VEL / COM_ARM_EKF_POS / COM_ARM_EKF_YAW	EKF 各状态的置信度阈值
COM_ARM_ARSP_EN	是否检查空速（固定翼）
COM_ARM_AUTH_REQ	是否需要外部授权才能解锁

COM_ARM_AUTH_REQ 是个好用的功能：开启后，解锁请求会先发到外部（比如地面站或者自定义的授权模块），批准之后才能真正解锁。做系留测试、多人协作场景很有用。

模式管理（Mode Management）。 这里有一个容易混淆的概念需要理清：

• nav_state（导航状态）：由 navigator 或者手动模式决定，是"飞机当前在干什么"。
• UserModeIntention（用户意图）：用户通过遥控器开关表达的"我想进什么模式"。
• vehicle_control_mode：控制层需要的标志位（是否位置控制、是否高度控制、是否解锁...）。

三者之间的关系是 commander 在维护的。当遥控器拨到某个档位（RC_MAP_FLTMODE 映射的通道），commander 把开关位置翻译成 UserModeIntention，然后校验能不能进这个模式（比如空中不能直接切到 Manual），最后设置 nav_state。

飞行模式一览 （VehicleStatus.msg 里的 NAVIGATION_STATE_*，共 31 个）：

NAVIGATION_STATE_MANUAL           = 0    手动
NAVIGATION_STATE_ALTCTL           = 1    定高
NAVIGATION_STATE_POSCTL           = 2    定点
NAVIGATION_STATE_AUTO_MISSION     = 3    自动任务
NAVIGATION_STATE_AUTO_LOITER      = 4    自动悬停
NAVIGATION_STATE_AUTO_RTL         = 5    自动返航
NAVIGATION_STATE_POSITION_SLOW    = 6    慢速定点
NAVIGATION_STATE_GUIDED_COURSE    = 7    引导航向（固定翼）
NAVIGATION_STATE_ALTITUDE_CRUISE  = 8    定高巡航
NAVIGATION_STATE_MANUAL_PARKING   = 9    手动泊车（无人车）
NAVIGATION_STATE_ACRO             = 10   Acro（纯角速率）
NAVIGATION_STATE_DESCEND          = 12   下降（无位置控制）
NAVIGATION_STATE_TERMINATION      = 13   终止（直接停桨/炸机保护）
NAVIGATION_STATE_OFFBOARD         = 14   Offboard（外部控制）
NAVIGATION_STATE_STAB             = 15   增稳
NAVIGATION_STATE_AUTO_TAKEOFF     = 17   自动起飞
NAVIGATION_STATE_AUTO_LAND        = 18   自动降落
NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19 跟随目标
NAVIGATION_STATE_AUTO_PRECLAND    = 20   精准降落
NAVIGATION_STATE_ORBIT            = 21   环绕
NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22  VTOL 自动起飞
NAVIGATION_STATE_EXTERNAL1..8     = 23-30 外部模式
NAVIGATION_STATE_MAX              = 31

几个值得注意的：

• TERMINATION（13） ：这是"最后一根救命稻草"。触发后直接切断动力（或者进入预设的终止动作）。参数 COM_FAIL_ACT_T 决定失控时是否使用。不是所有机型都启用这个模式 ------多旋翼切了就是自由落体，固定翼切了可能滑翔一段。用之前想清楚。 注意：v1.18 把这一套接口重命名了。commander lockdown on 现在是 commander termination，failsafe 动作 FORCE_FAILSAFE 现在是 TERMINATION，manual_lockdown 现在是 kill。如果你看到老教程里写 commander lockdown，那是 v1.17 及之前的叫法。
• EXTERNAL1..8（23--30） ：给外部组件（ROS 2 节点、自定义模块）注册的模式。这就是 ModeManagement.hpp 里 ModeExecutors 那套东西的用途：

class ModeExecutors
{
public:
	static constexpr int AUTOPILOT_EXECUTOR_ID = 0;   // 0 是 PX4 自己的
	static constexpr int FIRST_EXECUTOR_ID = 1;        // 外部从 1 开始
	static constexpr int MAX_NUM = 5;                  // 最多 5 个外部执行器
	...
};

外部组件通过发 register_ext_component_request / setpoint_config 这些 topic 来申请一个模式 ID。这是 PX4 支持 ROS 2 自定义飞行模式的底层机制。

Safety：失控保护。 除了健康检查，commander 还负责运行时安全：

保护类型	触发条件	参数	默认动作
遥控失控	RC 信号丢失	COM_RC_LOSS_T	悬停/返航/降落
遥控数据链路丢失	长时间没有 RC 更新	COM_RC_STICK_OV（摇杆超时）	同上
数据链丢失	MAVLink 没有地面站	COM_DL_LOSS_T	悬停/返航
电池低	电压/电量低于阈值	COM_LOW_BAT_ACT、BAT_LOW_THR	告警/返航/降落
地理围栏	超出 GF_* 定义的围栏	GF_ACTION	告警/悬停/返航/降落
位置失效	EKF 位置估计失效	自动	切换到定高/手动
电机故障	ESC 报告故障	自动	告警
空速失效	空速管故障（固定翼）	FW_...	切换到无空速模式

失控保护的动作由 COM_FAIL_ACT_* 参数族决定，不同机型的默认值不同。这是装机时必须逐项确认的。

一个好的实践 ：在飞之前，用 QGC 或者直接改 RC_MAP_KILL_SW 配置一个 kill switch，然后在地面测试它的行为。真出问题时，能在半秒内停桨，比什么都强。

10.2 navigator：飞哪、怎么飞

navigator 负责所有的"自主飞行"逻辑。它是个状态机，每个状态对应一种任务。

它的源码结构就是状态机的展开：

src/modules/navigator/
├── navigator_main.cpp            主循环（独立任务）
├── navigator_mode.cpp/.h         所有模式的基类
├── mission.cpp/.h                航线任务
├── mission_base.cpp/.h           任务基类
├── mission_block.cpp/.h          任务块（航点类型分发）
├── mission_feasibility_checker.cpp  任务可行性检查
├── mission_route_*.cpp           【新】航线规划相关（一堆）
├── full_mission_cache.cpp/.h     全任务缓存
├── loiter.cpp/.h                 悬停/盘旋
├── rtl.cpp/.h                    返航
├── rtl_direct.cpp/.h             直接返航
├── rtl_direct_mission_land.cpp/.h
├── rtl_mission_fast.cpp/.h       快速返航（走航线）
├── rtl_mission_fast_reverse.cpp/.h 反向快速返航
├── rtl_geofence_avoidance_helper.cpp  返航时避障
├── RTLPlanner/                   【新】返航路径规划器
├── land.cpp/.h                   降落
├── takeoff.cpp/.h                起飞
├── vtol_takeoff.cpp/.h           VTOL 起飞
├── precland.cpp/.h               精准降落
├── prec_takeoff.cpp/.h
├── course.cpp/.h                 航向控制（固定翼）
├── geofence.cpp/.h               地理围栏
├── geofence_*.yaml
├── DetectAndAvoid/               避障
├── MissionFeasibility/           任务可行性
└── mission_*.cpp                 一堆任务相关

注意 rtl_*.cpp 有 6 个文件。 这说明"返航"在 PX4 里不是一个简单的逻辑------它要根据当前状态、剩余电量、地理围栏、有没有障碍，选一条最合适的路线。RTLPlanner/ 是这套逻辑的新实现。

navigator 的主循环： 它以 10Hz 左右的速度跑（独立任务，优先级 25），每次迭代：

1. 读当前模式（nav_state）
2. 分发到对应的 NavigatorMode::run()
3. 该模式输出 setpoint（position_setpoint_triplet / trajectory_setpoint）

// 伪代码
void Navigator::run()
{
	// 1. 检查模式切换
	if (_navigation_mode->getID() != new_mode) {
		_navigation_mode->on_inactive();
		_navigation_mode = get_mode(new_mode);
		_navigation_mode->on_activation();
	}

	// 2. 跑当前模式的逻辑
	_navigation_mode->run(true);

	// 3. 发布 setpoint
	position_setpoint_triplet_s triplet{};
	_navigation_mode->get_setpoint(triplet);
	_pos_sp_triplet_pub.publish(triplet);
}

任务（Mission）是 navigator 里最复杂的一块。 一个任务是一串航点，每个航点有类型：

航点类型	含义	参数相关
NAV_WAYPOINT	普通航点	接受半径 NAV_ACC_RAD
NAV_LOITER_UNLIM	无限盘旋	半径、方向
NAV_LOITER_TIME	定时盘旋	时间
NAV_LOITER_TURNS	定圈数盘旋	圈数
NAV_TAKEOFF	起飞	高度、俯仰
NAV_LAND	降落	精确/普通
NAV_RETURN_TO_LAUNCH	返航
NAV_DELAY	延时	时间
NAV_VTOL_TAKEOFF	VTOL 起飞
NAV_VTOL_LAND	VTOL 降落
NAV_DO_JUMP	跳转到某航点	次数
NAV_DO_CHANGE_SPEED	改速度
NAV_DO_SET_HOME	设置 Home
NAV_DO_SET_ROI	设置关注点
NAV_DO_LAND_START	降落起始点
NAV_GOTO_...	go-to 变体

任务存在 dataman （Flash 上的键值存储）里，上电后加载到内存。任务上传走 MAVLink 的 MISSION_ITEM_INT 协议，由 mavlink_mission.cpp 处理。

地理围栏（Geofence）。 定义在 geofence.cpp + geofence_params.yaml。围栏有几种：

• 圆形围栏 （GF_MAX_HOR_DIST、GF_MAX_VER_DIST）
• 多边形围栏（上传的顶点列表）
• 高度围栏 （GF_MAX_VER_DIST）
• 禁飞区（inclusion/exclusion）

pxh> listener geofence_result     # 看是否在围栏内
pxh> listener geofence_status

GF_ACTION 决定越界后的动作：无动作 / 告警 / 悬停 / 返航 / 降落 / 终止。

land_detector：着陆检测。 这个小模块很重要，因为"什么时候算落地了"决定了能不能上锁、能不能切模式。

src/modules/land_detector/
├── LandDetector.cpp/.h              基类（状态机：GROUND_CONTACT → MAYBE_LANDED → LANDED）
├── MulticopterLandDetector.cpp      多旋翼版本
├── FixedwingLandDetector.cpp        固定翼版本
├── VtolLandDetector.cpp             VTOL 版本
├── RoverLandDetector.cpp            无人车版本
├── AirshipLandDetector.cpp          飞艇版本
└── land_detector_params*.yaml

多旋翼的着陆判据 （MulticopterLandDetector）：

• 垂直速度接近 0
• 高度变化小
• 加速度小（无振动）
• 电机输出低于阈值（LNDMC_THR_RANGE）
• 持续满足以上条件一段时间（LNDMC_TRIG_TIME）

常见的坑 ：如果动力很足、悬停油门很低，LNDMC_THR_RANGE 可能把"低空悬停"误判成"着陆"，导致空中自动上锁（然后就掉下来了）。这是个会摔飞机的参数，改的时候小心。

10.3 flight_mode_manager（新模块）

这是 main 上新增的东西，rc.mc_apps 里能看到它：

mc_hover_thrust_estimator start
flight_mode_manager start
mc_pos_control start

它解决什么问题？ 在老架构里，"哪个模式该用哪些 setpoint、手动输入怎么叠加到自动 setpoint 上"这套逻辑是散落在各个控制器和 commander 里的。新架构把它抽出来，变成一个独立的模块。

flight_mode_manager/
├── FlightModeManager.cpp/.hpp
├── Templates/                      ← 模式模板
├── tasks/                          ← 各种飞行任务
└── generate_flight_tasks.py        ← 代码生成

注意 generate_flight_tasks.py。 这是个代码生成器------模式任务是通过模板生成的。这算是 PX4 在减少样板代码上的一次尝试。

对你现在的影响不大，但你要知道这个模块存在，因为它在 setpoint 链路的中间：

navigator (自动模式的 setpoint)
manual_control_setpoint (手动输入)
        ↓
flight_mode_manager          ← 决定"听谁的"、怎么混合
        ↓
trajectory_setpoint / 各模式 setpoint
        ↓
mc_pos_control / fw_*_control

调试"手动模式下打杆没反应"这类问题时，flight_mode_manager 是必查的一站。

---

第 11 章 通信：和外面世界的接口

11.1 MAVLink：绝对主力

MAVLink 是 PX4 与地面站、伴飞计算机、其他飞控通信的标准协议。它是二进制、轻量、带校验的消息协议。

MAVLink 模块在哪：

src/modules/mavlink/
├── mavlink_main.cpp/.h         模块主体
├── mavlink_messages.cpp        【核心】uORB topic ↔ MAVLink 消息的映射
├── mavlink_command_sender.cpp  发送命令
├── mavlink_command_params.hpp
├── mavlink_ftp.cpp/.h          文件传输（FTP，用于传日志、任务）
├── mavlink_ftp_param.cpp/.h    参数传输
├── mavlink_mission.cpp/.h      任务协议
├── mavlink_log_handler.cpp     日志传输
├── mavlink_events.cpp/.h       事件转发
├── mavlink_statustext_handler.*
├── mavlink_tunnel?
├── mavlink_bridge_header.h
├── mavlink.c
└── mavlink/                     ← 从 mavlink 子模块生成的代码

一个关键认知：MAVLink 模块可以有多个实例。

pxh> mavlink status
instance #0:
	mavlink chan: #0
	type: GENERIC LINK OR RADIO
	flow control: OFF
	rates:
	  tx: 5.286 kB/s
	  txerr: 0.000 kB/s
	  tx rate mult: 0.800
	  tx rate max: 1000 B/s
	  rx: 0.000 kB/s
	...
	transport protocol: UDP (14550)

每个串口/UDP 通道一个实例：

# 典型启动命令
mavlink start -d /dev/ttyS6 -b 57600           # 遥测口
mavlink start -d /dev/ttyS0 -m onboard -r 4000000  # 板载计算机，高速
mavlink start -u 14550 -r 1000000              # 本地 UDP

参数控制启动行为 （rc.board_defaults 或机型文件里设）：

参数	含义
MAV_0_CONFIG / MAV_1_CONFIG / MAV_2_CONFIG	每个实例对应哪个串口（或 0 = 禁用）
MAV_0_MODE	实例类型（Normal / Onboard / Gimbal / OSD / ...）
MAV_0_RATE	波特率
MAV_0_FORWARD	是否转发到其他实例
MAV_SYS_ID	系统 ID（多机同空域时必须各不相同）
MAV_COMP_ID	组件 ID
MAV_PROTO_VER	MAVLink 版本（v1/v2）
MAV_HASH_CHK_EN	消息签名校验
MAV_TYPE	载具类型（四旋翼/固定翼...）

MAV_x_MODE 的几种模式，这个选择很关键：

模式	用途	默认 stream
Normal	正常遥测（给 QGC）	标准
Onboard	给板载计算机（MAVSDK/ROS）	更高速
OSD	给 FPV OSD	精简
Gimbal	给云台	云台协议
Camera	给相机
Config	只用于配置
Minimal	最小
OnboardLowBandwidth	低带宽板载（LTE）
uAvionix	ADS-B 相关
IridiumSBD	卫星通信

stream rate 配置。 MAVLink 消息发多少、发哪些，靠的是 MAV_x_... 或者更灵活的 mavlink 命令：

pxh> mavlink stream -u 14550 -s ATTITUDE -r 50

但现代做法是自动协商。 PX4 支持 MAV_PROTO_VER=2 时通过 MAV_CMD_SET_MESSAGE_INTERVAL 由地面站动态设置每个消息的频率。QGC 就是这么干的。

mavlink_messages.cpp 是你改 MAVLink 桥接的地方。 所有 uORB topic → MAVLink 消息的转换都在这。比如经典的：

class MavlinkStreamAttitude : public MavlinkStream
{
public:
	const char *get_name() const override { return MavlinkStreamAttitude::get_name_static(); }
	...
	bool send() override
	{
		vehicle_attitude_s att;
		if (_att_sub.update(&att)) {
			mavlink_attitude_t msg{};
			msg.time_boot_ms = att.timestamp / 1000;
			quaternion_to_euler(att.q, &msg.roll, &msg.pitch, &msg.yaw);
			...
			_mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE, &msg);
			return true;
		}
		return false;
	}
};

加自定义 MAVLink 消息的完整流程：

1. 在 mavlink/mavlink 子模块的 message_definitions/v1.0/ 里（或者你的自定义 XML）加消息定义
2. 用 mavgen 生成代码
3. 在 mavlink_messages.cpp 里写一个 MavlinkStreamXxx 类
4. 在 get_streams() 里注册
5. 重新编译

如果不想动 MAVLink 定义，PX4 还提供了 MavlinkTunnel topic------可以透传任意 payload。这是更轻量的选择。

MAVLink 的 command 体系。 地面站发的命令走 vehicle_command：

// 地面站发 MAV_CMD_DO_SET_MODE → PX4 收到 → 发布 vehicle_command topic
// commander 订阅这个 topic，执行
// 结果通过 vehicle_command_ack 返回

pxh> listener vehicle_command        # 看收到的命令
pxh> listener vehicle_command_ack    # 看命令的处理结果

调试"地面站点了没反应"的标准流程 ：看 vehicle_command 有没有进来 → 看 vehicle_command_ack 返回什么 → 在 commander 里找对应的处理分支。

11.2 uXRCE-DDS：ROS 2 的桥

这块是这几年变化最大的地方，务必注意。

老的名字是 microRTPS / micrortps_client（基于 Micro RTPS 桥）。新的名字是 uxrce_dds_client（基于 eProsima 的 Micro XRCE-DDS）。 网上那些讲 micrortps 的文章都已经过时。

src/modules/uxrce_dds_client/
├── uxrce_dds_client.cpp/.h
├── Micro-XRCE-DDS-Client/          ← 子模块
├── Micro-XRCE-DDS-Client-v3/       ← v3 版本
├── dds_topics.yaml                 ← 【关键】哪些 topic 要桥接出去
├── dds_topics.h.em                 ← 模板，生成桥接代码
├── generate_dds_topics.py          ← 生成脚本
├── srv_base.cpp/.h
├── vehicle_command_srv.cpp/.h      ← service（srv）支持
└── utilities.hpp

dds_topics.yaml 决定了哪些 uORB topic 会被暴露给 ROS 2。 这个文件是双向的：

publications:
  - topic: /fmu/out/vehicle_attitude
    type: px4_msgs::msg::VehicleAttitude
  - topic: /fmu/out/vehicle_local_position
    type: px4_msgs::msg::VehicleLocalPosition
  ...

subscriptions:
  - topic: /fmu/in/offboard_control_mode
    type: px4_msgs::msg::OffboardControlMode
  - topic: /fmu/in/trajectory_setpoint
    type: px4_msgs::msg::TrajectorySetpoint
  ...

命名约定（这个必须记住）：

• /fmu/out/xxx：PX4 → ROS 2（publications）
• /fmu/in/xxx：ROS 2 → PX4（subscriptions）
• /fmu/srv/xxx：service

注意：不是所有 topic 都桥接。 只在 yaml 里列出来的才会。你想在 ROS 2 里拿到某个 topic，得先加到这个 yaml 里并重新编译固件。这是新手常见困惑------"我在 ROS 2 里 echo 不到 vehicle_status"，先检查 yaml。

跑起来的样子：

# PX4 侧
pxh> uxrce_dds_client start -t udp -h 192.168.1.100 -p 8888
# 或者通过参数
XRCE_DDS_... 

# 板载/上位机侧
MicroXRCEAgent udp4 -p 8888

相关参数：

XRCE_DDS_CFG      UART 配置（哪个串口）
XRCE_DDS_PRT      端口
XRCE_DDS_AG_IP    代理 IP
XRCE_DDS_DOM_ID   DDS 域 ID（**多机同网时必须不同！**）
XRCE_DDS_KEY      客户端 key
XRCE_DDS_SYNCT    时间同步

XRCE_DDS_DOM_ID 是个坑。 同一个网里两台飞机都用默认域 ID，ROS 2 侧会收到混在一起的话题。多机场景必须区分。

ROS 2 侧的软件栈：

px4_msgs          ← 消息定义（从 PX4 的 msg/ 自动生成）
px4_ros_com       ← 桥接与例程
px4-ros2-interface-lib  ← 写自定义模式的 C++ 库（ModeExecutors 那套）

px4-ros2-interface-lib 是这几年比较重要的一个东西------它让你可以在 ROS 2 侧写一个 C++ 类，注册成 PX4 的一个外部飞行模式（用前面说的 EXTERNAL1..8），用起来跟写原生的 PX4 模块差不多。这是 PX4 提升"生态友好度"的关键一步。

11.3 Zenoh（新的东西）

main 上还有 zenoh 模块（src/modules/zenoh/，boards/px4/.../zenoh.px4board）。Zenoh 是一个现代的发布订阅协议，特点是轻量、支持网络拓扑发现、支持网内计算。

PX4 支持它是为了什么？ 主要是给"多机器人协同"和"低带宽网络"场景用------Zenoh 天生适合设备间组网，比 DDS 在广域网上更省。

现状判断 ：这个东西还在早期。如果你只是做单机 Offboard，用 uXRCE-DDS 就好。看到 zenoh 模块不用慌，它是可选编译的。

11.4 其他协议

除了 MAVLink 和 DDS，PX4 还支持一堆协议：

协议	模块	用途
DroneCAN (UAVCAN v0)	src/drivers/uavcan/	CAN 总线设备（ESC、GPS、电源模块）
Cyphal (UAVCAN v1)	src/drivers/cyphal/	新一代 CAN 协议
MSP	src/drivers/osd/	OSD、部分飞控互操作
CRSF	src/drivers/rc/	TBS 遥控
SBUS/DSM/PPM	src/drivers/rc_input/	各种遥控
RTCM	rtcm_data topic	RTK 差分数据
ADS-B	src/lib/adsb/	航空交通监视
Iridium SBD	IridiumsbdStatus topic	卫星通信
OpenDroneID	OpenDroneId* topics	远程识别（合规要求）
ULog Streaming	log_writer_mavlink.cpp	日志通过 MAVLink 实时下发

DroneCAN 值得单独提一句。 它是 PX4 上 CAN 设备的标准做法。一个 DroneCAN 的 GPS、一个 DroneCAN 的 ESC，都是通过 CAN 总线接入，驱动在 src/drivers/uavcan/ 里。它的优势是：一条双绞线串很多设备、支持固件在线升级（uavcan_firmware_update）。

---

第 12 章 日志与调试

12.1 logger 与 ULog

PX4 的日志系统由 logger 模块负责，格式是 ULog（一种自描述的二进制格式）。

src/modules/logger/
├── logger.cpp/.h           主模块
├── log_writer.cpp/.h       写日志的抽象
├── log_writer_file.cpp/.h  写到文件（SD 卡）
├── log_writer_mavlink.cpp/.h 通过 MAVLink 实时下发
├── logged_topics.cpp/.h    【关键】决定记录哪些 topic
├── messages.h              日志内的定义
├── util.cpp/.h             工具
└── watchdog.cpp/.h         看门狗（防止写 SD 卡卡住系统）

日志存在哪：

/fs/microsd/log/YYYY_MM_DD/HH_MM_SS.ulg

logged_topics.cpp 决定了哪些 topic 会被记录，这个选择靠的是 SDLOG_PROFILE 参数（位掩码）：

Bit	名称	记录什么
0	default	基础集（必须）
1	estimator	估计器细节（estimator_*）
2	controller	控制器细节
3	sensors	传感器（sensor_gyro、sensor_accel...）
4	gps	GPS 详细数据
5	system	系统状态
6	actuator	执行器输出
7	high_rate	高速记录（IMU 原始数据，用于 FFT 分析）
8	tuning	调参相关的调试 topic
9	thermal	温度校准
10	raw_IMU_Gyro_FIFO	IMU FIFO 原始数据
11	raw_IMU_Accel_FIFO
12	video_forwarding

几个实用的配置：

# 调试振动：打开 high_rate
pxh> param set SDLOG_PROFILE 255      # 全开（注意日志会很大）
# 或者精确控制
pxh> param set SDLOG_PROFILE 1        # 只记基础

注意：全开日志会让 SD 卡快速写满，而且高速记录会占 CPU。 长时间飞行别全开。SDLOG_MODE 控制什么时候开始记（上电即记 / 解锁后记 / 一直记）。

pxh> logger status

输出会显示当前记录频率、丢包情况、写入速度。logger status 里的 dropouts 如果持续增长，说明 SD 卡太慢或者 CPU 太忙，这会丢日志，影响事后分析。

日志是分析事故的唯一可靠手段。 我在 PX4 上总结出的第一条铁律是：

只要飞机起飞，就必须有日志。 没日志的飞行等于白飞。

12.2 日志分析工具

1. QGroundControl 的日志分析

简单看一下飞行轨迹、模式变化、关键告警。快速判断"大概出了什么事"。

2. Flight Review（网页）

把 ulg 传到 https://review.px4.io/。

• 自动健康检查（振动、磁场、GPS 质量、控制性能）
• 图表
• 它给的"Flight Assessment"很有价值，能自动指出潜在问题

3. PlotJuggler（推荐）

本地工具，能加载 ulg，自由画任意 topic 的曲线。

# 用它打开 ulg
plotjuggler log.ulg

这是我最常用的工具。 超过 4 个信号的对比，PlotJuggler 的效率远超看网页。特别适合：

• setpoint vs actual 的对比（找控制问题）
• 多路传感器叠加（找传感器问题）
• 频谱分析（找振动问题）

4. pyulog（脚本化）

Python 库，能编程式读取 ulg：

from pyulog import ULog

ulog = ULog('log.ulg')
data = ulog.get_dataset('vehicle_attitude')
timestamps = data.data['timestamp']
quat = data.data['q[0]']

什么时候用 pyulog？ 批量分析几十个日志找规律的时候。比如"我想统计所有飞行中 EKF 的位置协方差最大值"，写脚本比手动看快得多。

pip install pyulog
ulog_info log.ulg        # 命令行看摘要
ulog_messages log.ulg    # 看日志里的告警消息
ulog_params log.ulg      # 看当时的参数

ulog_params 特别有用------事后分析时想知道"飞的时候参数是什么"，一条命令搞定，不用问现场的人。

5. Tools/ecl_ekf/：PX4 仓库里自带的 EKF 分析脚本。专门用来评估状态估计质量，是调 EKF 的标准工具。

12.3 现场调试手段

MAVLink Shell（前面提过，这里再强调）

python3 Tools/mavlink_shell.py udp:0.0.0.0:14550

飞机在天上、你在地上，通过数传就能进 shell 敲命令。这是排查"飞起来之后才出现的问题"的唯一手段。

实时话题监控

pxh> listener vehicle_local_position
pxh> uorb top

listener 的隐藏用法 ：配合 -n 参数看速率变化。

pxh> listener -n 1 vehicle_attitude     # 每次都打时间戳

事件系统

pxh> events                       # 看事件历史
pxh> events help

这些事件是有优先级的 （emergency/critical/error/warning/info），而且会通过 MAVLink 转成 STATUSTEXT 报给 QGC。飞完下来先看这个，常常能发现"其实早就报警了但被忽略了"。

dmesg 和 hardfault_log

pxh> dmesg
pxh> hardfault_log info

hardfault_log 是排查"随机重启"的关键。芯片 HardFault 后现场存在 RTC 备份域，下次启动 rcS 会检查。

perf

pxh> perf                          # 所有计数器
pxh> mc_rate_control status        # 单个模块的 perf 输出

work_queue status

pxh> work_queue status

这个命令应该成为你的习惯。它会显示每个队列的负载和栈使用。看到某个队列的栈用了 80% 以上，就该考虑加栈了。

12.4 故障定位的套路

我把这些年的排故逻辑整理成一个决策树，希望帮你少走弯路。

症状：飞机起飞后抖

① 先看日志里的传感器
   → sensor_accel / sensor_gyro 的 clip_count 是否在涨？
   是 → 振动导致 IMU 饱和 → 去②
   否 → 去③

② 振动问题
   → 看 sensor_gyro_fft 的频谱，找峰值频率
   → 设置陷波滤波器 IMU_GYRO_NF0_FRQ / IMU_GYRO_NF1_FRQ
   → 检查机架的机械问题（螺丝、减震垫、桨的动平衡）
   → 顺便看 IMU_GYRO_CUTOFF / IMU_DGYRO_CUTOFF 是否过高

③ 控制参数问题
   → 把 ang_rate setpoint 和 ang_rate actual 画在一起
   → 如果 actual 跟随 setpoint 时高频振荡 → 角速率环 P/D 太大
   → 如果 actual 明显滞后/跟不上 → 角速率环增益太小
   → 检查 MC_ROLLRATE_K 和 _P/_I/_D 的搭配

症状：悬停时缓慢漂移

① 看 estimator_innovations
   → 位置新息是否持续偏置？
   是 → EKF 有问题（时间同步？磁力计校准？）→ 去②
   否 → 去③

② EKF 问题
   → 检查磁力计校准（CAL_MAG0_*）
   → 检查 GPS 质量（satellites_used、eph）
   → 看 estimator_status_flags 里的具体标志位
   → 检查有没有时间戳异常（timestamp_sample 是否连续）

③ 控制问题
   → MPC_XY_VEL_I_ACC 太小，抗风能力不足
   → 或者 MPC_XY_P 太小，位置跟踪迟缓

症状：飞控随机重启

① 查 hardfault_log info
   → 有记录 → 去②

② 看 hardfault 现场
   → hardfault_log commit 把现场写到 SD 卡
   → 用 pyulog 或者 arm-none-eabi-addr2line 解析栈回溯
   → 找是哪个函数踩的内存

③ 没有 hardfault 记录的情况
   → 看是不是看门狗复位（task_watchdog 的记录）
   → 看是不是电压跌落（battery_status 的电压曲线）
   → 看是不是过热（ESC 或飞控温度）

通用原则（我自己的三条排故铁律）：

1. 先看数据，再猜原因。 PX4 的日志记录了几乎所有东西，先看再想。
2. 一次只改一个变量。 同时改三个参数，出了问题你不知道是哪个。
3. 地面能复现的，绝不在天上试。 PX4 的 SITL 和 HIL 就是为这个存在的。

---

第 13 章 仿真：SITL 与 HIL

13.1 SITL 到底在做什么

SITL = Software In The Loop（软件在环）。

核心思想很简单：把 PX4 编译成 Linux 上跑的普通程序，然后用一个物理仿真器代替真实飞机。PX4 完全不知道自己在仿真里------它以为自己真的在控制一架飞机。

实现方式就是利用 PX4 的分层设计：

真实硬件                           SITL
────────────────────────────────────────────────────────
STM32H7 芯片          →          Linux x86 进程
NuttX RTOS            →          POSIX 线程
platforms/nuttx/      →          platforms/posix/
真实 IMU (SPI)         →          sensor_gyro_sim / sensor_accel_sim 等
真实电机 → PWM         →          pwm_out_sim
真实空中环境            →          Gazebo / JSBSim / jMAVSim / FlightGear

这是 PX4 架构成熟度的一个直接体现。 飞栈以上（src/modules/ 里的估计器、控制器、决策层）一行都不改，只在底层换实现。这就是为什么你在 SITL 上调好的参数可以直接拿到真机上用。

13.2 SITL 的组成

ROMFS/px4fmu_common/init.d-posix/     ← SITL 专用的启动脚本
├── rcS                                SITL 的启动入口
├── airframes/                         仿真机型（按名字匹配）
├── px4-rc.simulator                   启动仿真器
├── px4-rc.gzsim                       Gazebo 桥接
├── px4-rc.mavlink                     MAVLink 仿真
├── px4-rc.mavlinksim
├── px4-rc.sihsim                      SIH 仿真
├── px4-rc.rotorpy
└── rc.replay                          【日志回放】用日志喂 EKF

src/modules/simulation/
├── simulator_mavlink/                 ← 与外部仿真器通信（主力）
├── gz_bridge/                         ← Gazebo (gz) 桥接
├── gz_msgs/
├── gz_plugins/                        ← Gazebo 插件（传感器仿真）
├── pwm_out_sim/                       ← 仿真版的输出驱动
├── battery_simulator/                 电池模型
├── system_power_simulator/
├── sensor_agp_sim/                    辅助全球定位
├── sensor_airspeed_sim/
├── sensor_baro_sim/
├── sensor_gps_sim/
├── sensor_mag_sim/
└── simulator_sih/                     SIH（内置简化模型）

通信协议：simulator_mavlink 用 MAVLink 的 HIL（Hardware In the Loop）消息和仿真器对话：

PX4 → 仿真器：
  HIL_ACTUATOR_CONTROLS     舵机/电机输出
  HIL_SENSOR（时间同步）     请求仿真时间

仿真器 → PX4：
  HIL_SENSOR                 IMU、磁力计、气压计
  HIL_GPS                    GPS
  HIL_STATE_QUATERNION       真实状态（用于对比评估）
  HIL_OPTICAL_FLOW           光流
  HIL_DISTANCE_SENSOR        测距
  HIL_RC_INPUTS_RAW          遥控
  ...

为什么用 MAVLink 而不是自定义协议？ 因为 MAVLink 已经有现成的 HIL 消息定义，而且这样就天然复用了 MAVLink 的传输层（UDP/TCP/串口）------所以你能在另一台机器上跑仿真器。

lockstep（锁步）模式： 这是 SITL 精度最高的模式。仿真器和 PX4 严格交替执行：

PX4 跑一步（一个 IMU 周期）→ 发 HIL_ACTUATOR_CONTROLS → 等
仿真器收到 → 推进物理模型一个步长 → 发 HIL_SENSOR → 等
PX4 收到 → 继续下一步

这样仿真时间和真实时间解耦 ，仿真器可以比实时快（跑加速仿真）或慢。而且结果是完全可复现的------同样的输入必然产生同样的输出，这对调试至关重要。

boards/px4/sitl/ 里有 nolockstep.px4board（不开锁步）和默认配置（开锁步）。

13.3 怎么跑

最快的方式（推荐 Gazebo）：

# 会自己装 Gazebo（gz），然后启动 PX4 + 仿真
make px4_sitl gz_x500

启动后你会看到 PX4 的 pxh> 提示符，同时 Gazebo 窗口里出现一架 X500 四旋翼。这时候：

pxh> commander takeoff
pxh> commander arm
pxh> listener vehicle_local_position

其他选项：

make px4_sitl jmavsim              # jMAVSim（轻量，Java 写的，适合纯算法调试）
make px4_sitl gazebo-classic_iris  # Gazebo Classic（老，但资源多）
make px4_sitl sih                  # SIH：完全内置的简化模型，不需要外部仿真器
make px4_sitl none                  # 不启仿真器，只有 PX4 在跑

make px4_sitl none 很有用------当你只想测试一个纯粹的软件逻辑（比如参数系统、uORB、某个决策逻辑），不需要物理模型的时候。

跑之前需要什么：

# Gazebo (新版 gz)
python3 Tools/setup/ubuntu.sh --no-nuttx --no-sim-tools  # 只装基础
# Gazebo 由 make 自动下载（首次会比较慢）

# 或者手动
./Tools/simulation/gz/setup.sh

跟 ROS 2 一起跑：

# 终端 1：PX4 SITL
make px4_sitl gz_x500

# 终端 2：Micro XRCE Agent
MicroXRCEAgent udp4 -p 8888

# 终端 3：ROS 2 节点
source install/setup.bash
ros2 topic list
ros2 topic echo /fmu/out/vehicle_attitude

SITL 默认就把 uxrce_dds_client 起好了（px4-rc.mavlink / gzsim 里配的），连上 Agent 就能用。

用 SITL 跑自动化测试：

make tests                      # 单元测试（在 SITL 里跑）
make tests_integration          # 集成测试
make rostest                    # ROS 集成测试

test/ 目录下有大量测试，比如 test/mavsdk_tests/。这是学 PX4 的一个宝藏------想知道某个功能"应该怎么用"，看测试用例比看文档准。

13.4 日志回放（Replay）------ 一个被低估的功能

rc.replay 那个文件是干这个的：把一段真实飞行的日志喂给 EKF2，重放估计过程。

make px4_sitl_default
./build/px4_sitl_default/bin/px4 -s etc/init.d-posix/rc.replay log.ulg

这有什么用？ 极其有用：

1. 算法回归测试：你改了 EKF2 的代码，想验证"对同一段飞行数据，新代码的结果是否更好"。重放就行，不需要重新飞。
2. 事后分析：飞机出了怪事，你可以回到当时的传感器数据上，单步走 EKF，看它是在哪一刻开始发散的。
3. 参数验证：想试新参数但不敢飞？先在重放上跑一遍。

日志回放是 PX4 在工程严谨性上做得最到位的地方之一。 ArduPilot 也有类似功能，但 PX4 的这个用起来更顺。

13.5 HIL

HIL = Hardware In the Loop（硬件在环）。

和 SITL 的区别：PX4 跑在真实飞控硬件上，只是传感器数据来自仿真器。

真实飞控板（跑真固件）── 串口/UDP ── 仿真器
   真实的 CPU、内存、时序              虚拟世界

在 PX4 侧，启动方式：

pxh> simulator start -t <仿真器> 

HIL 的价值：

• 能在真硬件上验证性能和时序（SITL 跑在 x86 上，时序完全不同）
• 能测真实的传感器驱动以外的一切
• 可以在没有飞机的条件下测新固件

HIL 的代价：搭建麻烦（要处理串口/USB 的连接、波特率），而且如果是通过真实串口，带宽会成为瓶颈。

我的建议：95% 的开发用 SITL 就够了。只有当你要验证"时序相关的问题"（比如栈溢出、任务调度抖动）或者"硬件相关的问题"（比如串口驱动的 DMA 问题）时，才值得上 HIL。

---

第 14 章 端到端：一次自动起飞的全链路走查

这一章是把前面所有东西串起来。我沿着"你在 QGC 里点了自动起飞"这个动作，把信号流的每一站列出来。如果你能把这条线在脑子里走通，你对 PX4 架构的理解就到位了。

14.1 触发

你在地面站点了"起飞" → QGC 发一条 MAVLink 命令：

MAV_CMD_NAV_TAKEOFF
  param5 (lat): ...
  param6 (lon): ...
  param7 (alt): 10.0     ← 目标高度 10 米

MAVLink 模块接收 （mavlink_main.cpp / mavlink_receiver.cpp）：

// 收到命令 → 转成 uORB
vehicle_command_s cmd{};
cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF;
cmd.param7 = altitude;
cmd.timestamp = hrt_absolute_time();
_vehicle_command_pub.publish(cmd);

结果 ：vehicle_command topic 上出现了一条起飞命令。

14.2 commander 处理

commander 订阅了 vehicle_command ，在 Commander::handle_command() 里分发：

case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF:
	// 检查是否已解锁
	if (!_vehicle_status.arming_state == ARMING_STATE_ARMED) {
		// 未解锁 → 先走自动解锁流程
		...
	}
	// 检查当前模式能不能起飞
	// 设置导航状态
	_vstatus.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF;
	// 发布 ack
	ack_vehicle_command(VEHICLE_CMD_RESULT_ACCEPTED);
	break;

同时它还要跑一遍健康检查，确保所有前置条件满足（GPS 锁定、EKF 收敛、电池正常）。

结果：

• vehicle_status.nav_state = NAVIGATION_STATE_AUTO_TAKEOFF
• vehicle_command_ack 返回 ACCEPTED（QGC 显示"起飞中"）

14.3 navigator 接管

navigator 检测到 nav_state 变化 ，切换到 Takeoff 模式：

// navigator_main.cpp 的主循环
void Navigator::run()
{
	// 模式切换
	if (_navigation_mode->getID() != new_mode) {
		_navigation_mode->on_inactive();
		_navigation_mode = get_mode(new_mode);   // → Takeoff
		_navigation_mode->on_activation();
	}
	// 跑 Takeoff 的逻辑
	_navigation_mode->run(true);
	...
}

Takeoff::run() 做什么 （takeoff.cpp）：

- 记录起飞点（Home）
- 设置位置 setpoint：当前位置（水平不变）+ 目标高度
- 检查是否已离地（用 land_detector）
- 达到目标高度后 → 切到 Loiter 或者进入任务

结果 ：position_setpoint_triplet / trajectory_setpoint topic 更新为"目标高度 10 米"。

14.4 flight_mode_manager 与控制器

flight_mode_manager 看到 nav_state = AUTO_TAKEOFF，把 navigator 给的 setpoint 传递给位置控制器。

mc_pos_control 接手（MulticopterPositionControl::Run()）：

void MulticopterPositionControl::Run()
{
	// 1. 检查参数更新
	// 2. 读取输入
	_vehicle_local_position_sub.update(&_local_pos);
	_trajectory_setpoint_sub.update(&_setpoint);

	// 3. 跑位置控制器
	//    位置 → 速度 → 加速度 → 倾斜角 + 推力
	_control.setInputSetpoint(setpoint);
	_control.setState(...);
	_control.update(dt);

	// 4. 输出
	vehicle_attitude_setpoint_s attitude_setpoint{};
	_control.getAttitudeSetpoint(attitude_setpoint);
	_attitude_sp_pub.publish(attitude_setpoint);
}

结果 ：vehicle_attitude_setpoint 上出现"倾斜 0°、偏航不变、推力 0.7（归一化）"。

mc_att_control 接手：

- 输入：期望姿态（四元数）+ 当前姿态（来自 EKF）
- 计算姿态误差 → 期望角速率
- 输出：vehicle_rates_setpoint

结果 ：vehicle_rates_setpoint 上出现"角速率基本为 0"（因为姿态已经对了）。

mc_rate_control 接手（跑在最高优先级的 wq:rate_ctrl）：

void MulticopterRateControl::Run()
{
	if (_parameter_update_sub.updated()) { ... }

	vehicle_angular_velocity_s angular_velocity;
	if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
		// 计算 dt（带限幅）
		// 跑 PID
		Vector3f torque = _rate_control.update(rates, rates_sp, angular_accel, dt, landed);
		// 输出力矩 + 推力
		vehicle_torque_setpoint_s torque_sp{};
		vehicle_thrust_setpoint_s thrust_sp{};
		torque_sp.xyz = torque;
		thrust_sp.xyz = ...;
		_vehicle_torque_setpoint_pub.publish(torque_sp);
		_vehicle_thrust_setpoint_pub.publish(thrust_sp);
	}
}

结果 ：vehicle_torque_setpoint（三个轴的力矩）和 vehicle_thrust_setpoint（推力）。

14.5 control_allocator

void ControlAllocator::Run()
{
	// 1. 读 setpoint
	_vehicle_torque_setpoint_sub.update(&_torque_sp);
	_vehicle_thrust_setpoint_sub.update(&_thrust_sp);

	// 2. 构建控制目标
	Vector4f control = {torque.x, torque.y, torque.z, thrust};  // 4 个控制轴

	// 3. 分配（用 effectiveness matrix）
	_control_allocation->allocate();
	_control_allocation->setActuatorSetpoint(&control);
	_control_allocation->update();

	// 4. 输出到执行器
	actuator_motors_s motors{};
	motors.control[0] = _actuator[0];
	...
	_actuator_motors_pub.publish(motors);
}

分配过程（用四旋翼举例）：

输入：[τx, τy, τz, T] = [0.01, -0.005, 0, 0.7]     ← 要一点点滚转力矩 + 0.7 归一化推力

effectiveness matrix（X 型四旋翼，电机在四个角上）：
        M1      M2      M3      M4
τx   [ -d·k    +d·k    +d·k    -d·k ]    滚转
τy   [ +d·k    -d·k    +d·k    -d·k ]    俯仰
τz   [ -c      -c      +c      +c   ]    偏航（反扭矩）
T    [ +1      +1      +1      +1   ]    总推力

解出：[M1, M2, M3, M4] ≈ [0.7, 0.72, 0.68, 0.7]

结果 ：actuator_motors 上出现四个电机的归一化转速。

14.6 输出驱动

pwm_out / dshot / uavcan 驱动 （继承 mixer_module）读取 actuator_motors：

- 按 PWM_MAIN_FUNC / PWM_AUX_FUNC 参数把 actuator_motors[i] 映射到物理通道
- 归一化的 [-1, 1] → PWM 脉宽（1000--2000 μs）或 DShot 帧
- 考虑 failsafe、armed 状态、输出限幅
- 写硬件寄存器 / 发 DMA / 发 CAN 帧

结果 ：4 个 PWM 通道输出对应脉宽。同时发布 actuator_outputs（用于日志）。

14.7 反馈闭环

传感器端同时在进行：

IMU (SPI) → icm42688p 驱动 → sensor_gyro[0] / sensor_accel[0]
    ↓
sensors 模块 → vehicle_angular_velocity / vehicle_acceleration
    ↓
ekf2 → vehicle_attitude / vehicle_local_position / vehicle_global_position
    ↓
（回到 mc_pos_control / mc_att_control / mc_rate_control 的输入）

这个环的速度决定了控制带宽。 前面那张频率表，就是为了说明这个环有多快。

14.8 同时发生的事

在控制闭环跑的同时，这些事也在发生：

模块	干什么	频率
logger	把所有 topic 写进 ulg	按配置
mavlink	把状态发给 QGC 显示	按 stream 配置
land_detector	判断"是否已离地"	10 Hz+
commander	持续监控健康状态、失控保护	事件驱动
mc_hover_thrust_estimator	估计悬停推力（提高高度控制精度）	低频
ekf2	持续融合所有传感器	按 IMU 驱动
rc_update	持续解析遥控输入	按 RC 驱动
battery_status	监控电池	低频
mavlink_ftp	准备接你的文件传输请求	事件驱动

这就是"响应式架构"的样子：没有中心调度，每个模块各自被数据驱动，整个系统并行运转。

14.9 这条链上"如果断了会怎样"

这个表格是我在实际排故时最常用的：

断在哪	症状	第一眼去看
MAVLink 没收到命令	QGC 点了没反应	listener vehicle_command
commander 拒绝	QGC 弹错误提示	commander check、listener health_report
navigator 没切模式	模式没变	listener vehicle_status、navigator status
位置控制器没输出	有姿态但不动	listener vehicle_attitude_setpoint
姿态控制器没输出	姿态不动	listener vehicle_rates_setpoint
角速率环没输出	都不动	listener vehicle_torque_setpoint、mc_rate_control status
分配器没输出	有力矩无电机	listener control_allocator_status
输出驱动没输出	分配对了但电机不转	pwm_out status、actuator_outputs
传感器没有	EKF 不收敛	listener vehicle_angular_velocity、sensors status
EKF 不收敛	不能解锁/位置跳	listener estimator_status_flags、ekf2 status

这张表的价值在于：它把"飞机不动"这个模糊的现象，拆成了 10 个可逐一二分定位的环节。 从中间往两边查，一般三次就能找到断点。

---

第 15 章 动手：写一个自己的模块

理论说完了，动手。这一章带你从零写一个模块，把前面讲的东西全部用上。

15.1 最省事的开始方式

PX4 仓库里有两个模板目录，直接抄：

src/templates/template_module/     ← 标准模块模板
src/examples/                      ← 一堆示例
├── work_item/                     ← 工作队列 + WorkItem
├── px4_simple_app/                ← 最简单的模块
├── module_template/
└── ...

src/examples/px4_simple_app/ 是最小的例子，一个文件，几十行。我建议你先编译它跑一遍：

# 加到板子配置（或者用 SITL 的 all-yes 配置）
# boards/px4/sitl/default.px4board 加 CONFIG_EXAMPLES_PX4_SIMPLE_APP=y

make px4_sitl_default
# 跑起来后
pxh> px4_simple_app

看到输出，你就走通了"编进去、跑起来"的闭环。

15.2 写一个有用的模块

我们要写一个模块，功能是：监控电池电压，如果低于阈值就周期性发一条事件告警。

虽然 battery_status 模块已经做了类似的事，但这个例子刚好能覆盖"订阅 topic + 参数 + 定时任务 + 事件上报"这四个最常用的模式。

第一步：建目录和文件

src/modules/battery_watch/
├── CMakeLists.txt
├── Kconfig
├── battery_watch_params.yaml
├── BatteryWatch.cpp
└── BatteryWatch.hpp

第二步：定义参数 （battery_watch_params.yaml）

module_name: battery_watch
parameters:
- group: Battery Watch
  definitions:
    BW_WARN_THR:
      description:
        short: Low voltage warning threshold
        long: Battery voltage below this value triggers a warning event.
      type: float
      default: 10.5
      unit: V
      min: 3.0
      max: 30.0
      decimal: 2
      increment: 0.1

    BW_WARN_INTERVAL:
      description:
        short: Warning repeat interval
      type: float
      default: 5.0
      unit: s
      min: 0.5
      max: 60.0
      decimal: 1

第三步：写头文件 （BatteryWatch.hpp）

#pragma once

#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/parameter_update.h>

class BatteryWatch : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem
{
public:
	BatteryWatch();
	~BatteryWatch() override = default;

	static int task_spawn(int argc, char *argv[]);
	static int custom_command(int argc, char *argv[]);
	static int print_usage(const char *reason = nullptr);

	bool init();
	void Run() override;
	int print_status() override;

private:
	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};

	battery_status_s _battery_status{};
	hrt_abstime _last_warning{0};

	DEFINE_PARAMETERS(
		(ParamFloat<px4::params::BW_WARN_THR>)      _param_warn_thr,
		(ParamFloat<px4::params::BW_WARN_INTERVAL>) _param_warn_interval
	)
};

注意 DEFINE_PARAMETERS 这个宏。 它是 PX4 的参数访问机制：把 yaml 里定义的参数映射成 C++ 成员。px4::params::BW_WARN_THR 是自动生成的常量，跟你 yaml 里的名字对应。

第四步：实现 （BatteryWatch.cpp）

#include "BatteryWatch.hpp"

#include <px4_platform_common/events.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/log.h>

ModuleBase::Descriptor BatteryWatch::desc{task_spawn, custom_command, print_usage};

BatteryWatch::BatteryWatch() :
	ModuleParams(nullptr),
	// 挂到低优先级队列：这个任务不急，别占着高速队列
	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
{
}

bool BatteryWatch::init()
{
	// 每 500ms 跑一次
	ScheduleOnInterval(500_ms);
	return true;
}

void BatteryWatch::Run()
{
	if (should_exit()) {
		ScheduleClear();
		exit_and_cleanup(desc);
		return;
	}

	// ------ 参数更新处理（标准三段式）------
	if (_parameter_update_sub.updated()) {
		parameter_update_s param_update;
		_parameter_update_sub.copy(&param_update);
		updateParams();
	}

	// ------ 读电池状态 ------
	if (_battery_status_sub.update(&_battery_status)) {

		// 只在解锁后告警（地面上电压低很正常）
		if (_battery_status.connected && _battery_status.voltage_v < _param_warn_thr.get()) {

			const hrt_abstime now = hrt_absolute_time();
			const hrt_abstime interval_us = (hrt_abstime)(_param_warn_interval.get() * 1e6f);

			if (now - _last_warning > interval_us) {
				_last_warning = now;

				// 发事件：地面上站会显示、日志里也会记录
				events::send<float>(events::ID("battery_watch_low"), events::Log::warning,
						    "Battery voltage low: {1:.2V}", (double)_battery_status.voltage_v);
			}
		}
	}
}

int BatteryWatch::print_status()
{
	PX4_INFO("Battery: connected=%d  voltage=%.2f V  warn threshold=%.2f V",
		 (int)_battery_status.connected,
		 (double)_battery_status.voltage_v,
		 (double)_param_warn_thr.get());
	PX4_INFO("Last warning at: %" PRIu64 " us", _last_warning);
	return 0;
}

int BatteryWatch::custom_command(int argc, char *argv[])
{
	return print_usage("unknown command");
}

int BatteryWatch::task_spawn(int argc, char *argv[])
{
	BatteryWatch *instance = new BatteryWatch();

	if (instance && instance->init()) {
		// 把实例注册到 descriptor，ModuleBase::main 用得到
		desc.object.store(instance);
		desc.task_id = task_id_is_work_queue;   // 工作队列类型
		return PX4_OK;
	}

	PX4_ERR("driver start failed");
	delete instance;
	return PX4_ERROR;
}

int BatteryWatch::print_usage(const char *reason)
{
	if (reason) {
		PX4_WARN("%s\n", reason);
	}
	PRINT_MODULE_DESCRIPTION(
		R"DESCR_STR(
### Description
Watch the battery voltage and emit a warning event when it drops below a threshold.

### Examples
$ battery_watch start
)DESCR_STR");

	PRINT_MODULE_USAGE_NAME("battery_watch", "module");
	PRINT_MODULE_USAGE_COMMAND("start");
	PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Print status info");
	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

	return 0;
}

extern "C" __EXPORT int battery_watch_main(int argc, char *argv[])
{
	return ModuleBase::main(desc, argc, argv);
}

这段代码里有几个模式你要记住：

1. ModuleBase::Descriptor 静态成员：新架构的写法。
2. ScheduledWorkItem(MODULE_NAME, 队列)：声明自己属于哪个队列。
3. ScheduleOnInterval(500_ms)：周期执行。
4. should_exit() + exit_and_cleanup(desc) ：标准的退出处理，每个 Run() 开头都要有。
5. 参数三段式 ：updated() → copy() → updateParams()。
6. events::send<...>()：发事件，自动转 MAVLink STATUSTEXT + 记日志。
7. PRINT_MODULE_DESCRIPTION / PRINT_MODULE_USAGE_* ：这些宏生成 usage 输出，make module_documentation 还会把它们提取成文档。

第五步：CMakeLists.txt

px4_add_module(
	MODULE modules__battery_watch
	MAIN battery_watch
	SRCS
		BatteryWatch.cpp
	DEPENDS
		px4_work_queue
	)

第六步：Kconfig

menuconfig MODULES_BATTERY_WATCH
	bool "battery_watch"
	default n
	---help---
		Enable battery_watch module.

第七步：加到板子配置

在 boards/px4/sitl/default.px4board 加：

CONFIG_MODULES_BATTERY_WATCH=y

第八步：加到启动脚本

在 ROMFS/px4fmu_common/init.d/rc.mc_apps 末尾加：

battery_watch start

（或者更规范的做法：写一个 rc.board_extras，或者用 SD 卡的 extras.txt）

第九步：编译和测试

make px4_sitl_default
# 起来之后
pxh> battery_watch start
pxh> battery_watch status
pxh> param show BW_*

到这里你就走完了一个完整的模块开发循环。 是不是没有想象中那么难？

15.3 写模块的几个经验

1. 选对工作队列。

• 高速（>100Hz）、对延迟敏感的 → wq:rate_ctrl 或 nav_and_controllers
• 中速（10--100Hz）→ hp_default
• 低速（<10Hz）、不着急 → lp_default

选错的后果 ：放在 lp_default 上的高速任务会被延迟；放在 rate_ctrl 上的慢任务会拖慢整个控制环。

2. 不要在 Run() 里阻塞。

工作队列的 Run() 不能 sleep、不能等文件 IO、不能等网络。需要这些的话，用独立任务。

3. 不要在 Run() 里做动态内存分配。

new/malloc 在实时上下文里是禁忌（不确定的执行时间 + 碎片化）。所有缓冲区在构造函数里分配好。

4. Run() 要短。

用 perf 量一下：

perf_begin(_loop_perf);
// ...
perf_end(_loop_perf);

目标：单次执行时间 < 周期的 1/10。 比如 100Hz 的任务（10ms 周期），单次别超过 1ms。

5. 一定要有 status。

给别人（包括未来的自己）留一个能看状态的命令。pxh> xxx status 的输出质量，直接决定了三年后你还敢不敢动这个模块。

6. 用 PX4_INFO/WARN/ERR，不要用 printf。

这些宏会带上模块名，而且会通过 MAVLink 报给地面站。

PX4_INFO("something happened: %d", value);
PX4_WARN("this might be a problem");
PX4_ERR("this is definitely a problem");

15.4 关于 out-of-tree 模块

如果你不想改 PX4 仓库（比如你的代码要闭源，或者想独立于 PX4 的发布节奏），PX4 支持 out-of-tree 编译。

# 参考文档
docs/en/advanced/out_of_tree_modules.md

核心做法是写一个独立的 CMake 工程，find_package(px4) 然后 px4_add_module。也可以自己定义 out-of-tree 的 uORB 消息。

什么时候值得用 out-of-tree？

• 商业项目，代码不能开源
• 模块和 PX4 主干关系不大，不想跟着 rebase
• 想同时维护多个私有机型的适配

代价 ：PX4 的 API 在没有版本保证的情况下会变（尤其是内部头文件），out-of-tree 模块需要跟着升级。所以如果你只是想加个功能，直接改仓库往往更省事。

---

第 16 章 动手：移植一块新板子

移植是进阶内容，但我把要点列清楚，你需要的时候照着跑。

16.1 移植需要什么

硬件层面：

• 一块跑 STM32 的板子（PX4 官方支持 H7/F7/G4 等系列）
• 时钟树、内存布局、外设引脚（I2C/SPI/UART/CAN/PWM）
• 传感器清单（型号 + 总线 + 地址）
• Bootloader 硬件（启动引脚、Flash 布局）

软件层面：

• 一个"最像"的现有板子作为起点
• NuttX 的板级配置
• PX4 的板级配置

16.2 移植步骤

第一步：找最像的板子

# 比如你的板子是 H7 + ICM42688P + BMI088 + BMP388 + INA226
# 那就从 fmu-v6x 开始（它用的就是这套）
cp -r boards/px4/fmu-v6x boards/px4/myboard

第二步：改配置文件

需要改的：

boards/px4/myboard/
├── default.px4board           ← 传感器、串口、模块开关
├── nuttx-config/              ← NuttX 的 defconfig + 板级初始化
├── init/rc.board_*            ← 板级启动脚本
├── src/                       ← 板级代码（引脚定义、外设初始化）
├── extras/                    ← 额外的文件（bootloader、IO 固件）
└── cmake/                     ← 构建配置

default.px4board 是核心（前面看过 fmu-v6x 的）：

CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
CONFIG_BOARD_ARCHITECTURE="cortex-m7"
CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0"
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
CONFIG_BOARD_SERIAL_RC="/dev/ttyS5"
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
CONFIG_DRIVERS_BAROMETER_BMP388=y
CONFIG_DRIVERS_POWER_MONITOR_INA226=y
CONFIG_DRIVERS_PWM_OUT=y
CONFIG_DRIVERS_GPS=y
...

第三步：NuttX 配置

nuttx-config/ 下是 NuttX 的板级定义：

nuttx-config/
├── nsh/defconfig            ← NuttX 的功能开关
├── include/board.h          ← 引脚、时钟定义
├── scripts/                 ← 链接脚本、Flash 布局
└── src/stm32_*.c            ← 板级初始化代码（时钟、GPIO、外设）

改这块是移植里最费时间的部分。 时钟树配错 → 串口波特率不对；引脚配错 → 传感器找不到；Flash 布局配错 → 起不来。

第四步：引脚与传感器定义

在 boards/px4/myboard/src/ 下（或者 platforms/nuttx/src/px4/...）定义：

// I2C 总线
#define PX4_I2C_BUS_EXPANSION   1
#define PX4_I2C_BUS_ONBOARD     2

// SPI 总线
#define PX4_SPI_BUS_SENSORS     1
#define PX4_SPI_BUS_EXT         4

以及各传感器的 CS 引脚、中断引脚。

第五步：自动探测

现代驱动都支持 -a 自动扫描，所以只要总线定义对了，不用手动指定地址。但总线定义错了，探测就找不到。

第六步：编译测试

make myboard_default
make myboard_default upload

第七步：逐项验证

这是最重要的一步，顺序不能乱：

# 1. 能不能启动、进 shell
pxh> ver all
pxh> dmesg

# 2. 传感器有没有被发现
pxh> sensors status
pxh> listener sensor_gyro
pxh> listener sensor_accel
pxh> listener sensor_mag
pxh> listener sensor_baro

# 3. 数据合理性（静置时陀螺应该接近 0，加速度 z 轴约 -9.8）
pxh> listener sensor_accel 5

# 4. 输出
pxh> pwm_out status
pxh> actuator_test 0 0.1      # 小心！会转电机

# 5. 遥控输入
pxh> listener input_rc

# 6. 通信
pxh> mavlink status

# 7. 参数
pxh> param show -c
pxh> param save
pxh> reboot
pxh> param show -c            # 确认持久化

# 8. 校准
# 用 QGC 做传感器校准、遥控校准、ESC 校准

# 9. 解锁
pxh> commander check
pxh> commander arm

移植的常见坑（按我遇到过的频率排序）：

1. 时钟配置不对 → 一切时序都不对（串口乱码、SPI 频率错）
2. 引脚复用冲突 → 两个外设抢同一个引脚，症状随机
3. Flash 扇区布局不对 → bootloader 和固件重叠，起不来
4. 传感器中断引脚配错 → 数据能读但只有轮询频率
5. PWM 定时器通道配错 → 输出映射到错误的通道
6. 电源监控 ADC 系数不对 → 电压电流读数偏差
7. 忘记改 MAV_SYS_ID → 多机同空域时冲突（这不是移植问题但是装机必做）

建议 ：移植时一次只改一处，改完立刻验证。一次改十个地方然后发现不对，你会花十倍时间定位。

---

第 17 章 版本演进与常见误区

17.1 近几年的重要变化

我把 v1.12 到 v1.18 期间对架构影响最大的变化列一遍。这张表的内容来自仓库里的 docs/en/releases/*.md（官方发布说明），不是我的记忆，你读老资料时可以对照着看：

版本	变化	影响
v1.12	引入 EKF2Selector，支持多 EKF 实例并行	估计冗余
v1.13--v1.14	参数系统迁移到 YAML 定义 + 代码生成	PARAM_DEFINE_* 宏逐步消失
v1.14	Dynamic Control Allocation 默认启用，彻底取代 mixer 文件	控制分配架构换血
v1.14	microRTPS（Fast-RTPS 桥）被 uXRCE-DDS 取代	ROS 2 桥接方式变了，不再需要自定义构建目标
v1.14	新 Gazebo 成为默认仿真器；failsafe 简化（多个 failsafe 同时触发时取"更严重的动作"）；预检查改为 events 接口上报	仿真与安全逻辑
v1.15	EKF2 从"全状态"改为"误差状态"推导 ；SYS_MC_EST_GROUP 移除，改用 EKF2_EN / ATT_EN / LPE_EN	滤波器数值稳定性提升
v1.15	Gazebo 取代 jMAVSim 成为默认仿真引擎；引入 ROS 2 Interface Library；Position Slow 模式；抛飞模式（实验）	生态与模式
v1.16	uORB 消息版本化 （消息加 MESSAGE_VERSION 字段）+ ROS 2 Message Translation Node	ROS 2 跨版本兼容
v1.16	无人车支持重构（Ackermann / differential / mecanum 独立模块）；Gazebo Harmonic LTS；日志加密；双向 DShot	多机型
v1.17	新增多旋翼 Altitude Cruise 模式；MAV_PROTO_VER 默认改为 2；PWM_*_TRIM → PWM_*_CENTER；移除 RC 通道 1--8 的硬编码 1% 死区	多个破坏性参数变更
v1.17	Zenoh 中间件成熟到 rmw_zenoh 兼容；MC 神经网络控制（TensorFlow Lite Micro，实验性）	实验特性
v1.18	新增固定翼 Guided Course 模式；手动控制接管逻辑重做（MAN_OVERRIDE_SPD 取代 COM_RC_OVERRIDE/COM_RC_STICK_OV）；MAVLink 签名符合规范；EKF2_SENS_EN 位掩码 + 运行时可切换融合源；GPS 配置改为按接收机（SENS_GPS0_*）	若干破坏性变更
v1.18	commander lockdown on → commander termination，FORCE_FAILSAFE → TERMINATION，manual_lockdown → kill；SBOM 自动生成；UAVCAN 节点可从 SD 卡刷固件	接口重命名

main 分支上还有更靠前的东西（描述符化的 ModuleBase、flight_mode_manager、failure_injection_manager 等），它们会在 v1.19 或之后的版本里进稳定分支。

看这张表你就明白了 ：网上 2020--2021 年那批讲"PX4 的 mixer 在 src/lib/mixer 里，机型靠 .mix 文件定义"的教程，现在是完全错的------mixer 文件已经在 v1.14 被 Control Allocator 取代，机型几何改由 QGC 的 Actuators 界面 + CA_* 参数定义。

另外一个实用提醒 ：v1.17 和 v1.18 的升级说明里各列了十几条"升级前必读"。升级 PX4 前一定要读本版本的 release notes，尤其是那些参数被重命名或删除的条目------它们的旧值不会自动迁移，直接飞可能出事。

17.2 常见误区

误区 1：vehicle_attitude 是传感器数据。

错。它是 EKF2 融合后的估计值 。原始数据在 sensor_gyro / sensor_accel。

误区 2：mc_att_control 里有角速率环。

v1.15 之后没有了，角速率环在 mc_rate_control。

误区 3：改参数要重新编译。

不用。参数写进 Flash 就生效（除非参数本身标了需要对 reboot）。但加新参数需要重新编译（因为参数是代码生成的）。

误区 4：rcS 是 PX4 的启动脚本，改它要重新编译。

前半句对，后半句也对------但有替代方案。SD 卡上的 extras.txt 可以追加启动命令，不用编译。

误区 5：PID 调不好就要换算法。

99% 的情况是机械问题（振动、重心、桨）、校准问题（磁力计没校好）或者传感器问题。在动参数之前，先把机械和校准解决掉。

误区 6：EKF 参数可以随便调"优化性能"。

EKF2_GYR_NOISE / EKF2_ACC_NOISE 这些是物理参数（器件噪声水平），不是"性能旋钮"。乱调等于骗滤波器，短期内可能"看起来稳"，长期一定出事。

误区 7：多旋翼不需要空速管。

对纯悬停作业是对的，但一旦要飞高速航线（>15 m/s），空速对位置控制的效果提升很明显。PX4 支持多旋翼装空速管（SENS_EN_... + ASPD_... 参数）。

误区 8：日志只用来"出事了看看"。

日志的价值远不止事故调查。它是你改参数、验证算法、做系统辨识的唯一数据源。 我建议养成习惯：每次飞行后都看一眼关键曲线，即使飞得很好。你会发现很多"看起来没问题但已经在退化"的迹象。

误区 9：main 分支的代码就是"最新最好的"。

不。main 上有正在开发的东西，可能有回归。生产用 release/1.x。

误区 10：PX4 的代码量太大，一个人搞不懂。

我第一次打开 PX4 也是这个感觉。但你不需要懂全部。选一个方向（姿态控制、EKF、MAVLink、ROS 2 集成、驱动开发、仿真），往深里挖一年，你就是这个方向上的专家了。PX4 的模块化设计就是为了让你能"只懂一块"。

17.3 怎么跟上 PX4 的变化

几个有效的方法：

1. 订阅 PX4 的 GitHub Release （Watch → Releases only）。每个版本的 changelog 会列出破坏性变更。
2. 关注 docs/ 目录的变化。文档在哪改，说明哪在动。
3. 定期看 msg/ 目录的 diff。消息变了，接口就变了。
4. 读 PR 。PX4 的 PR 讨论质量很高，尤其是那些标了 breaking change 的。
5. 加入 PX4 的讨论渠道（Discord/Slack、开发者月会）。很多设计决策的理由只有在讨论里才看得到。

如果要长期做 PX4 开发，我建议的做法是"跟着一个 release 分支走，同时盯着 main 的 CHANGELOG"。 不要直接跟 main（会疲于奔命），也不要锁死在一个老版本（会失去生态支持）。

---

第 18 章 学习路线图

最后给你一条可执行的路线。我按"从入门到精通"分四个阶段，每个阶段都有明确的产出。

阶段一：用起来（1--2 周）

目标：能让一架飞机安全地飞起来。

• 装好工具链（强烈建议 Docker 或 WSL2）
• 编译一次 SITL：make px4_sitl gz_x500
• 在 SITL 里完成一次自动任务飞行
• 学会用 pxh> 里的 8 个核心命令：uorb top、listener、param show -c、commander check、work_queue status、perf、top、dmesg
• 用 QGC 做一次完整的传感器校准 + 遥控校准
• 飞一次真机，下载日志，用 Flight Review 看一遍

产出：能独立完成"装机 → 校准 → 首飞 → 日志复盘"。

阶段二：看明白（1--2 个月）

目标：能读懂核心源码，能定位常见问题。

• 完整读一遍 rcS + rc.mc_apps，画出启动流程图
• 读懂 uORB：读 platforms/common/uORB/uORBDeviceNode.cpp，能说清 5 种订阅方式的选择依据
• 读懂工作队列：读 WorkQueueManager.hpp，能背出主要队列的优先级
• 完整跟一遍 mc_pos_control → mc_att_control → mc_rate_control → control_allocator 的数据流
• 读 ekf2 的 aid_sources/ 目录，理解每个融合源的作用
• 读 control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp，理解效能矩阵
• 用 PlotJuggler 分析 3 个日志，每次找出至少一个问题

产出：能对着日志说清"哪个模块的哪一步出了问题"。

阶段三：改得动（3--6 个月）

目标：能独立开发模块、调参、定位疑难问题。

• 照第 15 章写一个自己的模块，跑在 SITL 和真机上
• 给一个现有模块加一个功能（比如给 mc_pos_control 加一个新的模式）
• 给某条 uORB 消息加字段，跑通整条链路（含 ROS 2 侧）
• 完整跑通 ROS 2 Offboard：uxrce_dds_client + px4_ros_com
• 用 px4-ros2-interface-lib 写一个自定义飞行模式
• 独立完成一次完整的 PID 调参（从 FFT 分析到参数填充到验证）
• 做一次日志回放（replay），验证你改的估计器逻辑

产出：能独立完成一个中小型定制开发任务。

阶段四：精通（1 年以上）

目标：能改架构、能吃透算法、能解决别人解决不了的问题。

• 读透 EKF2 的全部源码 + python/ 参考实现，能推导状态转移矩阵
• 移植一块新板子（从零到能飞）
• 给一个机型设计并验证新的控制分配策略
• 深入一个方向并产出：EKF 改进、新控制算法、新传感器融合、仿真精度提升......
• 参与 PX4 社区：提 issue、发 PR、参与设计讨论

产出：成为某个方向上的"那个人"。

---

附录 A：术语速查

术语	全称/含义	备注
uORB	micro Object Request Broker	PX4 的发布订阅消息总线
HIL	Hardware In the Loop	硬件在环仿真
SITL	Software In The Loop	软件在环仿真
EKF2	Extended Kalman Filter 2	主力状态估计器
ECL	Estimation and Control Library	历史上的估计与控制库名称
TECS	Total Energy Control System	固定翼纵向控制
L1	L1 制导	固定翼横向制导（老）
NPFG	Nonlinear Path Following Guidance	非线性路径跟随制导（新）
MAVLink	Micro Air Vehicle Link	通信协议
DDS	Data Distribution Service	ROS 2 的通信中间件
uXRCE-DDS	eProsima Micro XRCE-DDS	PX4 用的 DDS 桥
ULog	-	PX4 日志格式
airframe	机型配置	由 SYS_AUTOSTART 索引
BSP	Board Support Package	板级支持包
NSH	NuttX Shell	操作系统 shell
pxh	PX4 Shell	PX4 的命令行
FMU	Flight Management Unit	主控芯片
IO	Input/Output 协处理器	老 Pixhawk 的辅助芯片
ESC	Electronic Speed Controller	电调
DShot	Digital Shot	数字电调协议
DroneCAN	前身 UAVCAN v0	CAN 设备协议
Cyphal	前身 UAVCAN v1	新一代 CAN 协议
Offboard	外部控制模式	由伴飞计算机控制

附录 B：常用命令速查

# ---------- 系统状态 ----------
top                          # 任务列表（NuttX）
free                         # 内存
dmesg                        # 内核日志
work_queue status            # 工作队列状态（含栈使用）
perf                         # 性能计数
ver all                      # 版本信息
ps                           # 进程/任务列表

# ---------- uORB ----------
uorb status                  # 所有 topic
uorb top                     # 实时频率
uorb top -1                  # 打印一次
listener <topic>             # 看 topic 内容
listener <topic> 5           # 打印 5 条

# ---------- 参数 ----------
param show                   # 所有参数
param show -c                # 只看改动过的（**最常用**）
param show MC_*              # 通配符
param set <name> <value>
param get <name>
param reset <name>
param export /fs/microsd/params.txt
param import /fs/microsd/params.txt
param save

# ---------- 模块 ----------
<module> start|stop|status
commander check              # 解锁前检查
commander arm / disarm
mc_att_control status
ekf2 status
sensors status
control_allocator status
pwm_out status
mavlink status
logger status

# ---------- 故障排查 ----------
hardfault_log info           # 硬件异常记录
hardfault_log commit
events                       # 事件历史（**很有用**）
task_watchdog status            # 看门狗
listener estimator_status_flags # EKF 状态标志
listener health_report       # 健康检查结果
listener sensors_status_imu  # 传感器健康度

# ---------- 输出测试（小心！会转电机）----------
actuator_test 0 0.1
pwm_out status

# ---------- 工具（宿主机）----------
python3 Tools/px4.py /dev/ttyACM0            # 串口 shell
python3 Tools/mavlink_shell.py udp:0.0.0.0:14550  # MAVLink shell
python3 Tools/mavlink_shell.py /dev/ttyACM0
ulog_info log.ulg                            # 日志摘要
ulog_params log.ulg                          # 日志里的参数
ulog_messages log.ulg                        # 日志里的告警
plotjuggler log.ulg                          # 交互分析

附录 C：核心话题速查

传感器原始层

sensor_gyro / sensor_accel / sensor_mag / sensor_baro / sensor_gps
sensor_combined                 老的综合（逐步废弃）
sensor_optical_flow / distance_sensor / airspeed

整合层（sensors 模块输出）

vehicle_angular_velocity        角速度 + 角加速度
vehicle_acceleration            加速度
vehicle_imu[0..n]               单个 IMU 的整合结果
vehicle_magnetometer
vehicle_air_data                气压高度、温度
vehicle_airspeed
vehicle_gps_position
vehicle_optical_flow / vehicle_optical_flow_vel
sensors_status_imu / sensors_status_gnss / sensors_status

估计层（EKF2 输出）

vehicle_attitude                姿态（四元数）
vehicle_local_position          NED 本地位置/速度
vehicle_global_position         WGS84 全局位置
vehicle_odometry                标准里程计
vehicle_land_detected           着陆检测
estimator_status                估计器状态
estimator_status_flags          **状态标志（诊断核心）**
estimator_innovations           新息
estimator_states                状态与协方差
estimator_selector_status       多实例选择器状态
estimator_aid_source1d/2d/3d    各辅助源的状态
ekf2_timestamps                 EKF 时间戳信息

控制层

manual_control_setpoint         手动输入
trajectory_setpoint             轨迹 setpoint
position_setpoint_triplet       位置 setpoint（老，仍广泛使用）
vehicle_attitude_setpoint       姿态 setpoint
vehicle_rates_setpoint          角速率 setpoint
vehicle_torque_setpoint         力矩 setpoint
vehicle_thrust_setpoint         推力 setpoint
vehicle_torque_setpoint_virtual_mc / _fw    VTOL 虚拟 setpoint
actuator_motors / actuator_servos          执行器指令
actuator_outputs / actuator_outputs_sim    输出（用于日志）
control_allocator_status        分配器状态

状态与决策

vehicle_status                  总体状态（nav_state、arming_state、vehicle_type）
vehicle_control_mode            控制模式标志
vehicle_command / vehicle_command_ack   命令与应答
vehicle_local_position_setpoint
failsafe_flags                  失控保护标志
health_report                   健康报告
navigator_status                导航状态
position_controller_status      位置控制状态
rate_ctrl_status                角速率控制状态
mc_hover_thrust / hover_thrust_estimate
flight_phase_estimation         飞行阶段
rtl_status / rtls_status
mission_result
geofence_result / geofence_status

系统与诊断

parameter_update                参数更新通知（**几乎所有模块都订**）
cpuload / task_stack_info       负载与栈
log_message / logger_status
event / mavlink_log
power_monitor / battery_status / system_power
esc_status / esc_report
telemetry_status
timesync_status
input_rc / rc_channels

附录 D：参考资源

官方（按推荐顺序）

1. 文档站 ：https://docs.px4.io ------ 主文档
2. 开发者指南 ：docs.px4.io/main/en/development/
3. 模块参考 ：docs.px4.io/main/en/modules/modules_main.html
4. uORB 消息参考 ：docs.px4.io/main/en/msg_docs/
5. 参数参考 ：docs.px4.io/main/en/advanced_config/parameter_reference.html
6. 仓库内的文档源码 ：PX4-Autopilot/docs/en/（比网页好搜）
7. User Guide 源码 ：github.com/PX4/PX4-user_guide

源码必读的几个文件

文件	为什么读
ROMFS/px4fmu_common/init.d/rcS	系统的运行时地图
ROMFS/px4fmu_common/init.d/rc.mc_apps	多旋翼到底跑了哪些模块
platforms/common/include/px4_platform_common/module.h	模块框架
platforms/common/include/px4_platform_common/tasks.h	优先级体系
platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp	工作队列定义
platforms/common/uORB/uORBDeviceNode.cpp	uORB 的数据结构
docs/en/concept/architecture.md	官方架构概览
docs/en/middleware/uorb.md	uORB 完整说明
src/modules/ekf2/EKF/python/	EKF 的 Python 参考实现
src/lib/control_allocation/control_allocation/	控制分配算法

工具

工具	用途
QGroundControl	地面站
MAVSDK	高层 API（Python/C++/Java/JS）
PlotJuggler	日志与实时数据可视化
Flight Review	在线日志健康检查
pyulog	ULog 解析
Tools/mavlink_shell.py	MAVLink shell
Tools/px4.py	串口终端
Tools/ecl_ekf/	EKF 分析脚本
Gazebo (gz)	主推仿真器
jMAVSim	轻量仿真器

社区

• PX4 Discuss（论坛）
• PX4 Discord / Slack
• GitHub Issues / PRs（读 PR 讨论是学设计决策的最佳途径）
• 开发者月会（有录像）

---

最后

写到这里，报告已经很长了，但 PX4 的内容远不止这些。我刻意没写的部分包括：具体的 PID 调参实战、各机型的气动细节、RTK/PPK 的完整流程、视觉/VIO 的深度集成、系统辨识方法、以及 PX4 在商业产品里的落地经验。这些都是可以单独成篇的题目。

如果你读完这份报告，能建立起这样一张脑内地图------

知道系统分成中间件和飞栈两层；知道启动靠 rcS 脚本；知道模块之间靠 uORB 松耦合；知道控制器是四层级联；知道参数、日志、调试各有一整套工具；知道遇到问题该敲哪条命令、看哪个 topic、去哪个目录翻代码。

------那么这份报告的目的就达到了。

剩下的，交给你的飞机和你的日志。PX4 这东西，看一百页文档不如摔一架飞机学得多。（当然，最好是摔在仿真里。）

祝飞得稳。

---