基于 PX4 的全仓库架构、无人机算法解析与开发学习规划

阅读对象：希望从零理解无人机，并逐步设计一套具备状态估计、稳定飞行、自动任务、通信、故障处理和验证体系的飞控系统的开发者。

阅读与公式显示说明

本文所有公式使用普通 text 代码块，UTF-8 编码，不需要 MathJax、KaTeX 或 LaTeX 插件；矩阵也用文本排版。可直接在普通 Markdown 阅读器或文本编辑器中阅读。

写法	含义
`*`	标量乘法或矩阵乘法，按变量类型区分
`elem_mul(a, b)`	两个向量逐元素相乘
`^2`、`^T`	平方、矩阵或向量转置
`dX/dt`	X 对时间的导数
`~=`、`<=`	近似相等、小于或等于
`norm(v)`	向量的欧氏长度
`cross(a, b)`	三维向量叉积
`quat_mul(a, b)`	按正文约定的四元数乘法
`inverse(A)`、`pinv(A)`	矩阵逆、Moore-Penrose 伪逆
`sum_i(...)`	对下标 i 求和
`omega`、`Omega`	本文通常分别指机体角速度、旋翼转速
`tau`、`theta`、`phi`	力矩、倾角、滚转角；以对应章节定义为准
`_sp`、`_ff`、`_pred`	目标值、前馈值、预测值

同一字母在不同模型可能有不同含义：动力学的 R 是旋转矩阵，EKF 公式的 R 是观测噪声协方差；动力学的 I/J、控制积分项和单位矩阵也应按上下文区分。纯文本公式是数学记号，不是直接可执行的 Python 或 C++。

一、先理解目标：什么是完整无人机系统
二、当前仓库具有哪些能力
三、先做整机需求与硬件设计
[四、PX4 软件架构、启动与实时调度](#四、PX4 软件架构、启动与实时调度)
五、坐标系、时间与动力学
六、传感器、校准与信号处理
[七、状态估计与 EKF2](#七、状态估计与 EKF2)
[八、光流、测距、视觉里程计与无 GPS 定位](#八、光流、测距、视觉里程计与无 GPS 定位)
九、模式、状态机与指令权限
十、航线、航点、轨迹与自主规划
十一、位置和速度控制
十二、姿态和角速度控制
十三、电机分配与执行器输出
[十四、通信、遥控、数传与 ROS 2](#十四、通信、遥控、数传与 ROS 2)
十五、异常处理与系统可靠性
十六、如何组织自己的算法和代码
十七、仿真、测试、日志和调参
十八、从零到完整系统的实施路线
十九、逐步学习路线和源码阅读顺序
[二十、固定翼、VTOL 与高级算法扩展](#二十、固定翼、VTOL 与高级算法扩展)
二十一、设计交付物、问题排查和参考资料
二十二、全仓库架构总览与覆盖边界
二十三、操作系统、板级支持与运行架构
二十四、全部一级驱动目录与设备架构
二十五、基础服务与全部一级算法库
二十六、全部一级功能模块与多机型控制链
二十七、完整导航任务、业务载荷与外部模式
二十八、通信子协议、跨处理器通信与外部生态
二十九、健康检查、冗余、诊断与维护架构
三十、构建生成体系与全部系统命令
三十一、仿真后端、测试、回放与持续集成
三十二、跨模块数据流与实际开发追踪方法
三十三、架构覆盖审计与后续维护

一、先理解目标：什么是完整无人机系统

"能转电机""能稳定悬停""能自动执行航线"和"能长期可靠运行"是四个不同等级。

一套接近 PX4 完整程度的无人机系统，至少需要以下闭环：

层次	要回答的问题	主要输出
整机与动力	机体是否有足够推力、刚度、电量和散热能力	质量、惯量、动力曲线、能源预算
驱动与采集	当前测到了什么，测量是否及时可信	带时间戳和质量标志的测量
状态估计	当前在哪里、朝向哪里、移动多快	姿态、速度、位置、偏置、可信度
任务与规划	要去哪里，怎样走，什么时候到	路径、连续轨迹、航向和约束
控制	如何让实际状态跟踪目标	推力、姿态、角速度和力矩目标
分配与执行	各电机和舵机应该输出多少	电机归一化推力、PWM/DShot/CAN 输出
状态与故障管理	当前允许做什么，异常后如何降级	解锁状态、模式、限制和处置动作
通信与运维	人和其他计算机如何与飞机交互	遥测、任务、指令、参数、日志
验证与配置	如何证明这个版本能用于这架飞机	测试报告、参数基线、版本和追溯记录

最重要的设计思想：规划决定目标，估计提供反馈，控制产生作用，分配执行作用，状态机决定权限。

1.1 推荐的总体技术路线

采用两条互相促进的路线：

工程线：先使用 PX4 原有闭环，在 SITL 中完成起飞、悬停、航点、返航和故障处置，再逐个替换算法。
学习线：在独立仿真程序里实现小型四旋翼动力学、PID、姿态解算、简化 EKF 和分配器，理解每个公式。

不要同时自研飞控板、驱动、EKF、控制器和规划器。这样出现振荡或漂移时，很难区分是结构、坐标、延迟、估计还是控制的问题。

第一版建议复用 PX4 的驱动、EKF2、基础控制和失效保护，把自主能力放在伴随计算机；掌握系统后，再决定哪些模块值得自研。

1.2 一条典型的多旋翼自动飞行链路

text 复制代码

传感器 -> 驱动/校准/滤波/时间对齐 -> EKF2 -> 状态与有效性
                              |                |
                              |                +--> 位置控制/姿态控制
                              +--> 角速度反馈

地面站任务 -> Navigator -> FlightTask -> 轨迹目标 --+
伴随计算机规划器 -> Offboard -----------------------+--> 位置/速度控制
                                                         |
                                                         v
姿态目标 -> 姿态控制 -> 角速度目标 -> 角速度控制 -> 力矩/推力目标
                                                       |
                                                       v
                                              Control Allocator
                                                       |
                                                       v
                                              电机/舵机目标
                                                       |
                                                       v
                                              输出映射与驱动
                                                       |
                                                       v
                                              飞机运动 -> 传感器

Commander / 健康检查 / Failsafe：管理模式、控制权限、约束与故障处置。

图是逻辑关系，不表示这些模块处在同一个线程，也不表示所有机型都使用完全相同的链路。

二、当前仓库具有哪些能力

2.1 功能版图

能力	当前仓库入口	主要边界
IMU、磁力计、气压、GNSS、测距、光流驱动	`src/drivers/`、`src/modules/sensors/`	具体板卡不一定编译全部驱动
多传感器融合	`src/modules/ekf2/`	融合效果取决于标定、安装、时间和可观测性
多旋翼位置、姿态、角速度闭环	`mc_pos_control`、`mc_att_control`、`mc_rate_control`	高速飞行和极端机型需要专门验证
飞行任务切换与平滑目标生成	`src/modules/flight_mode_manager/`	不是通用三维环境路径搜索器
航点、起降、盘旋、返航、围栏	`src/modules/navigator/`	返航不等于已经具备三维环境避障
执行器几何和控制分配	`src/modules/control_allocator/`	是否容错取决于构型和剩余控制能力
遥控输入和手动指令处理	`rc_update`、`manual_control`	遥控协议和设备支持需检查配置
MAVLink 通信与任务协议	`src/modules/mavlink/`	协议不会自动保证无线链路质量
ROS 2 通信桥	`src/modules/uxrce_dds_client/`	话题要配置，消息版本和 QoS 要匹配
其他通信架构	`src/modules/zenoh/`	目录存在不代表目标固件默认启用
解锁检查、故障管理	`src/modules/commander/`	处置动作需要针对任务和机型配置
飞行日志与回放	`logger`、`replay`	需要记录足够的中间量才能定位问题
自动调参与悬停推力估计	`mc_autotune_attitude_control`、`mc_hover_thrust_estimator`	不能替代机械检查和基本稳定性验证
固定翼控制	`fw_mode_manager`、`fw_lateral_longitudinal_control`、`fw_att_control`、`fw_rate_control`	当前结构与一些旧教程不同
VTOL 控制	`src/modules/vtol_att_control/`	包含过渡与多类执行器耦合问题
其他载具与实验算法	rover、uuv、airship、spacecraft、`mc_nn_control`、`mc_raptor`	不作为本文四旋翼基础链路；存在不代表普遍成熟可用

2.2 不应误认为"PX4 已自动解决"的事情

以下通常属于整机集成或伴随计算机开发：

基于相机或激光雷达进行三维 SLAM 建图。
在地图中执行 A*、D*、RRT* 等全局搜索。
对动态障碍物预测并在线生成无碰撞轨迹。
计算测绘覆盖路线、作业效率、相机重叠率和多机任务分配。
根据真实风场、载荷、电池老化建立能耗和任务成功率模型。
实现业务载荷、识别、巡检和数据管理。
保证特定硬件平台的时延、冗余和产品可靠性。

仓库中确实存在 src/lib/collision_prevention/，它提供基于障碍距离等输入的运动限制能力。它与"全局地图搜索 + 局部轨迹优化 + 动态避障"不是同一层次。使用前应检查它在哪种 FlightTask、模式和参数配置下生效。

2.3 版本阅读原则

本地 git describe 只返回提交短哈希，本文不把它强行称为某个正式发行版。尤其注意：

当前采用 control_allocator，不能照搬以旧式 mixer 为核心的完整控制链解释。
部分消息位于 msg/versioned/，不是全部都在 msg/ 顶层。
当前 EKF 生成状态包含地形状态，不能把旧教程的状态数量直接当成现在的事实。
模块源码存在、板卡编译启用、启动脚本启动、运行时参数激活，是四个不同条件。

三、先做整机需求与硬件设计

3.1 先写需求表，再选算法

建议先确定一份可测量的目标规格：

项目	必须明确的内容	对算法的影响
用途	室内科研、航拍、巡检、测绘等	是否需要 GNSS、视觉、避障、载荷联动
构型	X 四旋翼、六旋翼、固定翼、VTOL	动力学、分配矩阵和故障策略不同
起飞质量与载荷变化	空载、满载、重心范围	悬停推力、惯量、增益和能源预算
飞行环境	室内外、光照、风、温度、雨尘	传感器选择、控制裕度和降级路径
运动指标	最大速度、爬升率、加速度、倾角	轨迹约束与动力能力是否一致
定位指标	精度、可用率、允许漂移	GNSS/RTK、光流、VIO 选型
任务指标	航程、续航、返航余量	航线规划与能源管理
通信指标	距离、延迟、丢包、带宽	链路和 Offboard 架构选择
维护指标	日志、升级、校准、版本回滚	软件工程和配置管理

数值应来自任务需求和试验，不要把某架小四旋翼的参数复制到另一架飞机。

3.2 动力与能源的第一轮计算

悬停时总推力近似为：

text 复制代码

T_hover = m * g
T_motor_hover ~= m * g / N

其中 N 是电机数，均分只适用于重心居中且对称的悬停。

总最大推力必须高于重量，并为姿态修正、倾斜飞行、低电压和风扰留出余量。适当余量取决于用途，不应把某个固定推重比当成所有无人机的硬性标准。

水平加速需要倾斜推力方向。忽略阻力、保持高度时：

text 复制代码

a_xy ~= g * tan(theta)
T ~= m * g / cos(theta)

这说明：大倾角不仅需要姿态能力，也会消耗更多总推力。

能源粗估：

text 复制代码

E_nominal_Wh = V_nominal * C_Ah
t_min ~= 60 * E_usable_Wh / P_avg_W

可用能量需要扣除返航余量、低温和老化影响。电机推力、电流、转速、效率应在实际桨、电压和安装条件下测量。

3.3 建议的硬件分层

飞控 MCU：执行传感器采集、状态估计、实时控制、模式与故障处理。
伴随计算机：运行视觉、地图、规划、任务业务和 ROS 2。
动力系统：电池、电源模块、ESC、电机、桨、供电线束。
导航系统：IMU、磁力计、气压计、GNSS；室内再加光流/测距或 VIO。
通信系统：遥控、低速遥测、伴随计算机链路、视频链路。
地面站：任务配置、参数管理、状态显示和日志分析。

3.4 硬件问题会直接变成算法问题

桨不平衡、机架共振 → IMU 噪声与角速度环振荡。
IMU 软安装过度 → 测量增加相位滞后，可能恶化闭环。
电源跌落 → 飞控重启或传感器重置。
大电流线靠近磁力计 → 航向估计随油门变化。
光流与测距视场错位 → 距离尺度错误。
相机安装外参变化 → 视觉状态突然偏移。
飞控安装角、轴向或电机方向配置错误 → 正反馈甚至起飞翻转。

因此，机械、电气、估计与控制必须共同设计。

四、PX4 软件架构、启动与实时调度

4.1 仓库地图

路径	作用	首次阅读重点
`boards/`	目标板配置与构建选项	目标板启用了哪些模块
`platforms/`	NuttX/POSIX 等平台适配、调度、uORB	运行环境和实时执行机制
`src/drivers/`	传感器、总线和执行器驱动	数据从硬件进入软件的入口
`src/modules/`	估计、控制、通信、任务等模块	业务功能的主要实现
`src/lib/`	可复用数学与算法库	尽量纯粹的控制和轨迹算法
`msg/`、`msg/versioned/`	消息定义	数据单位、坐标、有效值约定
`ROMFS/px4fmu_common/init.d/`	启动脚本和机型配置	哪些模块被启动、默认参数是什么
`Tools/`	构建、仿真和开发工具	环境、仿真和测试工作流
`test/`、`integrationtests/`	测试基础设施	回归和集成验证
`docs/en/`、`docs/zh/`	随仓库维护的文档	对照源码阅读，翻译可能滞后

4.2 启动链应该怎样读

先读 rcS，再读 rc.vehicle_setup 和 rc.mc_apps。

概念链路：

text 复制代码

板级启动 / 操作系统
  → 参数和硬件环境初始化
  → 驱动与公共服务
  → 机型与传感器配置
  → 对应机型的控制与任务模块
  → 通信、日志和运行状态管理

当前 rc.mc_apps 可以直接看到以下启动项：

text 复制代码

control_allocator
mc_rate_control
mc_att_control
mc_hover_thrust_estimator
flight_mode_manager
mc_pos_control
land_detector（multicopter）

自动调参、神经网络控制等还有条件启动逻辑。启动先后不等于控制数据流方向；消息是否到达和模式是否使能才决定运行行为。

4.3 uORB：内部模块的数据总线

入口：<platforms/common/uORB>。

uORB 是机内发布/订阅机制。例如：

EKF 发布 vehicle_attitude。
姿态控制订阅该消息，发布 vehicle_rates_setpoint。
角速度控制发布 vehicle_torque_setpoint 和 vehicle_thrust_setpoint。
控制分配器订阅它们，发布 actuator_motors。

它并不等于 MAVLink，也不是无线数传协议。ROS 2 桥负责把配置过的部分机内话题映射到外部。

必须理解的细节：

消息有类型、单位、坐标和有效性约定。
不要假设每个订阅者都会处理每一帧，队列深度和读取节奏会影响行为。
同名多实例常用于冗余传感器或多估计器。
同一个控制设定值若被多个模块同时发布，可能发生所有权冲突。
timestamp 与 timestamp_sample 分别表达发布时间/处理时间和采样时间等语义，具体以消息注释为准。
收到消息不代表消息仍然新鲜，必须检查时间和质量。

4.4 模块外壳与纯算法分离

例如位置控制包含两层：

MulticopterPositionControl.cpp：消息读写、模式、起飞管理、滤波、参数、时间和异常兜底。
PositionControl.cpp：位置/速度误差到推力目标的计算。

这种划分值得直接学习：算法可以用人工输入做单元测试，平台外壳负责真实世界的时间、状态和通信问题。

4.5 Work Queue 与实时执行

入口：ScheduledWorkItem.hpp。

很多模块通过工作队列运行，并用 SubscriptionCallbackWorkItem 在相关数据更新时获得执行机会。当前源码中：

模块	关键触发消息
角速度控制	`vehicle_angular_velocity`
姿态控制	`vehicle_attitude`
位置控制	`vehicle_local_position`
FlightModeManager	`vehicle_local_position`
ControlAllocator	`vehicle_torque_setpoint`

部分模块还设置备用定时调度。因此"PX4 所有控制都是固定 1 kHz while 循环"是错误理解。

自研时遵守以下规则：

控制回调里不进行阻塞串口读、等待网络、长时间文件操作。
避免每帧动态分配内存；固定大小向量、缓冲区提前分配。
dt 使用采样时间计算，并对异常值限幅或拒绝更新。
对执行时间、队列延迟和样本年龄分别测量。
高速控制与低速规划分开，伴随计算机暂停不能阻塞角速度控制。

4.6 建议的初始时序预算

下表是自研系统的起始设计范围，不是本仓库所有板卡的实际固定频率。

环节	建议量级	主要约束
IMU 采样	1--8 kHz 或设备支持范围	总线吞吐、噪声、抗混叠
角速度控制	250--1000 Hz	延迟、ESC、滤波相位
姿态控制	100--250 Hz	比角速度闭环慢，避免级联耦合
状态估计更新	100--250 Hz 起步	实际内部预测/发布节奏可能不同
位置控制	50--100 Hz	状态更新、轨迹连续性
局部轨迹生成/跟踪设定值	20--100 Hz	感知延迟和飞行速度
全局规划/业务决策	1--10 Hz 或按事件	地图规模和任务变化
地面遥测	1--50 Hz，分消息配置	无线带宽和重要性

应通过目标板上的实际测量确定预算。频率高不等于延迟低，低通滤波、线程等待、串口排队都可能增加相位滞后。

五、坐标系、时间与动力学

5.1 必须先固定的约定

PX4 常见约定：

本地导航系：NED，X 北、Y 东、Z 下。
机体系：FRD，X 前、Y 右、Z 下。
本地位置：米；速度：m/s；加速度：m/s²。
内部角度通常用弧度，角速度通常用 rad/s；参数、协议字段可能另有单位。
四元数顺序、旋转方向以消息和库定义为准，不能仅看变量名。
本地 z=-5 m 表示相对本地原点向上 5 m，并不必然表示离地 5 m。

ROS 常见 ENU/FLU 与 NED/FRD 不同。对于向量：

text 复制代码

[x, y, z]_NED = [y, x, -z]_ENU

text 复制代码

[x, y, z]_FRD = [x, -y, -z]_FLU

姿态要做两端坐标基变换，不能只交换四元数的几个分量。协方差同样应变换：P_new = R P_old Rᵀ；六维量使用对应的块矩阵。

5.2 高度有多种含义

必须区分：

海拔/平均海平面高度。
GNSS 椭球高。
相对起飞点高度。
本地 NED 原点高度。
离地高度 AGL。
测距传感器沿视线测得的斜距。

航点、地形、返航高度、相机规划若混用这些量，飞机可能执行完全错误的高度指令。

5.3 四旋翼教学动力学

令 R 把机体系向量变换到 NED，T≥0 为沿机体负 Z 方向的总推力大小：

text 复制代码

dp/dt = v

text 复制代码

m * dv/dt = m * g * e3 + R * [0, 0, -T]^T + f_drag + f_disturbance

text 复制代码

J * domega/dt = tau - cross(omega, J * omega) + tau_disturbance

text 复制代码

dq/dt = 0.5 * quat_mul(q, [0, omega_x, omega_y, omega_z])

最后一个式子采用"机体系到世界系"的四元数约定；实现必须与所用四元数库一致。

电机/桨可先用：

text 复制代码

T_i = k_T * Omega_i^2
Q_i = s_i * k_Q * Omega_i^2

再增加一阶电机动态、转速变化限制、电池电压变化和气动阻力。先把简单模型验证正确，再提高保真度。

5.4 时间是状态估计和控制的一部分

至少记录：传感器采样时刻、数据到达时刻、估计输出时刻、控制输出时刻。

例如视觉结果在 80 ms 后到达，不能假装它是"现在的位置"。应明确曝光/积分时刻，建立时钟同步或时差估计，并在融合中考虑延迟。

验收时测量：

text 复制代码

L_sensor_to_actuator = t_actuator - t_sensor_sample

同时看平均、P95/P99、最大值和抖动。平均延迟正常但偶发卡顿严重，仍然可能破坏飞行。

六、传感器、校准与信号处理

6.1 数据进入 PX4 的流程

text 复制代码

硬件寄存器/数据帧
 → 驱动解析与硬件时间戳
 → 传感器消息
 → 安装方向、校准、选择、积分或滤波
 → vehicle_imu / vehicle_angular_velocity / vehicle_air_data 等
 → EKF、控制器和健康检查

重点源码：

sensors.cpp：传感器模块组织。
VehicleIMU.cpp：IMU 数据组合与积分相关处理。
VehicleAngularVelocity.cpp：角速度处理链。
VehicleAirData.cpp：气压相关数据处理。
voted_sensors_update.cpp：传感器选择相关逻辑。

6.2 IMU：为什么它最重要

陀螺仪测角速度；加速度计测比力，而不是直接测世界坐标加速度。

text 复制代码

omega_measured = omega + b_g + n_g

text 复制代码

f_measured = R^T * (a - g_vector) + b_a + n_a

静止时加速度计不读零。若把其数值直接积分成位置而不处理姿态、重力和偏置，位置会快速发散。

校准项目：零偏、比例、轴间误差、安装旋转、温度影响。部分误差靠出厂/现场校准补偿，剩余缓慢偏置由估计器追踪。

6.3 滤波怎样设计

抗混叠应在适当的采样与降采样环节解决。
低通减少高频噪声，但会增加控制延迟。
陷波针对共振或电机相关频段，不能无限叠加。
角加速度或 D 项对高频噪声敏感，需要与角速度滤波一起设计。
动态陷波可参考转速遥测或频谱信息，仓库有 gyro_fft 相关模块。

调参顺序应先机械振动和传感器质量，再滤波，再控制增益。不能通过越来越重的滤波掩盖严重机械故障。

6.4 其他传感器的作用与限制

传感器	帮助估计什么	常见失效
磁力计	航向与磁场相关状态	电磁干扰、安装方向、局部磁异常
气压计	相对高度变化	桨流、天气、温度、机壳压力
GNSS	全局位置、速度、时间，特定设备可提供航向	遮挡、多径、跳变、更新慢
RTK	在有效改正和固定解条件下提高定位精度	固定解丢失、改正过期，不等于永远高精度
测距	离地/障碍距离	超量程、倾斜、低反射、玻璃、水面
光流	相对地面角运动，配合高度约束水平运动	无纹理、暗光、运动模糊、尺度错误
VIO	相对姿态、位置、速度	纹理丢失、外参、时间偏差、重定位跳变
空速计	固定翼相对气流速度	堵塞、结露、安装误差、低速精度差
电压电流	能源状态	标定错误、瞬时跌落、温度与老化

6.5 驱动验收不是"有数值"就结束

每个驱动至少检验：

单位和坐标是否正确。
时间戳与积分时间是否可靠。
是否有丢帧、FIFO 溢出、总线错误统计。
数据超时、设备拔出、重新连接后的行为。
静态噪声、动态量程、饱和剪切情况。
多实例和 device_id 是否稳定。
目标采样率下 CPU、总线占用与最长执行时间。

七、状态估计与 EKF2

7.1 估计器与控制器各自做什么

控制器需要真实状态，但传感器提供的是带噪声、延迟和偏置的测量。估计器负责利用动力学和多个测量推断状态与不确定性。

估计器回答"我现在可能在哪里"，控制器回答"为了到目标应该产生什么作用"。不能把位置漂移一律归因于 PID。

7.2 当前 EKF2 的代码分层

文件/目录	负责什么
EKF2.cpp	uORB 数据接入、参数、时间、核心 EKF 调用、结果发布
EKF2Selector.cpp	多估计器实例的健康评估与选择
EKF/ekf.cpp	初始化、预测与融合主过程
EKF/covariance.cpp	协方差预测与维护
EKF/control.cpp	融合来源与融合状态控制
EKF/aid_sources	GNSS、光流、外部视觉、气压等观测逻辑
EKF/output_predictor	从延迟融合结果向当前输出时刻预测
EKF/python/ekf_derivation	符号推导与自动生成的状态/观测相关代码

当前 Ekf::update() 可以看到 predictCovariance()、predictState() 和 controlFusionModes() 的调用，是阅读核心链的入口。

7.3 状态向量：不要死记旧版本"几维 EKF"

当前生成文件 state.h 明确给出：

状态	名义存储标量数	误差自由度
姿态四元数	4	3
速度	3	3
位置	3	3
陀螺仪偏置	3	3
加速度计偏置	3	3
地磁场状态 `mag_I`	3	3
机体磁场状态 `mag_B`	3	3
水平风速	2	2
地形	1	1
合计	25	24

四元数必须满足单位长度约束，姿态局部误差只有三个自由度。名义状态存储数与误差协方差维度不是一个概念。 并非所有状态在所有飞行条件下都可观测，也不意味着每个板卡启用所有融合来源。

7.4 EKF 的数学主线

以一般误差状态滤波框架理解：

预测：

text 复制代码

x_pred[k+1] = f(x_est[k], u[k], dt)

text 复制代码

P_pred[k+1] = F[k] * P[k] * F[k]^T + G[k] * Q[k] * G[k]^T

观测：

text 复制代码

r = z - h(x_pred)
S = H * P_pred * H^T + R

text 复制代码

K = P_pred * H^T * inverse(S)
delta_x = K * r

将误差注入名义状态，并更新协方差。教学实现可使用 Joseph 形式：

text 复制代码

P_updated = (I - K*H) * P_pred * (I - K*H)^T + K * R * K^T

姿态部分还要正确完成误差注入、四元数归一化及误差重置相关处理。本文公式用于理解，不声称 PX4 每个观测都直接按矩阵逆和 Joseph 形式逐行实现；源码使用了专门的推导与数值实现。

不同实现对 innovation 的符号定义可能相反。本文用 z-h(x)，阅读 PX4 时要同时检查状态修正的符号，不能单独复制一个表达式。

7.5 EKF 的难点在工程条件

延迟融合：将测量放在适当的历史时刻融合，而非简单按接收顺序处理。
输出预测：控制需要当前状态，不能直接使用明显滞后的融合状态。
观测门限：根据创新和创新方差判断异常测量。
启停融合：传感器恢复后，要满足条件才开始融合。
重置：位置、速度或航向需要重置时，下游控制目标要同步修正。
可观测性：无绝对水平位置源时，位置原点和长期漂移不能凭空消失。
多实例：冗余传感器和多 EKF 选择是系统工程，不是简单取平均。
数值稳定：协方差异常、接近零距离、四元数符号等都要处理。

7.6 关键输出与诊断

重点看：

vehicle_attitude：姿态及重置信息。
vehicle_local_position：位置、速度、有效标志、参考原点、重置信息。
vehicle_global_position：全局位置。
vehicle_odometry：里程计状态。
estimator_status_flags 与各类 estimator_aid_src_*：融合状态、观测、创新、方差和拒绝情况。

验收不能只看"轨迹看起来平滑"，还要看真值误差、创新统计、拒绝率、恢复时间和状态有效性。

7.7 自己实现估计器的顺序

静态陀螺零偏标定。
四元数积分和归一化。
加速度辅助的互补姿态估计，仅在适用动态条件下修正。
加入航向观测与干扰判断。
简化 IMU + GNSS 误差状态 EKF。
加入气压和测距，区分高度基准。
加入光流或 VIO，处理延迟和坐标。
加入观测拒绝、重置、超时、多源切换。
使用真实日志离线回放，对照 PX4 和真值评估。

学习滤波可以从小模型开始；工程第一版优先使用已验证的 EKF2。

八、光流、测距、视觉里程计与无 GPS 定位

8.1 光流究竟测到了什么

图像上的运动包括：相机平移、相机转动、场景自身运动、深度变化和测量噪声。

向下看的光流模块通常输出视场中的角位移。要估计真实水平运动，必须考虑：

积分时间。
相机/机体旋转造成的光流。
视线距离或地形高度。
传感器安装旋转和相对 IMU 的位置。
光照、纹理、质量与量程。

只买一个光流模块，不等于获得室内绝对定位，更不等于获得 SLAM。

8.2 当前 PX4 光流数据链

text 复制代码

PMW3901 / PAW3902 / PAA3905 / PX4Flow 等驱动
    → sensor_optical_flow
    → sensors/vehicle_optical_flow
    → vehicle_optical_flow
    → EKF2
    → EKF/aid_sources/optical_flow
    → 水平速度、位置和地形相关估计

源码入口：

8.3 消息字段必须正确理解

字段	当前含义	常见错误
`pixel_flow[2]`	积分角位移，单位 rad	把它当原始像素数量或直接当 rad/s
`delta_angle[3]`	同一积分期间的陀螺角增量	使用不同时间窗口或把角速度直接填进去
`integration_timespan_us`	累积测量时间，微秒	忘记换算为秒
`distance_m`	对应视场中心方向的距离信息	直接当海拔或无条件当垂直离地高度
`quality`	0--255 的质量信息	永远填最大值来绕过真实质量判断
`timestamp_sample`	采样时序信息	只用数据接收时间
最小/最大工作距离、最大流率	设备可用范围	超量程仍持续信任观测

原始传感器消息还有角增量和距离是否可用的标志；上层消息用 NaN 表示部分不可用量。实现时按当前消息定义逐项检查。

8.4 和源码对应的简化观测模型

当前 Ekf::predictFlow() 对传感器相对 IMU 的位置进行补偿，将速度转入机体系，并返回：

text 复制代码

flow_rate_x_pred ~= v_body_y / d
flow_rate_y_pred ~= -v_body_x / d

这里 d 对应模型中的视线距离，源码通过姿态和离地高度得到比例；它不是任意未经处理的测距值。

在平地、小倾角、已补偿转动、坐标完全一致时，才可直观写成：

text 复制代码

v_x ~= -d * flow_rate_y
v_y ~=  d * flow_rate_x

这只是解释式，完整融合还需要状态雅可比、噪声、门限、地形状态和传感器杆臂。

源码还有一个容易忽略的细节：内部 flow_gyro 约定为机体角速度的负值，杆臂速度的计算因此出现相应符号。不要用"减一下陀螺"替代对完整符号链的理解。

8.5 光流调通流程

无桨状态下检查设备方向、时间戳、积分时间和质量。
沿前后、左右方向平移，检查流方向是否和模型一致。
原地转动，检查旋转补偿后的虚假平移是否合理。
更改与地面距离，检查尺度是否随距离正确变化。
检查倾斜时测距与视场几何，不要把斜距直接当高度。
查看光流创新、拒绝情况、测距有效性和估计速度。
在仿真或受控环境中验证定点与刹停。
测试暗光、纯色地面、纹理突变、超高度范围与数据中断。
确认失效后进入预定模式，而不是继续认为位置可信。

8.6 光流和 VIO/SLAM 如何选择

方案	适合的问题	主要限制
光流 + 向下测距 + IMU	低空、地面纹理足够时的水平运动约束	漂移、光照、工作高度、地形假设
VIO	无 GNSS 环境的局部六自由度运动估计	算力、外参、同步、纹理和运动激励
SLAM	持续建图、重定位、地图导航	地图管理、闭环跳变、动态场景
GNSS/RTK + IMU	开阔室外全局导航	遮挡与多径、改正链路
多源组合	室内外切换与提高可用性	状态一致性和融合切换更复杂

8.7 外部视觉接入

阅读：src/modules/ekf2/EKF/aid_sources/external_vision/ 和 VehicleOdometry.msg。

基本接口：外部系统提供位置/速度/姿态、坐标标记、协方差、时间和重置信息，经适配进入 vehicle_visual_odometry。

必须设计：

相机、IMU、机体的外参。
外部时钟到飞控时间的同步。
局部原点、航向和重定位后的变换。
协方差与失效质量的表达。
视觉跟踪丢失后的退出与恢复条件。

不要同时向同一个估计器注入多个高度相关、却被错误当成相互独立的观测源，这会造成过度自信。

九、模式、状态机与指令权限

9.1 四个经常被混淆的概念

概念	例子	意义
解锁状态	已解锁/未解锁	是否允许正常动力输出
导航/飞行模式	手动、位置、任务、返航、Offboard	谁产生目标，当前如何飞
控制使能	位置、速度、姿态、角速度控制开关	哪些闭环参与
飞行阶段	地面、起飞、空中、降落	积分、推力和目标管理策略

模式切换不是简单改一个枚举。还要检查估计是否有效、输入是否新鲜、能否无突变交接、切换失败怎么处理。

9.2 源码责任划分

Commander.cpp：系统状态管理入口。
ModeManagement.cpp：模式管理。
HealthAndArmingChecks：健康和解锁条件。
failsafe：异常条件、优先级与动作。
FlightModeManager.cpp：选择并执行 FlightTask。
land_detector：落地状态判断。

9.3 FlightTask 的作用

同样是"操纵杆向前"，不同模式可以表示不同目标：

角速度模式：期望俯仰角速度。
姿态模式：期望俯仰姿态。
位置/速度类模式：期望水平速度或加速度。
自动任务：从航点生成轨迹。

FlightTask 把模式特有输入转换为统一的轨迹与约束。

当前 FlightModeManager::generateTrajectorySetpoint()：

准备空的 NaN 设定值和约束。
调用任务 updateInitialize()、update()。
成功时取得任务生成的轨迹与约束。
根据起飞状态处理重新激活等逻辑。
发布 trajectory_setpoint、vehicle_constraints。

失败时保留无效设定值，使后续控制模块能够走兜底路径；不是继续盲用上一个任意目标。

9.4 自研状态机的最小范围

建议至少包含：

text 复制代码

BOOT → SELF_CHECK → STANDBY → ARMED_GROUND → TAKEOFF → IN_AIR
                                                         ↓
                            STANDBY ← DISARM ← LANDED ← LANDING

异常处置应作为独立仲裁逻辑叠加，而不是为每个故障复制一整套飞行状态。

每次状态转移记录：触发原因、进入时间、所需条件、允许输出、退出条件、失败动作。对起飞、降落、返航、人工接管、Offboard 丢失逐个验证。

十、航线、航点、轨迹与自主规划

10.1 把"航线规划"分成五层

层次	输入	输出	典型算法/实现
任务规划	作业区域、相机、禁区、能源	有顺序的任务段	覆盖扫描、任务排序
全局路径	地图、起点、终点	无碰撞几何路径	A、Dijkstra、RRT
局部规划	当前状态、局部障碍、全局路径	短时可行运动	采样、优化、局部重规划
轨迹生成	路径、速度/加速度/jerk 限制	`p(t), v(t), a(t)`	S 曲线、多项式、样条
轨迹跟踪	连续目标、估计状态	姿态和推力	PX4 多旋翼级联控制

一串航点只是空间目标，不包含完整速度和动力学约束。无碰撞路径也不一定是飞机能跟踪的轨迹。

10.2 PX4 航点任务链路

text 复制代码

QGroundControl / MAVLink Mission 上传
 → MAVLink 任务处理与存储
 → Navigator 读取任务、检查可执行性、管理当前任务项
 → position_setpoint_triplet（上一点、当前点、下一点）
 → FlightTaskAuto 转换局部参考、决定运动类型
 → PositionSmoothing / VelocitySmoothing
 → trajectory_setpoint
 → mc_pos_control

重点源码：

FlightTaskAuto::_evaluatePositionSetpointTriplet() 中能看到经纬度转换到局部坐标、目标合法性检查和高度转换；update() 中能看到三点信息进入 generateSetpoints()。

10.3 当前源码也存在 Goto 目标平滑

位置控制模块包含 GotoControl。这提示你：当前版本的目标来源不应只按旧教程理解为"所有位置目标都从 Navigator 来"。阅读时应追踪具体消息来源及生效条件。

10.4 自己设计航点执行器

最小任务项建议包含：

text 复制代码

坐标及高度基准
目标速度和允许加速度
到达半径、是否需要停留
航向策略
载荷动作
超时与失败动作
任务标识和序号

到达判断不能只做 distance < radius。还要考虑垂直误差、速度、停留时间、是否飞越、轨迹是否已经经过目标区。

急转弯时需要提前减速，而不是到航点后突然切换位置目标。合理使用前一航点、当前航点和下一航点可以估计转弯需求。

10.5 从直线轨迹到平滑轨迹

教学第一步可使用五次多项式：

text 复制代码

p(t) = c0 + c1*t + c2*t^2 + c3*t^3 + c4*t^4 + c5*t^5

用起终点位置、速度、加速度共六个边界条件求系数。对于起终点速度与加速度为零，令 s=t/T：

text 复制代码

s = t / T
p(t) = p0 + (p1 - p0) * (10*s^3 - 15*s^4 + 6*s^5)

然后解析求导得到速度和加速度。

必须继续做约束检查：

text 复制代码

norm(v) <= v_max
norm(a) <= a_max
norm(j) <= j_max

还要检查推力、倾角、爬升率和障碍距离。五次曲线本身不会自动满足这些约束；可增大 T，也可使用带约束的轨迹生成器。

Minimum-jerk/minimum-snap、B 样条等可作为后续学习，不应误称为当前 PX4 多旋翼默认全部采用的轨迹算法。

10.6 覆盖航线：测绘和巡检的例子

建议步骤：

将作业多边形转为合适的局部平面坐标。
设置边界、禁区、相机参数和高度基准。
根据相机地面覆盖宽度 W 与旁向重叠率 r_side，初估航线间距：spacing = W(1-r_side)。
根据前向覆盖长度和前向重叠率确定拍照距离或周期。
选择扫描方向，兼顾转弯次数、风向、起降点和地形。
将扫描线裁剪到区域，连接成往返航线。
给转弯和进出任务区留出空间。
估计时间、能量、返航余量；必要时拆分任务。
将结果转换为 PX4 支持的任务项，检查地理坐标和高度语义。
仿真验证任务中断、继续、返航和载荷触发。

10.7 全局规划如何选

方法	适用场景	实现注意事项
Dijkstra	小地图、需要参考最优代价结果	搜索开销较大
A*	栅格/体素地图，起终点已知	启发函数、邻接和碰撞检测一致
D* 类增量搜索	地图随观测变化	更新和状态维护复杂
RRT/RRT*	高维或连续空间	路径不平滑，采样随机，仍需后处理
轨迹优化	需要连续与动力学约束	初始化、局部最优、求解超时

第一版建议：二维或有限高度的已知静态地图 + A* + 障碍膨胀 + 简化路径 + 受限轨迹生成。验证完后再扩展三维与动态障碍。

10.8 局部避障必须包含"来得及停下"

简化停止距离：

text 复制代码

d_stop ~= v*L + v^2 / (2*a_brake) + d_margin

L 包括感知、传输、规划、控制和执行延迟；a_brake 应使用经验证的实际可用减速度。jerk 限制、转弯、风和状态误差还会增加所需空间。

因此不能说"雷达 5 m 内能看到，就一定能避障"。速度越大，对视距、延迟和刹停能力要求越高。

局部规划器至少应：

按机体尺寸、定位误差与跟踪误差膨胀障碍。
将未知区域按项目规则处理，不能默认全可通行。
检查整段轨迹扫过体积，不能只检查离散航点。
每次重规划保持位置/速度连续，尽可能保持加速度连续。
保留有效的刹停/悬停候选轨迹。
求解超时或感知失效时，输出明确降级状态。

10.9 规划器放在哪里

十一、位置和速度控制

11.1 输入和输出

输入包括 trajectory_setpoint、vehicle_local_position、约束、起飞/落地状态等；主要控制输出为 vehicle_attitude_setpoint。

源码重点：

11.2 为什么位置环外面不能再堆一层随意 PID

当前核心结构可以理解为：

text 复制代码

v_sp = elem_mul(Kp_pos, p_sp - p) + v_ff

text 复制代码

a_sp = elem_mul(Kp_vel, v_sp - v) + I_v
       - elem_mul(Kd_vel, dv/dt) + a_ff

位置是 P 环，速度是带积分、测量微分与前馈的控制。若伴随计算机又实现一个不了解内环的强位置 PID，再给 PX4 位置目标，会产生不必要的重复反馈与时延耦合。

11.3 `PositionControl::update()` 按函数拆解

_inputValid()：检查每轴是否有可控目标，以及相关状态是否有效。
_positionControl()：位置误差产生速度修正，与速度前馈相加，限制水平和上下速度。
_velocityControl(dt)：生成加速度请求，并进行推力分配约束与抗积分饱和。
_accelerationControl()：根据加速度、重力和悬停推力构造推力向量，限制倾角。
填充缺省航向/航向速度并检查输出有限性。
getAttitudeSetpoint()：调用推力到姿态的转换，输出机体推力和期望四元数。

11.4 加速度为何能转成姿态

普通四旋翼不能独立产生机体横向推力，必须通过倾斜把主推力投影到水平方向。

教学推导：

text 复制代码

f_desired = m * (a_sp - g * e3)

推力沿机体负 Z，因此目标机体 Z 轴应与 -f_desired 对齐；再结合目标航向构造完整姿态。

PX4 的实现还包含悬停归一化推力、垂直/水平加速度解耦选项、最小推力与倾角限制，不能把上式直接当成全部代码。

11.5 推力饱和与抗积分饱和

当前 _velocityControl() 包含：

垂直积分约束。
垂直推力饱和时抑制继续恶化饱和的积分。
保留水平推力余量并优先满足垂直控制的分配逻辑。
由总推力上限计算剩余可用水平推力。
水平 tracking anti-windup，用实际可产生的加速度与请求差值修正积分。

飞机已经满推力时，继续增加积分不能产生更多升力，只会导致退出饱和后严重过冲。

11.6 NaN 是接口语义，不只是错误

TrajectorySetpoint.msg 注明：NaN 表示对应状态不控制。

例如速度控制可以令位置分量为 NaN；若无意把位置填零，可能激活"回到本地原点"的位置反馈。

当前 _inputValid() 要求每个轴至少有位置、速度或加速度目标之一，且水平 X/Y 的同类目标成对有效。不要自行组合不符合输入契约的混合目标。

同时，当前消息的 jerk 注释是 for logging only。这并不意味着轨迹生成器不限制 jerk，而是不能假定向此字段写数值就会给位置控制器增加 jerk 前馈。

11.7 悬停推力估计与起飞管理

mc_hover_thrust_estimator 用于改善归一化推力与悬停需求的关系。PositionControl::updateHoverThrust() 调整垂直积分，使悬停推力估计变化不会无故引起输出跳变。

模块外壳还处理地面/起飞阶段、推力爬升、估计重置和失败设定值。直接替换纯控制公式时，不要把这些工程逻辑一并删除。

十二、姿态和角速度控制

12.1 级联关系

text 复制代码

姿态目标 q_sp
  → 姿态误差控制
  → 机体角速度目标 ω_sp
  → 角速度 PID + 前馈
  → 归一化力矩目标 τ_sp

位置闭环慢、姿态环居中、角速度环快；实际带宽由机体、电机、滤波和时延共同决定，不能仅靠设置线程频率获得。

12.2 姿态控制源码

mc_att_control_main.cpp：目标来源、模式、估计重置、手动输入和发布。
AttitudeControl.cpp：四元数控制律。

AttitudeControl::update(q) 的主要步骤：

获取当前与期望机体 Z 轴。
构造忽略偏航、优先对齐推力方向的 reduced attitude。
处理推力方向完全相反时的几何边界。
提取剩余偏航旋转并按 yaw weight 缩放。
计算 qe = q.inversed() * qd。
通过四元数规范化符号选择处理 q 与 -q 表示同一姿态的问题。
用 eq = 2 * imag(qe.canonical()) 得到误差向量。
rates_sp = Kp ⊙ eq。
将世界 Z 轴航向速度前馈转换到机体系叠加。
对各轴角速度目标限幅。

这不是"欧拉角三个 PID"。优先推力方向的几何设计有助于多旋翼在姿态误差较大时保持滚转/俯仰控制需求。

12.3 为什么不用欧拉角直接相减

欧拉角有奇异性和角度跨界问题，例如 179° 与 -179° 的差不应当被当作 358°。

四元数同样有陷阱：乘法顺序、旋转方向、左右扰动、符号等必须一致。学习时先写已知旋转和逆变换测试，再实现控制律。

12.4 角速度控制源码

MulticopterRateControl.cpp：读取角速度、角加速度、目标、模式和分配饱和信息。
rate_control.cpp：核心控制律与积分。

当前核心形式为：

text 复制代码

tau_cmd = elem_mul(K_P, omega_sp - omega) + I
          - elem_mul(K_D, domega/dt)
          + elem_mul(K_FF, omega_sp)

注意：

D 项作用于测量角加速度，不是直接对误差求导。
这样可以减轻目标阶跃带来的微分冲击。
当前标准消息 vehicle_torque_setpoint.xyz 是归一化力矩目标，不应直接当 N·m。
RateControl::update() 内部根据落地状态决定是否更新积分；外层还处理解锁和积分重置等条件。

12.5 积分为什么要和分配器通信

当前角速度模块读 control_allocator_status.unallocated_torque，生成每轴正/负饱和标志，再传入 RateControl::setSaturationStatus()。

例如正向滚转力矩已经无法实现，就抑制会继续增大正向积分的误差；反向误差仍允许帮助退出饱和。

updateIntegral() 还会：

在角速度误差很大时降低积分增益影响。
检查数值是否有限。
限制积分幅度。

这体现了完整闭环：不仅状态反馈给控制器，执行器做不到的部分也必须反馈给控制器。

12.6 调参顺序与判据

核实电机方向、机体轴向、质量、惯量和振动。
建立合理滤波，确认角速度测量没有明显剪切或延迟异常。
先调角速度环，再调姿态环，再调速度/位置环。
用适当小幅激励检查延迟、上升时间、超调、振荡和输出饱和。
加入载荷、不同电压、风扰等情况检验鲁棒性。
调整前馈改善目标跟踪，不能用前馈掩盖不稳定反馈环。

更高阶的 LQR、几何控制、INDI、MPC 可以放在后续阶段。它们仍然需要高质量状态、正确执行器模型和时延控制。

十三、电机分配与执行器输出

13.1 先分清四个量

飞机需要的物理力/力矩，单位 N 和 N·m。
PX4 控制链中按约定归一化的推力/力矩目标。
每个电机的归一化推力目标。
最终发送给 ESC 的 PWM、DShot 或 CAN 指令。

它们之间存在模型和映射，不能直接认为 0.5 就是 50% 转速，也不能认为 PWM 与推力严格线性。

13.2 当前 PX4 分配链路

text 复制代码

vehicle_torque_setpoint + vehicle_thrust_setpoint
 → ControlAllocator
 → 机型对应 ActuatorEffectiveness
 → 效能矩阵、分配算法、限幅/去饱和、变化率处理
 → actuator_motors / actuator_servos
 → 输出功能映射、解锁约束、驱动
 → ESC / 舵机

源码入口：

ControlAllocator.cpp。
VehicleActuatorEffectiveness。
ControlAllocationPseudoInverse.cpp。
ControlAllocationSequentialDesaturation.cpp。
mixer_module.cpp。
PWMOut.cpp、DShot.cpp。

虽然输出层还存在 mixer_module 这个名称，当前标准控制分配已由独立 control_allocator 负责。不能因为名字里有 mixer 就把它理解成全部姿态混控算法。

13.3 从每个旋翼的几何建立效能矩阵

一般表达式：

text 复制代码

u = A * f

其中 u 是机体力/力矩请求，f 是各执行器作用量，A 的每一列表示该执行器对各控制轴的贡献。

当前 ActuatorEffectivenessRotors.cpp 的 computeEffectivenessMatrix() 读取：

相对位置 r_i。
归一化旋翼轴向 a_i。
推力系数 c_t。
力矩比系数 k_m。

源码计算对应列的贡献：

text 复制代码

F_i = c_t * a_i

text 复制代码

tau_i = c_t * cross(r_i, a_i) - c_t * k_m * a_i

矩阵前三行是力矩，后三行是推力。这里是单位执行器变量的效能列；系数如何标定和归一化要与整个分配接口一致。

旋转方向符号体现在参数和上述约定中，不要只凭"顺时针给正号"的记忆写死。

13.4 教学四旋翼 X 构型算例

下面是独立教学编号，不代表 PX4 任意机型的默认电机编号。

采用 FRD 坐标，l 为每个电机位置在 X/Y 轴上的绝对投影距离：

电机	位置 `(x,y,z)`	正推力引起的反扭矩符号
1 前右	`(l,l,0)`	`+κ f1`
2 后右	`(-l,l,0)`	`-κ f2`
3 后左	`(-l,-l,0)`	`+κ f3`
4 前左	`(l,-l,0)`	`-κ f4`

所有正推力均沿机体负 Z。令 T 为向上的总推力大小，则：

text 复制代码

u = [T, tau_x, tau_y, tau_z]^T
f = [f1, f2, f3, f4]^T

    [  1       1       1       1     ]
A = [ -l      -l       l       l     ]
    [  l      -l      -l       l     ]
    [  kappa  -kappa   kappa  -kappa ]

u = A * f

其中滚转、俯仰符号由 r×F 推出。这个四维教学矩阵与 PX4 的六维矩阵行顺序不同，且 PX4 的 Fz=-T。移植时必须做正确映射。

如果 d 是中心到电机的臂长，则对 45° X 构型有 l=d/√2，不能把臂长直接当 X/Y 投影长度。

13.5 伪逆、饱和与去饱和

无约束情况下：

text 复制代码

f = pinv(A) * u

但电机只能在允许范围内工作：

text 复制代码

f_min <= f_i <= f_max

如果简单逐电机裁剪，得到的滚转/俯仰/偏航/总推力比例可能全部改变。

当前 CA_METHOD 在配置中包含：

伪逆后裁剪。
伪逆加 sequential desaturation。
按机型自动选择。

顺序去饱和还与 MC_AIRMODE 有关，源码分 mixAirmodeDisabled()、mixAirmodeRP()、mixAirmodeRPY() 等路径。不能把优先级描述为所有构型、所有模式都完全相同。

可作为自研进阶的带约束优化形式：

text 复制代码

Choose f to minimize:
  cost(f) = norm(W * (A*f - u))^2 + lambda * norm(f - f_prev)^2

满足输出上下限及变化率限制。这个 QP 是扩展方案，不是声称当前默认分配器始终在求 QP。

13.6 分配剩余量有什么价值

当前分配器发布：

text 复制代码

u_unallocated = u_requested - u_achieved

对应 control_allocator_status 的力矩/推力剩余量和达成标志。它可用于：

角速度积分抗饱和。
判断当前任务是否超出动力能力。
分析大倾角时高度保持失败。
识别重心、几何或推进系统异常。

有非零剩余量不必然是分配算法写错，也可能是请求在物理上不可实现。

13.7 电机失败不是"重新算逆矩阵就能继续飞"

普通四旋翼失去一个电机后，通常无法继续独立实现总推力和三轴力矩全部要求。某些研究策略允许放弃偏航等目标，但不等于当前普通配置可以保证稳定安全飞行。

六旋翼等冗余构型也必须检查剩余效能矩阵的秩、推力裕量、可行域、检测延迟和控制策略。故障重分配必须先在仿真中验证，不能仅以"还有电机在转"作为验收。

13.8 从归一化推力到 ESC

ActuatorMotors.msg 当前支持最多 12 个电机控制量，定义范围 [-1,1]，负值仅对支持反向推力的输出有意义，NaN 对应停止/未解锁输出语义。

输出层还要处理：

Motor 功能到物理端口的映射。
未解锁、失效保护、测试模式的输出值。
油门到推力的非线性。
最低稳定转速和最大值。
电调协议与更新频率。
可逆电机、舵机中位和方向。
支持时的 ESC 转速、电压、电流、温度遥测。

DShot 是数字协议，能减少模拟脉宽校准相关问题，但不会自动实现精准推力闭环。若需要推力精度，仍需推进模型、转速反馈或更直接的测量。

13.9 执行器验证顺序

无桨确认物理端口对应电机编号。
无桨确认旋转方向与配置。
确认实际桨方向与目标气流方向。
用测试向量验证"正滚转/俯仰/偏航/推力"对应的电机增减。
仿真检查组合饱和与低推力场景。
有防护的动力试验中测量推力曲线和响应。
记录解锁、失联和停机时的实际输出行为。

十四、通信、遥控、数传与 ROS 2

14.1 按层分清通信方式

层次	示例	回答的问题
电气/物理接口	UART、USB、CAN、以太网、SPI、I²C	比特怎样到达另一端
承载与网络	串口流、UDP、TCP、无线电、Wi-Fi、蜂窝网络	数据怎样运输
应用协议	MAVLink、DroneCAN、设备私有协议	数据帧表达什么
软件中间件	uORB、DDS/ROS 2、Zenoh	程序如何发布和接收类型化消息
业务	遥控、遥测、任务、参数、视频	为什么传输这些数据

"使用 MAVLink"并没有回答数传频段、距离、带宽和延迟；"用串口"也没有回答消息协议和数据语义。

14.2 推荐的整机链路划分

text 复制代码

遥控器 → 遥控接收机 → 飞控：人工操纵与接管
地面站 ↔ 遥测无线链路 ↔ 飞控：状态、任务、参数、指令
伴随计算机 ↔ 串口/USB/以太网 ↔ 飞控：外部定位和轨迹目标
相机 → 伴随计算机 → 视频链路 → 地面：图像和业务数据
ESC/智能传感器 ↔ PWM/DShot/CAN 等 ↔ 飞控：执行与设备状态

链路可以共享某些硬件，但必须明确关键流量的带宽和优先级。视频拥塞不能让控制目标和健康状态长期排队。

14.3 MAVLink 源码如何读

入口：

mavlink_main.cpp：实例、传输与发送调度等。
mavlink_receiver.cpp：接收消息、校验语义、转换到 uORB。
src/modules/mavlink/streams/：遥测数据流。
src/modules/mavlink/ 下的任务、参数、命令相关实现。

可选择三个端到端案例：

HEARTBEAT：系统身份、状态和链路活跃。
COMMAND_LONG/COMMAND_INT：命令接收、转内部命令、ACK 和实际状态变化。
SET_POSITION_TARGET_LOCAL_NED：坐标、掩码、目标类型到内部控制设定值。

ACK 表示协议/命令处理状态，不一定代表飞机已完成实际动作。 例如起飞请求获接受后，仍需通过状态与高度确认已起飞。

14.4 任务、命令、设定值是三种不同事务

类型	特征	实现原则
任务上传	有序的多个任务项	序号、数量、确认、重试和一致性
离散命令	解锁、切模式、起飞等	明确目标系统、ACK、超时、状态确认
连续设定值	持续的位置/速度/姿态目标	时效优先、检查新鲜度，不积压旧目标

规划器不能把一秒前排队的目标持续送给当前飞机。更可靠的策略是保留最新有效目标，并监控目标年龄与数据源状态。

14.5 数传带宽怎样估算

对第 i 类消息，频率 f_i、有效载荷 L_i、协议及承载开销 H_i：

text 复制代码

B_bytes_per_second ~= sum_i( f_i * (L_i + H_i) )

UART 常见 8N1，每字节约 10 bit，所以：

text 复制代码

B_serial_bits_per_second ~= 10 * B_bytes_per_second

例：若一类消息实际每帧约 80 字节、50 Hz，则仅该流约占 4000 B/s，即 40 kbit/s 的 UART 线速预算。多个高频流叠加，很容易占满 115200 baud 链路。

MAVLink 2 常见未签名帧基础开销为 12 字节，签名再增加 13 字节；实际还要考虑载荷截断、无线封装、重传和双向业务。数传标称空口速率不等于可用业务吞吐。

建议分级：

关键状态、故障、命令确认：优先保障。
姿态/位置：按监控与控制需要设置频率。
慢变量：电量、温度、GPS 质量用较低频率。
调试和日志下载：避免挤占飞行关键流。
完整高频飞行数据优先本地记录 ULog。

14.6 遥控链路与遥测链路不要混为一谈

遥控通常用于人工输入和接管；遥测用于状态和配置。某些设备能同时传输多类数据，但飞控仍需独立判断：

遥控输入是否有效。
地面站心跳是否丢失。
Offboard 控制信号是否丢失。
伴随计算机是否存活。

地面站连接正常，不等于控制目标仍在更新；伴随计算机心跳正常，也不等于规划线程没有卡住。

14.7 ROS 2 与 uXRCE-DDS

当前入口：dds_topics.yaml。

逻辑关系：

text 复制代码

飞控 uORB ↔ uXRCE-DDS Client ↔ Agent ↔ DDS / ROS 2 节点

从飞控视角：

/fmu/out/... 通常是飞控对外发布。
/fmu/in/... 通常是外部向飞控输入。

但并非所有 uORB 消息自动桥接。当前配置还包含输出限频，以及被注释而默认不导出的条目。

接入时逐项确认：

px4_msgs 与固件消息定义和版本一致。
目标话题被编入桥接配置。
发布/订阅 QoS 兼容。
坐标、时间、NaN 语义正确。
连接断开和 Agent 重启行为明确。
同一设定值只有预定的合法写入者。
外部模式或注册组件接口按当前版本使用，不能只套旧示例。

14.8 Offboard 的正确设计

建议将外部控制程序分成：

text 复制代码

估计质量检查
    → 规划状态机
    → 连续目标生成
    → 目标合法性/新鲜度检查
    → Offboard 控制模式与目标发布
    → 模式/命令 ACK/实际状态监测

文档和常见接入流程通常要求持续提供高于 2 Hz 的存活信号，并在切入前稳定发送一段时间；工程上可从 20--50 Hz 的合适目标更新频率起步，但最终以当前版本、接口与运动需求为准。

当前仓库的重要事实 ：HealthAndArmingChecks/checks/offboardCheck.cpp 使用 offboard_control_mode.timestamp、COM_OF_LOSS_T 和相应状态有效性判断可用性。COM_OF_LOSS_T 当前参数默认值为 1.0 s。因此不能把"某个固定频率"当成源码里唯一的失联规则。

ROS 2 路径中 Offboard 存活消息与轨迹目标可以分离。仅心跳持续更新、轨迹生成器却卡住，是你自己的系统仍需识别的故障。

14.9 安全、完整性与可观测性

通信设计至少包含：系统/组件身份、命令路由、重复命令处理、超时、版本、时间同步、链路统计。

MAVLink 签名可用于认证与完整性保护，不提供载荷加密。需要保密时使用适当的安全承载；密钥、参数写入权限和远程升级属于整机设计的一部分。

验收记录 RTT、单向延迟（有可靠同步时）、丢包、抖动、最大连续中断、队列长度、有效吞吐和恢复时间。

十五、异常处理与系统可靠性

15.1 故障处理必须与状态估计联动

返航需要合适的定位、有效返航目标与可行路径。位置失效时"所有故障都返航"不是完整方案。

建议建立如下分析表，具体动作由项目决定并在仿真验证：

故障	检测依据	需要评估的处置
遥控丢失	输入年龄、接收机状态	是否继续任务、悬停、返航或降落
地面站丢失	心跳/链路状态	是否影响自主任务，是否触发预设动作
Offboard 丢失	控制信号和目标生产者健康	切入已验证的机内模式
GNSS 异常	创新、质量、有效性	是否仍有光流/VIO；能否维持局部控制
全部水平定位失效	本地位置/速度无效	保留可用控制层级并按场景降级
姿态/IMU 异常	冗余检查、估计状态	切换健康源或进入预设紧急处置
低电量	电压、电流、剩余量与返航需求	提前返航、限制任务或降落
围栏风险	位置、边界、预测运动	限制目标、停留、返航等
ESC/电机异常	转速、电流、通信、控制残差	是否具备剩余可控性，执行对应策略
伴随计算机重启	数据中断、序号、时间基准	飞控独立保持已验证状态
计算超时	deadline、执行时间、watchdog	隔离低优先任务，触发降级

15.2 故障逻辑的结构

建议采用：

text 复制代码

原始状态 → 健康判定 → 故障去抖与持续时间
        → 多故障优先级仲裁 → 动作可行性检查
        → 执行动作 → 退出/锁存/人工接管规则

关键要求：

异常出现和异常解除使用适当滞回，避免模式抖动。
多故障同时出现时，动作优先级可解释。
降级后是否自动恢复，由明确规则决定。
记录故障原因、原模式、新模式和触发时间。
人工接管也需要检查该控制模式所需状态是否可用。

15.3 能源管理不能只有一个电压阈值

电池电压受电流负载影响；SOC、可用能量、老化和温度共同影响剩余飞行能力。

完整设计可逐步加入：

电压/电流校准。
库仑计量与电压模型。
低压及瞬时跌落区分。
返航距离、爬升高度与逆风余量。
任务消耗预测与保守落地余量。

第一版可以采用已验证的保守策略，复杂能源模型作为后续增强。

15.4 可靠性不仅是软件故障码

还应验证供电冗余、线束、存储卡、连接器、散热、EMI、震动、低温、传感器重启、参数损坏和启动失败。

PX4 规模的成熟度来自大量边界条件处理与测试积累，不是实现几个先进公式即可达到。

十六、如何组织自己的算法和代码

16.1 两种可落地的代码组织

独立教学飞控：

text 复制代码

flight_stack/
  platform/          时钟、线程、总线、硬件抽象
  drivers/           IMU、气压、测距、ESC 等
  messages/          类型、单位、坐标、有效性
  estimation/        姿态、导航、传感器质量
  guidance/          航点、轨迹、目标管理
  control/           位置、姿态、角速度算法
  allocation/        几何、约束、执行器映射
  commander/         状态机、权限、故障管理
  communication/     协议、命令、遥测
  parameters/        参数定义、校验和持久化
  logging/           日志、事件、性能统计
  simulation/        动力学、传感器与故障模型
  tests/             单元、回放、闭环和集成测试

基于现有 PX4 扩展：

优先把纯算法放入清晰的库或模块子目录。
用模块外壳订阅已有消息并发布约定输出。
需要新增接口时再定义 uORB 消息，不随意改变共享消息语义。
按目标板配置加入 Kconfig、CMakeLists.txt、.px4board 等所需选项。
需要启动的新模块加入适当机型启动逻辑，并验证启动失败行为。
从参数默认关闭、仿真启用、日志影子运行开始，再进入闭环。

16.2 模块接口契约模板

每个模块写出下面内容，比直接写 C++ 更重要：

项目	必须说明
输入	消息、坐标、单位、来源和所需有效性
时间	触发事件、频率范围、最大可接受样本年龄
输出	消息、量纲、限幅、NaN/失效含义
状态	初始化、激活、停用、重置、模式切换
参数	合法范围、默认值、是否允许运行时改变
失败行为	超时、数值异常、求解失败时输出什么
性能	CPU、栈、堆、最长执行时间
诊断	日志、中间量、计数器、状态报告
测试	正常、边界、随机、故障和回归案例

16.3 算法外壳示意代码

下例是设计伪代码，不是可直接复制编译的 PX4 模块：

cpp 复制代码

void ControllerModule::Run()
{
    // 1. 先处理生命周期；退出时注销回调并释放调度资源。
    if (should_exit()) {
        stop_scheduling();
        return;
    }

    update_parameters_if_needed();
    update_mode_and_health();

    State state;
    if (!read_new_state(state)) {
        return;
    }

    const Time now = monotonic_time();
    if (!is_fresh(state, now) || !state_required_fields_valid(state)) {
        report_invalid_state();
        apply_defined_failure_policy();
        return;
    }

    const float dt = validated_sample_interval(state.sample_time);
    handle_estimator_reset_and_mode_transition(state);

    if (!owns_control_output_in_current_mode()) {
        maintain_inactive_state();
        return;
    }

    Setpoint sp;
    if (!read_valid_setpoint(sp, now)) {
        apply_defined_failure_policy();
        return;
    }

    ControlInput input = build_input(state, sp, dt);
    ControlOutput output = algorithm.update(input);

    if (!output.is_finite() || !output.within_contract()) {
        report_numerical_failure();
        apply_defined_failure_policy();
        return;
    }

    publish_output(output, state.sample_time, now);
    publish_diagnostics_if_due();
}

这里的失败策略必须事先定义，不能默认"一遇到坏数据就把电机设零"，也不能无限维持旧输出。具体行为由控制层级与系统状态机共同决定。

16.4 真实 PX4 模块从哪里抄结构

参考：

重点学习生命周期、调度、参数和消息接口，不要从示例直接推断飞行控制的全部异常处理。

16.5 推荐的算法替换顺序

自研目标	优先替换位置	应保留什么
新的业务航线	外部任务生成器或任务上传层	全部基础估计与控制
新局部规划器	伴随计算机 Offboard 位置/速度层	机内失联处理与轨迹跟踪
新轨迹平滑	FlightTask 或对应目标生成层	模式、约束与下游控制
新位置控制	`PositionControl` 算法层及必要适配	起飞、状态检查、姿态角速度环
新姿态控制	`AttitudeControl`	模式、目标管理、角速度和分配
新角速度控制	`RateControl`	采样滤波、输出与分配反馈
新构型	ActuatorEffectiveness 与机型配置	成熟控制分配和输出基础设施
新定位源	传感器适配/外部视觉接口	EKF 框架与有效性处理

一次只改变一个主要闭环，使实验结果可归因。

16.6 影子运行与切换

新算法先只计算和记录，不发布会被实际执行的控制目标。与原算法比较：误差、输出、饱和、计算耗时和失败次数。

真正切换前，需要保证：

输出所有权唯一。
积分与内部状态合理初始化。
目标与状态连续。
估计器重置信息已处理。
回退路径明确，并在仿真中验证。

16.7 第一版应该交付的工程文件

至少维护：需求规格、坐标与接口规范、模块设计、参数表、板级配置、机型配置、测试用例、日志分析脚本、版本清单、试验记录。

算法源码之外的这些文件决定了系统能否重复构建、复现实验和长期维护。

十七、仿真、测试、日志和调参

17.1 先建立验证金字塔

从低成本、高可重复性验证，逐步走向真实系统：

text 复制代码

数学与接口单元测试
  → 控制器 + 理想动力学闭环
  → 带噪声、延迟、饱和的闭环仿真
  → PX4 SITL 完整任务
  → 真实日志离线分析 / 适用模块回放
  → 飞控硬件运行与 HITL
  → 无桨电气和执行器检查
  → 有防护的动力系统测试
  → 分阶段实机飞行
  → 多环境、长时间与故障回归

每一级都有不同目的：SITL 验证完整软件逻辑，HITL 增加真实处理器与接口约束，实机才会暴露完整振动、气动、供电和射频问题。任何一级都不能完全代替另一级。

17.2 用什么仿真

方式	适合验证	主要不足
Python/Matlab/C++ 教学仿真	动力学、控制公式、参数敏感性	通常缺少真实飞控软件链路
PX4 SITL + Gazebo	任务、控制、通信、传感器集成	模型仍需标定，不能等同真实硬件
PX4 SIH 类仿真	较轻量的闭环和逻辑验证	环境/感知能力取决于具体仿真配置
日志回放	估计器修改、传感器问题、离线诊断	记录的运动不会随新控制输出改变
HITL	目标飞控时序、接口、完整栈	真实动力、电源和振动仍未完全覆盖
实机	整机闭环表现	成本较高、重复性较差、问题更难隔离

特别注意：对同一段历史日志重新算一个控制器输出，不等于证明新控制器闭环稳定，因为历史状态是旧控制器产生的。

17.3 第一批可以执行的学习命令

以下是本仓库文档中可找到的典型入口，本文编写期间没有执行构建、下载依赖或飞行测试。运行前应确认本机开发环境、子模块和仿真资源已准备好。

在仓库根目录的主机终端：

bash 复制代码

# 记录自己的源码基线
 git rev-parse HEAD
 git status --short

# 启动典型四旋翼 SITL；依赖与模型可用时使用
 make px4_sitl gz_x500

# 仅作为需要时的单元测试入口
 make tests TESTFILTER=Attitude
 make tests TESTFILTER=PositionControl

测试过滤器匹配的是实际测试名称；应检查输出中选中的测试数量，不能把"没有选中任何测试"当成测试通过。不同分支、构建选项和依赖状态可能改变可用目标。

在 PX4 的 shell 中，以下命令用于查看状态，不要直接放进普通 Bash 当作系统命令：

text 复制代码

commander status
ekf2 status
mc_att_control status
mc_rate_control status
mc_pos_control status
control_allocator status
mavlink status
uorb top
listener vehicle_attitude
listener vehicle_local_position
listener trajectory_setpoint
listener vehicle_rates_setpoint
listener control_allocator_status
listener actuator_motors

模块必须存在且已启动；详细参数用对应 help 查看。频繁打印高频消息会扰动受限硬件的运行，不应将大量控制台输出作为实机长期观测方案。

17.4 单元测试应该测试什么

子系统	有意义的测试
坐标与数学	坐标往返一致；单位四元数；`q` 与 `-q` 等价；180° 邻域
轨迹生成	起终边界；速度/加速度/jerk 限制；重规划连续性；零距离目标
位置控制	NaN 语义；速度限幅；推力饱和；高度与水平耦合；积分恢复
姿态控制	零误差；大偏航；推力方向相反；角速度限幅
角速度控制	正负反馈方向；积分限制；饱和方向；落地/解锁切换
控制分配	单轴请求；几何变更；组合饱和；无效旋翼轴；输出残差
状态估计	静止；匀速；延迟；跳变；观测拒绝；失效和恢复
状态机	不满足条件不切换；故障优先级；去抖；接管；恢复规则
通信	错包；重复；乱序；超时；版本不匹配；链路恢复

不要只测试"函数返回 true"。例如四旋翼分配的输出应重新代入正向模型，检查生成的力矩/推力是否符合请求与约束。

17.5 仓库已有测试值得作为学习教材

AttitudeControlTest.cpp。
PositionControlTest.cpp。
ControlMathTest.cpp。
ControlAllocationPseudoInverseTest.cpp。
ControlAllocationSequentialDesaturationTest.cpp。
ActuatorEffectivenessRotorsTest.cpp。
PositionSmoothingTest.cpp。
VehicleOpticalFlowTest.cpp。
ekf2/test。
failsafe_test.cpp。

阅读测试时记录：作者认为哪些情况必须成立、如何构造输入、哪个边界条件需要特别保护。测试往往比头文件更容易解释设计意图。

17.6 设计可量化的验收指标

位置跟踪误差：

text 复制代码

e_p(t) = p(t) - p_sp(t)
RMSE_p = sqrt( sum_k(norm(e_p[k])^2) / N )

姿态误差可用相对四元数的旋转角，而不是直接比较三个欧拉角差值：

text 复制代码

e_theta = 2 * acos(clamp(abs(q_err_w), 0, 1))

建议记录：

类别	指标
控制	RMSE、最大误差、超调、整定时间、角速度振荡、积分大小
估计	对真值误差、创新统计、拒绝率、漂移、失效恢复时间
分配	饱和占比、未分配力矩/推力、输出变化率
实时性	执行耗时、端到端延迟、P99、最大样本年龄、deadline miss
任务	完成率、飞行时间、重复性、转弯误差、载荷动作准确性
通信	丢包、最长中断、带宽、重试、恢复、目标新鲜度
能源	平均功率、任务能耗、返航余量、最低供电电压
故障	检测延迟、动作延迟、恢复时间、误报率、模式抖动

不要直接把仿真器真值输出当成估计器性能。控制器使用估计状态，评估程序使用独立真值，两条数据链应区分。

17.7 初始验收门槛怎么定

不存在对所有机型通用的"悬停误差必须小于某个固定厘米数"。可以使用以下方法：

先让未修改的 PX4 在目标仿真条件下运行，形成基线分布。
为任务设置绝对要求，例如允许偏离范围和最长中断时间。
为改动设置相对要求，例如不增加关键场景的饱和占比和最大误差。
确定足够多的重复次数、随机种子、载荷/风/噪声组合。
所有关键故障场景必须满足预定状态转移与输出约束。
接受平均表现改善的同时，检查最差情况是否变坏。

例如，一个规划器把平均航程缩短 5%，却使求解偶发超时达到数秒，就不能仅凭航程指标判定更好。

17.8 建议的故障注入矩阵

场景	注入变化	观察重点
GNSS 退化	暂停、噪声增大、位置跳变	拒绝、状态有效性、模式动作
光流失效	质量下降、停止数据、错误尺度	是否退出融合、是否保持错误自信
视觉重定位	原点或航向重置	控制目标是否发生不合理跳变
Offboard 停止	停消息、停止规划但保留心跳	机内超时和外部健康机制分别生效
链路拥塞	延迟、抖动、丢包、乱序	是否执行过期目标、队列是否增长
动力下降	推力上限减小、响应变慢	高度、饱和、积分恢复
电机异常	仿真中的单电机效能下降	检测、剩余可控性、既定处置
载荷变化	质量/惯量/重心变化	悬停推力、跟踪、分配偏置
风扰	阵风、恒风、侧风	轨迹误差、推力余量、能源消耗
计算压力	感知/规划耗时增加	控制循环是否保持独立及时

故障注入 API 和支持范围随仿真后端、构建和配置变化。先确认故障实际施加到了预期数据链，不能仅根据"注入命令被接受"判断测试有效。

17.9 日志应该记录哪些量

至少保存：

Git 提交、板卡、机型、参数、任务和试验环境。
位置/速度/姿态/角速度目标与实际状态。
估计器有效性、重置、融合来源、创新与方差。
推力/力矩目标、分配剩余量、电机/舵机输出。
模式、解锁、起飞/落地、故障原因与事件。
电池、ESC 和传感器错误信息。
算法运行时间与通信数据年龄。

具体消息是否记录、记录频率多少，由 logger 配置决定。本文列出的主题存在，不等于默认 ULog 中一定含有完整高频数据。

17.10 一次调参试验的标准流程

固定版本、机型、载荷和测试任务。
记录基线并确认传感器与动力没有异常。
明确要改善的现象，例如角速度超调，而不是笼统"飞得不好"。
每次只调整少量有明确因果关系的参数。
重复相同激励，比较前后指标和饱和情况。
检查其他轴、其他电量和不同载荷是否退化。
保存参数差异与结论；失败试验也应留有记录。

十八、从零到完整系统的实施路线

18.1 用里程碑管理，不以"写了多少代码"衡量

下面是一条面向四旋翼的参考路线。时间取决于 C++、嵌入式、控制、数学和硬件经验；从零基础到成熟产品通常远超过一个短期项目。阶段完成应按验收，而不是按日历自动进入下一步。

阶段	参考投入	核心产物	进入下一阶段的条件
M0 需求与基线	1--2 周	需求、架构、源码/参数基线	目标和边界可测量
M1 软件与仿真入门	2--3 周	SITL 起降、日志和话题图	能复现完整原版任务
M2 数学与教学闭环	3--5 周	四旋翼动力学、姿态/角速度闭环	仿真稳定，坐标和分配测试通过
M3 传感器与估计	4--8 周	校准、融合学习、延迟与失效实验	能解释状态来源和质量
M4 位置控制与任务	3--5 周	定点、直线、转弯、起降、RTL	约束和失败路径可验证
M5 通信与外部控制	2--4 周	ROS 2/MAVLink 接入、Offboard 状态机	断链与重启测试通过
M6 光流或视觉	3--6 周	无 GNSS 局部定位方案	漂移、质量和失效边界明确
M7 规划与业务	4--8 周	覆盖航线、已知地图规划、局部轨迹	动力学与碰撞约束满足
M8 硬件与整机验证	持续投入	板级验证、参数、试验报告	分阶段整机验证通过
M9 产品化	持续投入	回归、维护、版本、可靠性体系	面向目标工况有充分验证证据

各阶段有可并行学习内容，但核心闭环验证应按依赖顺序推进。这份时间表不是对复现 PX4 全部成熟度的工期承诺。

18.2 M0：定义一架明确的无人机

任务：

确定机型、质量、用途、环境、速度、续航、定位和通信需求。
选择已有飞控板作为第一版实时平台。
建立 BOM、传感器接口图、供电图、坐标系图。
记录当前 PX4 提交、目标板、机型和默认参数。

交付物： 需求规格.md、系统框图、接口约定.md、测试计划.md。

验收： 每个目标能找到相应测试方法；没有"高精度""低延迟"这类无数值或无工况的空泛指标。

18.3 M1：不改算法，跑通 PX4

任务：

在 SITL 中完成手动/位置模式、任务起飞、航点、返航和降落。
观察 uORB 消息与模块状态。
导出并分析一份 ULog。
画出从 trajectory_setpoint 到 actuator_motors 的消息链。
记录原版算法在几种任务中的基线误差。

验收： 能解释飞机此刻在哪种模式、目标由谁生成、状态由谁估计、哪个模块发布电机目标。

18.4 M2：写自己的小型闭环仿真

建议按以下顺序实现：

一维垂直动力学与高度控制。
单轴转动动力学与角速度 PID。
四元数姿态运动学。
三轴姿态与角速度级联。
X 四旋翼效能矩阵与输出上限。
三维平移和位置/速度控制。
电机响应、噪声、延迟、载荷和风扰。

交付物： 可重复仿真程序、指标曲线、单元测试和模型说明。

验收： 零输入、静止悬停、单轴激励、正负符号、饱和等基础案例均符合物理预期。不要用调 PID 的方式修复错误的坐标或动力学符号。

18.5 M3：理解并验证估计链

任务：

学会查看原始传感器与估计状态。
写独立互补姿态估计与简化 EKF 学习实现。
阅读 EKF2 的输入、预测、融合、输出预测和有效标志。
对比 GNSS、气压、光流、视觉分别提供什么信息。
在仿真/回放中加入延迟、偏置、丢失和跳变。

验收： 能通过日志区分"传感器错误""融合拒绝""状态重置""控制跟踪错误"。

18.6 M4：自主起降和航点任务

任务：

理解 Navigator 的任务项与 FlightTask 的轨迹生成。
实现独立五次轨迹或 S 曲线学习版本。
比较直线、圆、矩形和急转弯任务。
检查速度、加速度、jerk、倾角和推力约束。
验证模式切换、任务暂停、继续与返航。

验收： 目标没有不合理跳变；飞机在给定能力内完成任务；不可行任务被拒绝、减速或进入已定义处理路径。

18.7 M5：通信与 Offboard

任务：

选择一个主接口先实现，不要同时维护多套未验证协议。
获取飞控状态、时间、位置和命令 ACK。
完成 Offboard 目标发布、模式确认和退出。
实现目标年龄、规划器心跳和模式监控。
分别测试程序退出、网络断开、Agent 重启和规划线程卡住。

验收： 各种断链都进入指定动作，恢复后不会未经设计自动抢回控制权。

18.8 M6：光流/VIO 定位

如果场景只需要室内低空稳定，先光流 + 测距；如果需要较大空间导航与建图，规划 VIO/SLAM。

任务：

标定旋转、位置外参和时间。
测量不同高度/纹理/光照下的误差。
验证杆臂、倾斜、失效和原点重置。
确定可用速度、高度和场景边界。

验收： 不只是一次悬停成功，还能重复说明何时可信、何时退化、如何降级。

18.9 M7：地图和规划

从小问题开始：已知静态地图、有限速度、简单场景。

任务：

建立地图接口与障碍膨胀规则。
实现 A* 路径和约束轨迹。
检查轨迹连续碰撞、动力限制与停止距离。
引入局部重规划和求解超时处理。
最后再考虑未知地图、动态障碍与任务优化。

验收： 路径可通行、轨迹可执行、失效可处理；规划器失败不会使实时控制失效。

18.10 M8：从软件走向真实飞机

推荐顺序：

飞控供电、传感器、通信和日志正常。
不装桨核对执行器映射与方向。
受控动力测试验证实际推力和电流能力。
确认质量、重心和结构满足模型。
在适当的测试条件下开展短时、简单飞行。
逐渐增加高度、速度、航线、载荷和环境复杂度。
每次扩展前审核对应日志和异常表现。

系留或测试架会改变动力学，不能把受约束测试中的稳定直接等同自由飞行稳定。

18.11 M9：怎样接近"PX4 这种完全体"

还需要补齐：

多板卡配置、驱动维护与硬件版本管理。
参数兼容、升级迁移、出厂与用户校准。
故障覆盖、自动回归、长时间稳定性。
模块资源预算、栈溢出和看门狗等异常处理。
地面站体验、日志诊断、可维护性。
不同构型与载荷的配置和验证。
团队代码评审、规范、文档与问题追踪。

个人可以逐步做出可理解、可验证的完整四旋翼系统；达到 PX4 的广泛机型支持和长期可靠性，需要更大规模的工程积累。合理目标是先把自己的目标机型和使用场景做好，再逐步扩大能力范围。

十九、逐步学习路线和源码阅读顺序

19.1 知识依赖图

text 复制代码

C++ / Linux / Git / 构建 ----------> PX4 模块 / uORB / 调度 --+
数学 -> 坐标 / 四元数 / 动力学 -> 控制与估计 ------------------+--> 源码数据链
                                                                  |
                                                                  v
                                                   任务 / 轨迹 / 通信 / 故障
                                                                  |
                                                                  v
                                                   地图 / 规划 / 光流 / 视觉
                                                                  |
                                                                  v
                                                   整机验证 / 工程化

不必先学完所有数学才运行 PX4，但每写一层算法，必须补齐该层所需基础。

19.2 基础科目与具体练习

科目	必须掌握	对应练习
C/C++	引用、对象生命周期、模板基本使用、内存、浮点	写固定大小向量运算和无堆分配更新函数
Linux/Git/CMake	终端、调试、版本、目标构建	构建示例模块，记录可复现版本
嵌入式	UART/SPI/I²C/CAN、DMA、中断、RTOS、时钟	解释一帧 IMU 数据的采集到发布流程
线性代数	旋转矩阵、特征值、伪逆、协方差	四旋翼矩阵分配和正向验证
微积分/数值方法	导数、积分、离散化、误差	对比 Euler/RK 积分，理解 `dt` 的影响
控制	闭环、PID、带宽、相位、饱和、稳定性	给单轴模型加延迟，看稳定范围变化
概率/估计	噪声、贝叶斯、KF/EKF、可观测性	一维位置速度 KF，再扩展姿态误差状态
机器人	坐标树、位姿、标定、时间同步	ENU/FLU 到 NED/FRD 的完整转换
规划	图搜索、约束优化、轨迹、碰撞检测	A* 输出路径再生成可行轨迹
软件验证	单元、集成、回放、随机测试	同一任务多随机种子统计误差

19.3 24 周学习安排：建立能力，不承诺产品成熟度

每周投入较稳定、且已有一定编程基础时，可按以下次序组织。基础不足时延长相应阶段。

周次	学习主题	本周必须完成的产物
1	Git、仓库结构、开发环境	源码版本记录、目录地图
2	SITL、地面站、日志	一次完整起降任务与日志
3	uORB、模块生命周期、工作队列	一个只订阅和打印低频状态的示例
4	坐标、四元数、单位、时间	坐标变换和旋转测试
5	动力学与推进模型	一维高度和单轴转动仿真
6	PID、滤波、饱和	单轴角速度控制与延迟实验
7	四元数姿态控制	三轴姿态仿真、阅读 AttitudeControl
8	电机几何与分配	X 构型正逆映射及饱和测试
9	IMU、标定、振动	原始/滤波数据分析报告
10	互补估计、KF 基础	互补姿态估计、一维 KF
11	EKF2 输入与状态	状态表、预测/融合调用图
12	延迟融合、有效性、重置	一组传感器延迟/丢失分析
13	位置速度控制	PositionControl 函数笔记、基线曲线
14	起飞、降落、模式状态机	模式和阶段状态转移图
15	航点、Navigator、FlightTask	三航点任务完整消息追踪
16	轨迹生成与约束	五次轨迹或 S 曲线验证程序
17	MAVLink、命令和遥测	消息/带宽表与命令状态机
18	ROS 2/Offboard	外部轨迹任务与断链验证
19	光流、测距与融合	光流单位/符号/高度尺度实验
20	VIO、外参和时间	外部视觉接口设计或接入验证
21	全局搜索与地图	已知地图 A* 路径规划
22	局部轨迹与停止距离	动力约束、碰撞检查、超时处理
23	Failsafe、能源与可靠性	故障矩阵、自动测试结果
24	项目整合与复盘	演示、架构说明、日志指标与后续计划

24 周可以建立系统认知与研究原型；实机可靠性、复杂视觉导航、固定翼和产品化是继续迭代的阶段。

19.4 源码阅读顺序：先短链，再全局

第一轮：先把控制链读通。

msg/versioned/TrajectorySetpoint.msg。
PositionControl::setInputSetpoint()、update()、getAttitudeSetpoint()。
ControlMath::thrustToAttitude()。
AttitudeControl::update()。
RateControl::update()、updateIntegral()。
ControlAllocator 的主运行和分配调用。
ActuatorEffectivenessRotors::computeEffectivenessMatrix()。
ActuatorMotors.msg 与输出映射。

第二轮：看模块怎样把纯算法连接起来。

MulticopterPositionControl::Run()。
mc_att_control_main.cpp 的主运行逻辑。
MulticopterRateControl::Run()。
SubscriptionCallbackWorkItem 和模块初始化。
参数更新、模式使能、时间计算、状态重置和发布。

第三轮：追踪目标从哪里来。

FlightModeManager::Run() 和任务选择。
FlightTaskAuto::updateInitialize()、update()。
PositionSmoothing、VelocitySmoothing。
Navigator、mission、rtl。
MAVLink 任务和 Offboard 接收。

第四轮：追踪状态从哪里来。

VehicleIMU、VehicleAngularVelocity。
EKF2.cpp 的输入与发布。
Ekf::update()、predictState()、协方差预测。
先读一种观测源，例如气压或 GNSS，再读光流和视觉。
输出预测、多实例选择、观测拒绝和重置。

第五轮：补齐完整系统。

Commander、解锁检查、Failsafe、启动脚本、板卡、日志、测试、通信与升级配置。

19.5 每读一个模块，都回答这十个问题

谁创建和启动它？
它运行在哪个任务或工作队列？
谁触发它运行，何时不运行？
输入消息的单位、坐标和时间是什么？
它在什么模式下拥有输出权限？
内部状态有哪些，何时初始化和重置？
输入异常或超时时做什么？
输出经谁转换后作用于飞机？
参数改变如何生效，是否改变稳定性？
用哪些日志和测试证明它运行正确？

如果只能解释核心公式，答不出以上问题，就还没有真正理解飞控模块。

19.6 建议的综合学习项目

项目 A：完整理解悬停。

从地面站起飞到固定高度，画出位置、速度、姿态、角速度、推力、力矩和电机输出；解释每一层误差如何影响下一层。

项目 B：自己设计平滑航线。

使用三个非共线航点，分别比较直接目标切换、五次轨迹和 PX4 原生平滑的速度、加速度、误差和推力饱和。

项目 C：无 GNSS 定点。

使用光流/测距或外部视觉，描述坐标、时间和观测方程；演示数据退化后的状态变化与预设降级。

项目 D：通信中断。

分别中断地面站、Offboard 目标、外部存活信号和伴随计算机进程，证明它们触发的检查不是同一件事。

项目 E：自研算法替换。

只替换一个模块，从影子运行、单元测试、SITL 对比到闭环启用，形成一份可复现的实验报告。

二十、固定翼、VTOL 与高级算法扩展

20.1 为什么先学四旋翼，再扩展固定翼

四旋翼低速时可近似悬停，主要通过推力方向控制平移；固定翼必须维持足够空速，升力、阻力、航迹、风和能量耦合更强。

问题	常规多旋翼	固定翼
低速停留	可以悬停	一般需要盘旋，不能任意停住
横向运动	倾斜推力产生加速度	滚转产生向心加速度并改变航向
高度与速度	主要通过推力与倾角协调	油门和俯仰共同分配能量
主要限制	推力、倾角、角速度、电量	失速、空速、载荷因子、转弯半径
执行器	电机为主	舵面和推进器，效能随空速变化
降落	可垂直下降	需要进近、航迹、下滑和接地管理

20.2 当前固定翼源码结构

本仓库有：

FixedWingModeManager.hpp 引用 DirectionalGuidance；FwLateralLongitudinalControl.hpp 引用 CourseToAirspeedRefMapper、AirspeedDirectionController 和 TECS。因此，不应强行在当前仓库寻找某些旧教程中的单一 fw_pos_control 结构。

20.3 横向引导与风

固定翼的地速是空速与风速叠加：

text 复制代码

v_ground = v_air + v_wind

飞机机头指向不必等于地面航迹方向。横向引导必须考虑侧风、可行空速和路径误差，生成合适的方向/横向加速度等目标。

对理想协调平飞转弯：

text 复制代码

R_turn ~= V^2 / (g * tan(phi))

这里只是简化关系，V 的含义和风场需要在具体推导中明确。实际地面轨迹受风影响，不能把无风公式当成所有情形的精确转弯半径。

学习重点：路径跟随与追逐一个点不同；目标应包括路径切向、横向误差和速度可行性。

20.4 TECS：高度与空速的能量协调

单位质量势能和动能：

text 复制代码

E_p = g * h
E_k = 0.5 * V^2

总能量与能量平衡可以用来理解：

油门主要影响总能量供给。
俯仰主要影响势能与动能的分配。

真实实现还需要空速滤波、爬升/下沉限制、油门/俯仰限制、欠速保护和飞行阶段处理。TECS 不是简单把高度误差接油门、空速误差接俯仰的两个独立 PID。

20.5 VTOL 的新增问题

VTOL 需要同时理解多旋翼与固定翼，还要处理：

垂直起降、过渡、固定翼巡航等状态。
过渡期间气动和旋翼效能随时间、空速、倾转角改变。
不同控制器的目标与积分如何交接。
舵面、电机、倾转舵机的联合分配。
最低转换高度、转换时间、空速有效性与失败回退。
电源、热负荷和过渡能耗。

入口：vtol_att_control，以及控制分配中的 ActuatorEffectivenessStandardVTOL、ActuatorEffectivenessTiltrotorVTOL、ActuatorEffectivenessTailsitterVTOL。

过渡失败不总能在任何状态下可靠恢复。必须按具体机型建模，明确剩余高度、空速、执行器状态和恢复条件。

20.6 高级控制算法什么时候值得引入

方法	可能解决的问题	前置条件与代价
LQR	多变量耦合与性能权衡	模型和线性化工作点合理
几何控制	大姿态机动、避免局部参数奇异	SO(3)/SE(3)、模型和约定正确
INDI	基于增量响应改善扰动抑制	角加速度测量、时延匹配、效能估计
MPC	显式处理预测与多类约束	模型、求解时间上界、不可行时兜底
扰动观测/自适应	载荷和扰动变化	可辨识性、噪声、稳定性分析
学习控制	复杂非线性和特定任务优化	数据覆盖、分布外行为、可解释的保护和验证

选择依据应是已经测到的瓶颈。例如当前问题是视觉 150 ms 延迟不稳定，就应先修正时间链，而不是首先把 PID 换成神经网络。

20.7 建议的进阶顺序

用日志辨识角速度/电机动态与推力曲线。
对现有级联控制做频域或时域性能分析。
改善轨迹前馈、模型参数和时延。
尝试单一高级算法并与原版建立公平基线。
引入负载变化、风、噪声、丢包和模型误差验证鲁棒性。
证明计算资源与最差运行时间满足飞控实时要求。

二十一、设计交付物、问题排查和参考资料

21.1 核心算法规划总表

以下表格可直接作为项目任务拆分的起点。

子系统	第一版采用	后续增强	核心输出	验收重点
动力模型	实测静态推力 + 一阶电机动态	电压/温度/入流影响	效能、响应、限值	能否解释实测响应
姿态估计	复用 EKF2；独立互补滤波用于学习	冗余与多源评估	姿态、有效性	静态/动态误差和恢复
位置估计	GNSS/气压/IMU	光流、VIO、RTK、多源切换	位置、速度、协方差	漂移、延迟、失效边界
航点任务	原生 Navigator	作业业务、覆盖路线	任务项、目标点	可执行性和任务恢复
轨迹生成	原生平滑/受限 S 曲线	样条、优化轨迹	连续 p/v/a/航向	约束和连续性
全局规划	已知地图 A*	三维与增量搜索	几何路径	可通行、计算开销
局部规划	静态障碍短时重规划	动态障碍预测	可行轨迹及有效期	碰撞、停止距离、超时
位置控制	PX4 P + 速度 PID/前馈	模型前馈、MPC 等	推力与姿态	跟踪、推力裕度、积分
姿态控制	PX4 四元数姿态控制	几何控制等	角速度目标	大姿态、角度跨界、限幅
角速度控制	PX4 PID + 前馈	INDI/LQR 等	归一化力矩	延迟、振荡、抗饱和
分配	几何效能 + 伪逆/去饱和	约束优化、容错	执行器目标	可行域与剩余控制量
通信	MAVLink + 一个外部控制接口	冗余与业务网络	状态、命令、目标	带宽、时效、恢复
故障管理	PX4 健康/模式/故障框架	任务相关仲裁	模式、限制、动作	多故障和动作可行性
运维	ULog、参数与版本基线	自动诊断与回归	可复现实验记录	问题是否可追溯

21.2 一次"飞向航点"发生了哪些代码事件

用普通多旋翼自动任务作为例子，按数据依赖读源码：

步骤	模块/函数入口	此时应该检查的问题
1 接收任务	`src/modules/mavlink/` 的任务处理	数量、序号、坐标、高度基准是否合法
2 管理任务	`navigator/mission.cpp`、`mission_base.cpp`	当前任务项、到达条件、任务完成状态
3 输出导航目标	`navigator` 与 `position_setpoint_triplet`	上一、当前、下一点是否有效
4 选择任务算法	`FlightModeManager::Run()`	当前是否确实选择自动任务
5 生成连续轨迹	`FlightTaskAuto::update()`	参考原点、轨迹限制、当前速度
6 平滑与发布	`PositionSmoothing`、`generateTrajectorySetpoint()`	位置、速度、加速度与航向一致吗
7 位置反馈	`MulticopterPositionControl::Run()`	本地状态是否新鲜有效、是否起飞阶段
8 算出推力方向	`PositionControl::update()`	速度误差、倾角/推力限制、积分
9 算出姿态目标	`ControlMath::thrustToAttitude()`	航向与推力轴是否一致
10 算出角速度	`AttitudeControl::update()`	四元数误差、偏航权重、角速度限幅
11 算出力矩	`RateControl::update()`	实测角速度、角加速度、积分和饱和
12 分配到电机	`ControlAllocator`、`allocate()`	几何、输出约束、剩余力矩
13 输出到硬件	`mixer_module`、PWM/DShot/CAN 驱动	端口映射、解锁、协议输出
14 获得新测量	驱动与 `sensors`	采样时刻、校准、滤波、错误
15 更新状态	`EKF2`、`Ekf::update()`	融合、创新、状态有效性
16 判断完成	Navigator 任务项判定	是否满足到达/停留/后续任务条件

这些步骤并非严格串行执行的一条线程。它们通过不同速率的消息连接，理解时间关系与数据版本和理解函数一样重要。

21.3 必须熟悉的消息速查表

消息/话题	核心语义	主要消费者
`vehicle_imu`	IMU 积分与时序数据	估计器等
`vehicle_angular_velocity`	处理后的机体角速度及相关导数信息	角速度控制
`vehicle_attitude`	当前姿态与重置信息	姿态控制、任务等
`vehicle_local_position`	本地位置/速度、有效性、原点与重置	任务、位置控制
`vehicle_global_position`	全局位置	导航与遥测
`vehicle_visual_odometry`	输入的外部视觉里程计话题	EKF2
`vehicle_optical_flow`	机体系光流积分、角增量、距离与质量	EKF2
`trajectory_setpoint`	NED 位置/速度/加速度和航向目标	位置控制
`vehicle_attitude_setpoint`	期望姿态与机体推力	姿态控制
`vehicle_rates_setpoint`	期望机体角速度与推力	角速度控制
`vehicle_torque_setpoint`	归一化机体力矩请求	控制分配
`vehicle_thrust_setpoint`	归一化机体推力请求	控制分配
`control_allocator_status`	达成情况、剩余力/力矩等	控制反馈、诊断
`actuator_motors`	每个电机的归一化推力请求	输出层
`vehicle_control_mode`	当前使能的控制层级	各控制模块
`vehicle_status`	系统与导航状态	多数模块
`offboard_control_mode`	外部控制层级与活跃信息	状态检查与控制管理
`vehicle_command` / `vehicle_command_ack`	离散命令与处理确认	命令处理模块和外部程序

消息文件名通常使用 CamelCase，话题名使用 snake_case；有些消息通过 # TOPICS 声明多个话题，因此话题名未必一一对应一个同名文件。

21.4 参数应该按子系统阅读

参数用于理解算法接口，不是提供可盲目复制的飞行参数。

类别	当前参数示例	源码定义入口
姿态 P 增益	`MC_ROLL_P`、`MC_PITCH_P`、`MC_YAW_P`	`mc_att_control_params.yaml`
偏航与角速度限制	`MC_YAW_WEIGHT`、`MC_ROLLRATE_MAX` 等	`mc_att_control_params.yaml`
角速度 PID/前馈	`MC_ROLLRATE_P/I/D/FF/K` 及其他轴	`mc_rate_control_params.yaml`
位置 P	`MPC_XY_P`、`MPC_Z_P`	`multicopter_position_control_gain_params.yaml`
速度增益	`MPC_XY_VEL_P_ACC/I_ACC/D_ACC` 及 Z 轴	同上
速度/倾角/推力限制	`MPC_XY_VEL_MAX`、`MPC_TILTMAX_AIR`、`MPC_THR_MIN/MAX`	`multicopter_position_control_limits_params.yaml`
自动任务限制	`MPC_XY_CRUISE`、`MPC_ACC_HOR`、`MPC_JERK_AUTO`	`multicopter_autonomous_params.yaml`
起飞降落	`MPC_TKO_RAMP_T`、`MPC_TKO_SPEED`、`MPC_LAND_SPEED`	`multicopter_takeoff_land_params.yaml`
几何与分配	`CA_AIRFRAME`、`CA_METHOD`、`CA_ROTOR...`	`control_allocator/module.yaml` 等
GNSS 融合	`EKF2_GPS_CTRL`	`ekf2/params_gnss.yaml`
光流融合	`EKF2_OF_CTRL`	`ekf2/params_optical_flow.yaml`
外部视觉融合	`EKF2_EV_CTRL`	`ekf2/params_external_vision.yaml`
Offboard 失联	`COM_OF_LOSS_T`、`COM_OBL_RC_ACT`	`commander/commander_params.yaml`

表中带 / 的写法是参数组缩写，例如 MC_ROLLRATE_P/I/D 表示三个独立参数，并不是一个实际参数名。

角速度控制还有重要细节：当前 MulticopterRateControl::parameters_updated() 将 MC_*RATE_K 分别乘到 P、I、D，再传入核心控制器；FF 单独传入。解释有效增益时要考虑这一层，而不能只看 P 参数。

查某个参数时建议使用：

bash 复制代码

rg -n 'MC_ROLLRATE_K' src/modules/mc_rate_control
rg -n 'MPC_XY_VEL_P_ACC' src/modules/mc_pos_control
rg -n 'EKF2_OF_CTRL' src/modules/ekf2
rg -n 'COM_OF_LOSS_T' src/modules/commander

先看定义的单位、范围和描述，再追踪其读取位置，最后看机型启动脚本是否覆盖默认值。

21.5 常见现象如何分层排查

现象	优先检查	容易误判的地方
起飞瞬间翻转	电机顺序、转向、桨向、飞控轴向、分配符号	一开始就调 PID
高频抖动	机械振动、滤波、角速度增益、延迟	认为是位置环太强
悬停慢漂	定位源、航向、光流尺度、视觉坐标、风扰	只提高位置 P
高度忽上忽下	气压/测距、垂直速度、推力曲线、起飞阶段	只看气压高度一条曲线
大倾角掉高	推力饱和、垂直优先和动力余量	认为加大高度积分就能解决
转弯猛冲	目标不连续、速度/加速度限制、航点处理	直接降低所有控制增益
Offboard 自动退出	信号年龄、状态有效性、模式请求与链路	只检查 ROS 节点有没有进程
Offboard 不退出但目标失效	独立心跳仍在、规划器卡住、旧目标保持	认为存活消息代表规划健康
光流速度方向反了	安装方向、坐标、积分/角速度、补偿符号	用负 PID 掩盖方向错误
视觉重定位后突然移动	原点变换、reset counter、目标同步	归因于电机输出随机变化
某电机长期偏高	重心、几何、桨/电机效能、安装、偏置	认为电机输出应该始终相同
仿真稳定实机不稳	模型、时延、振动、电源、推进响应	认为仿真已经证明全部正确

排查顺序建议：物理与坐标 → 测量与时间 → 状态估计 → 目标生成 → 控制 → 分配与执行 → 故障和通信。 根据日志证据收敛问题，不要一次同时改多个子系统。

21.6 项目交付检查表

系统与硬件

需求、工况和验收指标明确。
飞控、伴随计算机、传感器、ESC、电池和链路接口完整。
质量、重心、惯量估计、推进曲线与供电能力有记录。
安装方向、外参、线束、振动和热设计经过检查。

算法与数据

所有输入输出明确坐标、单位、时间与有效性。
各控制环、轨迹约束、分配边界与真实动力能力一致。
定位源失效与恢复、状态重置和目标连续性得到验证。
无效数据、求解失败、超时和数值异常有明确处理。

软件与通信

输出权限唯一，模块生命周期与参数更新清楚。
工作队列中没有不可控阻塞。
协议版本、消息定义、QoS、命令确认和重试正确。
目标新鲜度、链路丢失与计算机重启行为得到验证。
固件、配置、参数、任务和试验版本可以追溯。

验证与维护

单元、SITL、故障、整机测试按阶段完成。
日志包含足够的状态、目标、残差、模式和时间信息。
最差误差、最差耗时和边界工况已评估。
有明确的回归任务和已知限制清单。
文档说明系统能做什么、在哪些条件下不能保证性能。

21.7 优先阅读的本地官方资料

随仓库保存的文档适合对照当前代码；翻译和个别章节可能滞后，涉及接口时以源码与消息定义核实。

补充数学与控制教材方向：

线性系统与反馈控制：学习极点、带宽、相位裕度、离散化和抗饱和。
概率机器人与状态估计：学习 KF/EKF、协方差、可观测性、误差状态和多传感器融合。
机器人运动学：学习 SO(3)、SE(3)、四元数、外参与坐标树。
小型无人机动力学与控制：例如 Beard 与 McLain 的 Small Unmanned Aircraft: Theory and Practice，固定翼内容尤其值得结合源码阅读。
运动规划：图搜索、采样规划、轨迹优化、碰撞检查和实时求解。

21.8 本文分析范围与使用方式

前 21 章详细展开四旋翼核心闭环、传感器融合、任务、通信与工程验证；后续第 22--33 章补充全仓库架构和按目录核对的组件清单。覆盖口径是当前提交的架构层和一级组件，具体驱动型号通过对应目录继续下钻；这不是对所有文件的逐行注释，也不把目录存在等同于特定固件已启用或已验证。

建议第一次阅读先抓住第 1、4、5、11、12、13 章，建立完整控制链；第二次读第 6--10、14--15 章，补齐状态和目标来源；随后按第 17--19 章完成实验，将自己的源码笔记、参数、日志和测试结果逐步补充到项目文档中。

二十二、全仓库架构总览与覆盖边界

22.1 如何定义本次补充的"全部架构"

本次补充按当前提交实际目录建立全景：覆盖平台、板级、驱动、通信、基础库、全部一级业务模块、系统命令、构建生成、仿真测试、开发工具与外部生态接口。第 24--26、30 章的目录表与本地目录集合进行核对。

必须区分三个完整性层次：

层次	本文覆盖情况	还需做什么
仓库架构层与一级组件	给出全量清单、分层职责及主要连接关系	随源码版本变化更新清单
核心算法与业务链	四旋翼详细展开，其他机型和服务给出独立链与入口	针对实际选型深入相关实现
每种芯片寄存器、每个参数、每行代码	不声称逐行解释全部实现	进入对应驱动、消息、参数和测试逐项阅读

"一级目录全覆盖"不等于所有芯片、第三方子模块、机型脚本均已逐行审计，也不等于某个二进制固件包含全部这些功能。本章以架构全景为目标，保留可进一步追踪的路径。

22.2 从物理世界到开发工具的完整分层

text 复制代码

外部生态：操作员 / QGroundControl / MAVSDK / ROS 2 / 地图与视觉系统
                   | 任务、参数、命令、状态、轨迹、外部定位
                   v
协议与外部接口：MAVLink / uXRCE-DDS / Zenoh / 遥控协议 / CAN 设备协议
                   |
                   v
系统决策：Commander / 模式管理 / 健康与解锁检查 / Failsafe
                   |
                   +--> 任务导航：Navigator / 各机型任务与引导
                   +--> 业务载荷：相机 / 云台 / 投放 / 发动机
                   v
估计与控制：传感器融合 / 飞行阶段 / 多机型控制器 / 控制分配
                   |
                   v
数据与基础服务：uORB / 参数 / 时间 / 事件 / dataman / 日志 / 性能统计
                   |
                   v
驱动与设备抽象：传感器 / RC / PWM / DShot / CAN / 电源 / 存储等
                   |
                   v
平台与板级：NuttX / POSIX / 其他适配 / BSP / 中断 / DMA / 时钟 / 总线
                   |
                   v
真实硬件或仿真设备

贯穿各层：构建配置 / 自动生成 / 元数据 / 单元与集成测试 / CI / 诊断工具

这是一张责任分层图，不代表底层服务只允许单向调用。例如 logger 订阅多个层次的消息，Commander 同时消费估计、任务、通信和电源状态。

22.3 三种架构视图必须同时存在

静态视图： 目录、类、库依赖、构建目标。回答"代码在哪里"。

运行视图： task、work queue、消息、设备、中断、时间。回答"代码何时运行、如何交换数据"。

配置视图： 板卡、Kconfig、机型脚本、参数、设备实例、输出映射。回答"这架飞机实际启用了什么"。

仅看 src/modules/ 不能得到完整 PX4 架构，正如仅看一张控制框图无法解释启动、存储或升级。

22.4 一个功能从源码到生效的完整条件

text 复制代码

源码与依赖存在
  -> Kconfig / 板卡配置允许编译
  -> CMake 把目标与依赖链接进固件
  -> 启动脚本或显式命令启动模块
  -> 驱动发现正确设备 / 消息来源可用
  -> 参数与运行模式激活相应分支
  -> 健康检查与控制权限允许参与
  -> 实际输出被下游消费

例如 mc_nn_control 存在于仓库，不代表所有飞机使用神经网络控制；uavcan 驱动存在，不代表板上有 CAN 收发器；编译了光流驱动，不代表 EKF 已融合光流。

22.5 顶层目录和外部边界

路径	架构角色
	厂商/板卡配置、引脚、总线、计时器、板级启动与固件变体
	操作系统与处理器适配、公共模块框架、uORB 和调度
	编入固件或仿真运行环境的初始化脚本和机型默认配置
	驱动、库、模块、命令、示例、模板及公共头文件
、	消息/服务接口源定义与版本化接口
	构建函数、参数选择、元数据、覆盖率与打包规则
	环境、构建辅助、消息/参数生成、仿真、分析与上传工具
、	单元相关基础设施与端到端集成测试
<test_data>	测试使用的数据资源
	模块等配置的校验 schema
	多语言文档、生成和发布工具
	ROS/仿真等启动集成文件，注意接口代际
	POSIX 场景配置相关内容
<.github/workflows>	构建、检查、测试、文档和消息同步等 CI 流程
`build/`、`cmake-build-debug/`	本地生成的构建产物，不是新的飞控功能层

QGroundControl、ROS 2 运行环境、Micro XRCE-DDS Agent、MAVSDK、外部 VIO/SLAM 算法不因 PX4 有接口就全部包含在本仓库。采购/部署时应单独管理其版本、许可证、依赖和验证。

二十三、操作系统、板级支持与运行架构

23.1 NuttX 和 POSIX 两条主路径

入口	作用	学习重点
<platforms/nuttx>	嵌入式 NuttX 平台集成	板级初始化、任务、设备、时钟、中断、内存
<platforms/posix>	POSIX 平台与主机运行支持	进程入口、线程、仿真时钟和平台接口
<platforms/common>	跨平台公共实现	ModuleBase、工作队列、uORB、日志、总线辅助
<platforms/qurt>	QuRT 相关平台支持	与特定处理器/系统搭配的执行与跨核通信
<platforms/ros2>	ROS 2 平台适配相关代码	平台级适配与常规 uXRCE-DDS 桥不是同一层

目录存在只说明仓库保存了相应实现，不能由此推断当前支持质量、默认板卡或所有功能与 NuttX 完全等价。

23.2 板级 BSP 到底包含什么

以 <boards/px4/fmu-v6x> 为真实阅读样本：

default.px4board 及其他 .px4board：选择功能和固件变体。
src/：板级 C/C++、引脚、总线、计时器、初始化等。
nuttx-config/：NuttX 相关配置。
init/：板级启动片段。
firmware.prototype：打包所需固件描述相关内容。
bootloader.px4board：该板对应引导程序构建配置之一。

自研飞控板的工作不是只增加一个名字，还包括晶振与时钟树、Flash/RAM 布局、启动地址、供电检测、总线设备、CS/DRDY 引脚、DMA/定时器冲突、USB、SD、CAN、调试与恢复路径。

23.3 总线与硬件中断

text 复制代码

传感器硬件采样
  -> 数据就绪中断 / 定时查询 / FIFO 达到阈值
  -> SPI/I2C/UART/CAN 传输
  -> DMA或CPU读取、CRC/状态解析
  -> 驱动转换、校准所需标识与时间戳
  -> uORB 测量

中断应尽量短，把需要较长时间的处理交给适当线程或工作队列。多个 SPI 设备共享总线时，设备采样频率和总线事务的最坏占用必须一起设计。

高分辨率时钟接口可从 drv_hrt.h 与具体平台实现追踪。主机仿真可能使用模拟时间，不能把墙上时间、RTC、飞控启动时间和仿真时间混作一类。

23.4 任务与工作队列的分工

执行类型	典型用途	工程要求
中断上下文	数据就绪、计时、捕获等	短小、有界、不做复杂阻塞操作
独立 task/thread	通信、存储或独立循环	分配栈、优先级，处理退出与阻塞
WorkItem	数据触发的短处理	同队列串行，不能拖慢其他工作
ScheduledWorkItem	定时与事件结合	检查调度周期、超时与重复触发
后台低优先工作	持久化、维护、慢状态处理	不能侵占关键控制时间

工作队列共享线程不等于每个模块有独立线程；两个回调的执行时间相加，可能形成其他任务看不到的延迟。

23.5 内存与资源

需要关注：

程序代码 Flash、静态数据 RAM、栈、堆、DMA 缓冲区和缓存一致性。
内核/用户态保护配置，以及跨边界的设备、参数和消息调用。
每个任务与工作队列栈峰值；平均 RAM 充足不代表某个栈不会溢出。
传感器 FIFO、uORB 队列、日志缓冲、串口/网络缓冲的容量与上限。
数值精度与算法负载，尤其是多 EKF、视觉接口和学习控制推理。
平台专属的紧耦合内存、缓存与代码放置策略，需按硬件验证。

23.6 FMU 与 IO 协处理器

相关入口：px4io 驱动、px4iofirmware。

在具备 IO MCU 的板卡上，FMU 运行主要飞控逻辑，IO MCU 承担配置规定的输出、输入和相关独立行为。没有 IO MCU 的板卡可能直接由 FMU 生成执行器输出。

实际故障能力取决于板卡连接、固件和配置。不能笼统认为"有 IO 就一定具备完整双飞控冗余"。必须确认 FMU 失效后实际输出是什么、指令多久过期、是否还有可靠测量和控制来源。

23.7 启动路径有平台区别

嵌入式主要看 ROMFS/px4fmu_common/init.d/，POSIX/SITL 还需要看 init.d-posix。

当前通用 rcS 包含存储和参数处理、板级脚本、数据管理、传感器、估计器、遥控、Commander、输出驱动、机型应用、串口配置、Navigator 与业务模块等条件启动逻辑。

尤其注意 rc.serial 等内容可能由构建工具生成。运行时脚本引用的路径不一定有一个同名的源码文件；应追踪生成规则，而不是判断代码"缺失"。

二十四、全部一级驱动目录与设备架构

24.1 驱动层不是算法层的简单前置

驱动需要把"设备特定的数据帧"变为"统一物理意义的消息"，也需要把标准执行器目标变成设备可理解的协议。它负责设备生命周期、总线、频率、量程、错误计数、时间和配置。

下表覆盖本提交 src/drivers/ 的 57 个一级子目录。目录中可能包含多个芯片/厂商子驱动；这些子驱动共享对应类别的架构位置，不表示它们可以互换而无需配置。

驱动目录	职责与架构位置
actuators	执行器厂商接口集合，如 Vertiq、VOXL ESC 等子目录
adc	模拟量采集，供电检测等功能的基础
auterion_autostarter	根据 EEPROM/探测结果自动启动相关电源监测设备
barometer	多种气压计设备，输出气压/温度测量
batt_smbus	SMBus 电池接口相关驱动
bootloaders	引导/应用共享数据和启动相关基础支持，不是传感器
camera_capture	相机实际曝光/捕获反馈信号采集
camera_trigger	相机触发输出和触发事件
cdcacm_autostart	USB CDC ACM 连接上的服务自动启动处理
cyphal	Cyphal/CAN 设备节点、发布订阅与服务集成
differential_pressure	差压测量，通常参与空速计算
distance_sensor	激光、超声等距离传感器设备集合
dshot	DShot 电机指令与支持的 ESC 遥测
gnss	新增/特定 GNSS 驱动集合，当前包括 Septentrio 子目录
gpio	GPIO 设备接口支持
gps	GNSS 接收、协议适配与定位数据发布
heater	传感器等加热控制相关实现
hygrometer	湿度传感器
imu	加速度计/陀螺仪与组合 IMU 设备集合
ins	外部惯性导航设备及其协议集成
irlock	红外目标检测设备接口，可用于降落目标估计
lights	LED/灯光设备控制
linux_pwm_out	Linux 平台相关 PWM 输出
magnetometer	磁场传感器设备集合
optical_flow	光流设备接口与积分角位移数据
osd	视频叠加显示等设备接口
pca9685_pwm_out	PCA9685 外部 PWM 控制器输出
power_monitor	电压、电流、电源监测芯片集合
pps_capture	PPS 脉冲捕获与时间相关支持
pwm_input	PWM 输入测量
pwm_out	飞控 PWM 输出与定时器通道管理
px4io	FMU 与 PX4 IO 协处理器通信
qshell	特定 QuRT/协处理器环境的命令交互支持
rc	CRSF、DSM、GHST、SBUS 等独立接收协议驱动集合
rc_input	遥控接收输入与协议检测/处理入口之一
roboclaw	RoboClaw 电机控制器接口
rpi_rc_in	Raspberry Pi 平台遥控输入支持
rpm	转速传感器设备接口
rpm_capture	基于脉冲捕获的转速测量
safety_button	硬件安全按钮状态输入
smart_battery	智能电池设备集成
stub_keystore	密钥存储后端占位/适配实现；不能仅凭目录认定有硬件安全存储
sw_crypto	软件密码运算后端支持
tap_esc	TAP ESC 接口
tattu_can	Tattu CAN 电池等对应设备接口
telemetry	BST、FrSky、HoTT、Iridium 等专用遥测接口
temperature_sensor	独立温度测量设备
test_ppm	PPM 输入相关测试支持
tone_alarm	蜂鸣器/提示音输出
transponder	应答机与空中交通信息相关设备
uavcan	DroneCAN/UAVCAN v0 生态的传感器、执行器和节点服务
uavcannode	将相应设备/固件作为 CAN 外设节点运行的支持
uwb	超宽带测距等设备支持
voxl2_io	VOXL2 平台 IO 接口
vtx	视频发射机相关协议/设备控制
vtxtable	视频发射机频率/功率表等配置支持
wind_sensor	风速/风向等设备接口

24.2 同类芯片如何继续下钻

以 IMU 为例，先看类别 Kconfig，再看厂商/芯片的 Kconfig、CMakeLists.txt、头文件、init/probe、采样回调和发布代码。最后看板级总线定义与 rc.board_sensors 或机型启动配置。

不要以"存在驱动目录"推断板子上的 SPI 总线号、CS 引脚、安装旋转或设备型号。硬件连接与驱动实现必须两边对应。

24.3 两套 CAN 架构不能混写

uavcan 目录包含 libdronecan；cyphal 包含 libcanard、类型、节点管理、服务与发布订阅管理。它们涉及不同协议代际与类型体系，不是改个波特率就能互通。

CAN 集成需要明确：协议族、位速率、节点身份、数据类型、设备配置、总线终端、电源、错误恢复、带宽与固件版本。

24.4 驱动增加的具体开发步骤

写设备数据手册摘要：寄存器、量程、采样时刻、CRC、复位与启动时间。
确定统一输出消息与单位，避免把厂商私有格式传播到控制器。
实现探测、配置、采集、恢复和状态报告。
使用公共驱动基类/总线工具，保留 device_id 与错误计数。
加入 Kconfig/CMake/板级选择和启动配置。
验证时间、方向、满量程、故障恢复与总线竞争。
检查上层是否真正使用数据：传感器选择、EKF 融合和控制有效性。

二十五、基础服务与全部一级算法库

25.1 四种数据机制不要混淆

机制	用来保存/传递什么	特点
uORB	运行中的状态、目标、事件与测量	消息式交互，关注时间、实例和队列
参数系统	持久配置、校准和算法选项	类型、默认值、更新通知与持久化
dataman	航点、围栏、集结点等结构化持久数据	索引化记录及读写接口
logger/ULog	运行历史和诊断证据	按配置记录，带宽和存储受限

不是所有数据都应该塞进参数，也不应把 uORB 当成可长期保存任务记录的数据库。

25.2 参数链路

text 复制代码

源定义（YAML / 相应源码定义）
  -> 构建工具解析与生成
  -> 固件参数默认与元数据
  -> 启动加载持久参数、机型/板级默认处理
  -> 模块读取参数
  -> parameter_update 通知
  -> 模块执行必要的重新配置或重新初始化

入口：<src/lib/parameters>、<Tools/module_config>、param 命令、mavlink_parameters.cpp。

需要区分"源默认""板级/机型设定默认""用户保存值""运行中的临时值"。新固件参数改名、删除或语义改变时，要考虑迁移逻辑，不能只保证编译通过。

25.3 事件系统与 send_event 的区别

<src/lib/events> 及构建工具负责结构化事件相关基础能力；MAVLink 有专门的事件传输实现。

src/modules/events/ 的可执行命令是 send_event，当前源码描述它为在低优先队列执行维护任务的后台模块，并提及遥控丢失提示音。不能根据目录名把它当成整个结构化事件系统的唯一中心。

25.4 全部 61 个基础库目录

以下目录通常被模块链接使用，不一定是能执行 xxx start 的独立模块。

库目录	职责
adsb	空中交通冲突计算相关逻辑
airspeed	空速换算与相关计算
atmosphere	大气、密度与高度相关模型
battery	电池状态计算与公共模型
button	按键处理公共逻辑
cdev	字符设备抽象与公共接口
cdrstream	CDR 序列化等通信数据编码支持
circuit_breaker	特定检查/功能的断路器配置支持
collision_prevention	障碍距离输入下的运动限制
component_information	组件元信息相关支持
control_allocation	通用效能与分配算法
controllib	控制块、滤波/控制基础工具
conversion	数值/单位等公共转换
crc	CRC 校验算法
crypto	密码功能抽象与支持
dataman_client	数据管理客户端访问与缓存相关能力
drivers	传感器、设备、SMBus 等驱动公共基类/封装
events	结构化事件公共接口
field_sensor_bias_estimator	场传感器偏置估计工具
fw_performance_model	固定翼性能模型
geo	地理坐标、投影和距离计算
gnss	GNSS 公共工具与处理支持
heatshrink	轻量数据压缩依赖
hysteresis	带时间/状态滞回的判定
lat_lon_alt	经纬高坐标表达与运算
led	指示灯公共控制接口
mathlib	限幅、滤波和常用数学工具
matrix	向量、矩阵、旋转与四元数
metadata	构建生成执行器等元数据的集成
mixer_module	输出功能提供者、映射与执行器输出管理
modes	模式定义、UI 映射等公共逻辑
motion_planning	位置/速度/航向平滑与轨迹约束
npfg	固定翼非线性路径/方向引导相关组件
parameters	参数类型、存储层、更新、生成与迁移
perf	性能计数器和耗时统计
pid	通用 PID 支持
pid_design	PID 设计相关计算
pure_pursuit	路径追踪几何算法
rate_control	角速度控制核心算法
rc	遥控协议解析等公共支持
ringbuffer	环形缓冲区
rl_tools	强化学习/策略推理相关依赖
rover_control	地面车辆控制公共算法
rtl	返航时间估计等公共逻辑
sensor_calibration	传感器校准参数与补偿公共实现
slew_rate	信号变化率限制
stick_yaw	操纵杆航向目标生成
sticks	操纵杆输入整形和运动目标辅助
system_identification	系统辨识相关算法
systemlib	系统级公共函数与工具
tecs	固定翼总能量控制
tensorflow_lite_micro	微控制器神经网络推理依赖
terrain_estimation	地形估计相关公共能力
timesync	时间同步计算与辅助
tinybson	BSON 编解码，参数等数据格式支持
tunes	提示音定义和处理
variable_length_ringbuffer	变长数据环形缓冲
version	源码、构建、板卡等版本信息
weather_vane	风标/迎风航向相关辅助
wind_estimator	风估计及空速相关辅助估计
world_magnetic_model	世界地磁模型相关数据和计算

25.5 第三方依赖与 PX4 自有模块的边界

matrix、各控制库、驱动公共层与内嵌第三方代码在维护方式上不同；具体来源以 .gitmodules、许可证和构建文件为准。相同算法在教学实现、生成代码、外部依赖里可能分别出现。

改动前应确认：文件是否自动生成、是否子模块、上游是否维护、改动是否影响其他目标板、是否需要同步生成脚本。EKF 符号生成文件尤其不应只改产物而不改推导源。

二十六、全部一级功能模块与多机型控制链

26.1 模块清单的阅读规则

下表覆盖 src/modules/ 当前 57 个一级子目录。"输入/输出"列列出主要架构接口，不穷举每个订阅话题；具体字段和多实例要打开模块头文件与消息定义核对。

"启用边界"说明何时需要该模块，实际是否编入目标固件由板级 Kconfig 决定。某些目录是多个子模块的集合，不能机械地把目录名当命令名。

模块目录	核心职责与主要数据关系	启用边界
airship_att_control	飞艇姿态相关控制；消费机体状态与目标，生成构型相应控制请求	飞艇机型
airspeed_selector	多空速源验证、选择、比例/风估计辅助；输出 `airspeed_validated`	固定翼/VTOL 等需要空速的配置
attitude_estimator_q	四元数姿态估计实现；利用惯性等信息估计姿态	按 `ATT_EN` 等配置的替代估计路径
battery_status	电源测量到电池状态的计算与发布	电池来源和启动配置选择
camera_feedback	触发/曝光反馈结合车辆状态，生成 `camera_capture`	相机触发/反馈业务
commander	系统状态、模式、解锁、健康与故障仲裁	主系统状态管理
control_allocator	力矩/推力请求到电机/舵机目标，发布分配状态	对应机型分配链
dataman	任务、围栏、集结点等数据持久化与访问	任务/导航及后端配置
ekf2	多传感器导航融合、输出预测、多实例选择	常用导航估计链，受 `EKF2_EN` 等配置控制
esc_battery	将 ESC 状态中的电源信息转换成电池状态	选择 ESC 为电池信息来源时
events	`send_event` 后台维护与提示相关任务	与结构化事件库区分
flight_mode_manager	FlightTask 生命周期、模式目标与轨迹/约束生成	多旋翼/VTOL 的相应控制阶段
fw_att_control	固定翼姿态目标到角速度等内环目标	固定翼/VTOL 固定翼链
fw_autotune_attitude_control	固定翼控制辨识和自动调参流程	支持的机型、参数与实验条件
fw_lateral_longitudinal_control	固定翼横纵向控制、空速方向与 TECS；命令名 `fw_lat_lon_control`	固定翼/VTOL
fw_mode_manager	固定翼模式、起降和路径引导目标生成	固定翼/VTOL
fw_rate_control	固定翼角速度控制，结合空速等条件生成内环作用	固定翼/VTOL
gimbal	遥控/MAVLink 输入到云台输出协议或通道	`MNT_*` 等配置与设备支持
gyro_calibration	运行阶段相关陀螺校准/偏置处理	由 IMU 校准配置启用
gyro_fft	陀螺频谱分析，为振动/滤波相关处理提供信息	算力及 `IMU_GYRO_FFT_EN` 等配置
hardfault_stream	将保留的硬故障信息经 MAVLink 传送	支持平台、故障记录和链路配置
internal_combustion_engine_control	点火、油门、阻风门、启动机与转速相关状态管理	内燃机机型，ICE 与转速采集配置
land_detector	根据状态、运动与控制信息判断着陆阶段	不同载具选择对应 detector
landing_target_estimator	结合 IRLock 等目标报告与机体状态，发布 `landing_target_pose`	精准降落目标估计链之一
load_mon	CPU/运行负载等监测	平台与资源诊断
local_position_estimator	LPE 局部位置估计实现，供替代估计配置使用	`LPE_EN` 等配置，不能默认与 EKF2 混用输出
logger	订阅状态与诊断，记录 ULog；文件/链路后端	日志配置、带宽和存储条件
mag_bias_estimator	磁偏置估计与校准相关辅助	`MBE_ENABLE` 等配置
manual_control	手动输入源选择、有效性与操作意图处理	遥控/手动 MAVLink 等输入
mavlink	遥测、命令、任务、参数、时间同步、文件/日志和事件协议	按串口/UDP 等建立实例
mc_att_control	多旋翼姿态到角速度目标	标准 MC 控制链
mc_autotune_attitude_control	多旋翼辨识与姿态/角速度控制调参流程	`MC_AT_EN` 及相应状态条件
mc_hover_thrust_estimator	悬停归一化推力估计	多旋翼高度/推力控制辅助
mc_nn_control	神经网络多旋翼控制路径；源码声明由状态/目标直接产生四个电机动作	条件启动与专用模式，不是标准 PID 链必经步骤
mc_pos_control	位置/速度闭环、目标/起飞管理、推力与姿态目标	标准 MC 位置控制链
mc_raptor	RAPTOR 策略飞行模式及其状态/目标处理	专用策略、构建和运行启用条件
mc_rate_control	角速度 PID/前馈、积分与饱和反馈	标准 MC 内环
muorb	uORB 跨处理器/特定运行环境的通信支持集合	与 QuRT/POSIX 等特定平台组合
navigator	航点、返航、围栏、起降与导航任务状态	自动导航与相关监测
payload_deliverer	抓取器/绞盘类载荷交付流程、超时/反馈和结果确认	载荷设备与任务配置
px4iofirmware	IO MCU 上执行的固件	有对应 IO 硬件及固件构建路径
rc_update	原始遥控通道标定、映射和状态处理	RC 输入链
replay	按日志重放数据，支持相应离线分析/估计验证	回放构建与日志内容要求
rover_ackermann	阿克曼转向车的位置、姿态、速率、速度与执行器控制	前轮转向等对应构型
rover_differential	差速车导航与左右驱动协调	差速构型
rover_mecanum	麦克纳姆轮平面运动与轮组控制	全向地面车构型
sensors	多传感器选择、校准应用、组合、积分和滤波	测量到估计/控制的公共入口
simulation	仿真桥、动力学、虚拟传感器/电源/输出等子模块	仿真后端与构建目标
spacecraft	专用载具的位置、姿态与速率子控制器组织	特定 2D/3D 构型和仿真/硬件；不是完整航天 GNC 系统
task_watchdog	检测任务长时间占用并保留寄存器/栈/负载诊断	支持的 NuttX 配置；不等同所有硬件看门狗功能
temperature_compensation	传感器温度相关补偿/校准支持	相应传感器和参数配置
time_persistor	周期保存 RTC 时间并在启动恢复	无电池 RTC 等场景；不代替传感器时间同步
uuv_att_control	水下载具姿态相关控制	UUV 机型
uuv_pos_control	水下载具位置相关控制	UUV 机型
uxrce_dds_client	配置过的 uORB 话题与外部 DDS/ROS 2 通信	Client/Agent、消息版本与传输配置
vtol_att_control	VTOL 飞行阶段、过渡与 MC/FW 控制输出协调	Standard/Tiltrotor/Tailsitter 等构型
zenoh	基于 Zenoh 的消息发布订阅桥接支持	独立启用、话题与网络配置

26.2 运行上下文怎样准确查

不同模块的执行上下文不应只凭名字推测。每个模块按下面顺序追踪：

text 复制代码

CMakeLists.txt 的 MAIN
  -> xxx_main() / ModuleBase::main()
  -> task_spawn() / init()
  -> 类继承 WorkItem、ScheduledWorkItem 或独立 task
  -> 构造函数中的 wq_configurations / task 优先级与栈
  -> registerCallback / ScheduleOnInterval / poll / 主循环
  -> Run() 或对应任务入口

已核对的例子：camera_feedback 使用 hp_default WorkItem；payload_deliverer 和 time_persistor 使用 lp_default ScheduledWorkItem；esc_battery 使用 ESC 状态回调；landing_target_estimator 使用独立 task 启动。具体周期和优先级仍以当前源码为准。

26.3 固定翼完整控制链

text 复制代码

任务/手动模式 + 位置/空速/风等状态
  -> fw_mode_manager：飞行阶段与路径引导
  -> fixed_wing_lateral_setpoint / fixed_wing_longitudinal_setpoint 等接口
  -> fw_lat_lon_control：方向/横向作用与 TECS 纵向能量控制
  -> 姿态、油门等目标
  -> fw_att_control
  -> fw_rate_control
  -> control_allocator（推进器与舵面效能）
  -> 舵机/电机输出

这是自动控制主线，手动模式可能绕过部分外环。空速选择、失速相关限制、着陆过程和风估计会影响目标或约束，不能仅复制多旋翼链后改模块名字。

26.4 VTOL 的两条链和过渡协调

当前 rc.vtol_apps 会启动 MC、FW 与 VTOL 协调模块，MC/FW 控制器以相应 VTOL 参数启动。

text 复制代码

MC 状态/目标 -> MC 控制链 -> 虚拟 MC 控制输出 --+
                                               +-> VTOL 协调/过渡
FW 状态/目标 -> FW 控制链 -> 虚拟 FW 控制输出 --+       |
                                                       v
                                          随构型和阶段变化的效能/分配
                                                       |
                                                       v
                                          旋翼 / 推进器 / 舵面 / 倾转机构

虚拟话题、混合关系和控制权限随具体类型而变，应结合 vtol_att_control 与 ActuatorEffectiveness*VTOL 阅读。不要简单同时发布两套普通力矩目标，让接收者"取最后一条"。

26.5 地面车与水下/飞艇/专用载具

阿克曼车：转向角与速度存在非完整运动约束，不能原地独立横移。
差速车：左右驱动力差影响转向，轮地打滑会改变几何模型。
麦克纳姆车：平面全向运动依赖轮组几何与摩擦。
UUV：浮力、阻力、推进器方向与常规多旋翼不同。
飞艇：惯性、阻尼、推力响应和风敏感性不同。
Spacecraft 目录：包含位置、姿态、速率子控制器与专用效能构型；不等于仓库同时提供轨道规划、完整星敏导航等所有航天功能。

设计新载具时，先定义可控自由度、执行器作用方向和环境动力学，再选择可复用的控制层。

26.6 直升机为什么没有一个同名主模块

功能不一定与一级目录一一对应。当前控制分配目录包含直升机、同轴等效能类，机型脚本与参数负责选择对应配置，部分上层控制逻辑可以复用。

学习直升机需要额外理解总距/周期变距、斜盘几何、尾部控制、主旋翼转速以及响应与约束；应从对应 ActuatorEffectivenessHelicopter*、RpmControl 和机型配置向上追踪。

26.7 替代估计器与替代控制器的输出所有权

有多个估计器或多个控制器的源码，不表示可以全部无条件向同一话题发布。应确认实例、选择器、模式注册、输出使能和回退路径。

尤其是直接电机动作的学习控制器，可能绕过标准姿态、角速度和分配链。使用前要重新核对控制输出约束、解锁路径、故障行为和基线比较方法。

二十七、完整导航任务、业务载荷与外部模式

27.1 Navigator 子系统地图

入口	职责
`navigator_main.cpp`、`navigator_mode.*`	导航主循环、模式公共接口
`mission.`、`mission_base.`、`mission_block.*`	任务序列、任务项、到达条件和公共行为
`MissionFeasibility/`、`mission_feasibility_checker.*`	任务可执行性检查
`takeoff.`、`land.`、`vtol_takeoff.*`	不同起降任务
`loiter.*`	停留/盘旋导航行为，实际运动由机型决定
`rtl.*`、`rtl_base.h`	返航方案选择与公共接口
`rtl_direct.*`	直接返航路径
`rtl_direct_mission_land.*`	结合任务降落方案的直接返航
`rtl_mission_fast.`、`rtl_mission_fast_reverse.`	任务路径相关返航策略
`safe_point_land.hpp`	安全点降落相关支持
`geofence.*`、`GeofenceBreachAvoidance/`	围栏状态与边界规避相关逻辑
`precland.*`	精准降落任务状态

这些是导航任务能力，不表示已经有通用的环境感知避障地图。返航路径、围栏、交通信息和局部障碍是不同约束来源。

27.2 FlightTask 全部一级目录的用途

当前任务目录包括：

任务目录	角色
`FlightTask`	公共基类、状态和目标接口
`AltitudeCruise`	对应高度/巡航类运动模式的目标生成
`Auto`	自动航点与起降相关连续目标
`AutoFollowTarget`	跟随目标任务
`Descend`	下降目标策略
`Failsafe`	失败情形下的任务目标策略
`ManualAcceleration`	手动加速度类运动目标
`ManualAccelerationSlow`	低速/受限手动加速度目标
`ManualAltitude`	手动高度类目标基线
`ManualAltitudeSmoothVel`	高度相关速度平滑
`ManualPosition`	手动位置类任务基础
`Orbit`	环绕轨迹目标
`Transition`	VTOL 过渡相关任务
`Utility`	操纵、目标和任务辅助工具

是否有地面站直接显示的同名模式，需要继续看 Commander 与任务选择，不应把基类或辅助目录当作一个用户可选模式。

27.3 精准降落链路

text 复制代码

目标检测设备 / 外部目标观测
  -> 目标观测消息（例如 IRLock 路径）
  -> landing_target_estimator 或相应目标接口
  -> landing_target_pose
  -> Navigator precland 状态与目标
  -> 多旋翼目标生成与控制

设计重点：目标坐标、角度到位置的尺度、传感器外参、观测时间、目标速度、丢失时搜索/等待/回退行为，以及近地测量和下降速度。

只有精准降落状态机，没有可靠目标测量，就无法得到"精准"降落；只有视觉识别，也不等于已完成降落闭环。

27.4 相机的三种时间不能混淆

text 复制代码

任务发出拍照要求
  -> camera_trigger 发出触发
  -> 相机实际曝光（有反馈时由 camera_capture 捕获）
  -> camera_feedback 结合当时车辆状态生成 camera_capture 消息
  -> MAVLink CAMERA_IMAGE_CAPTURED / ULog / 地理标记

相机命令发送、硬件触发、实际曝光可能相差明显。测绘地理标记应尽量使用有效曝光反馈与同步状态，而不是只用任务项执行时刻。

飞控一般处理触发与元信息，不负责从普通相机获取、编码和传输全部视频数据；视频业务多由相机或伴随计算机承担。

27.5 云台架构

gimbal 有独立 input/output 层，支持遥控、MAVLink 等输入和对应输出方式。

需要明确：控制权来源、角度/角速度目标、地理指向、机体姿态补偿、云台反馈、限位和失联动作。云台内置控制器与飞控云台管理是两个层次，不应由两个控制器同时无协调地控制同一轴。

27.6 载荷交付与内燃机

payload_deliverer 把抓取器/绞盘动作视为带状态、超时、反馈与确认的过程，而不是发送一个 PWM 就认定完成。

internal_combustion_engine_control 管理点火、节气门、阻风门、启动机与转速等条件。这属于动力系统状态机；它不替代全机姿态控制，也不自动解决全部发动机健康诊断。

27.7 外部模式不只有 Offboard

当前 ModeManagement.cpp 中可看到：

register_ext_component_request/reply。
unregister_ext_component。
外部解锁检查注册与响应。
模式与模式执行器角色的注册资源、兼容版本及无响应处理。

这里的"模式执行器角色"指 mode executor，不是电机 actuator。外部组件可以参与模式体系与检查，而不只是发送传统 Offboard 目标。

接入时必须核对当前 API 版本和配套库，实现注册失败、组件消失、模式结束与重新接管，不应从单个旧 Offboard 示例推断所有扩展方式。

二十八、通信子协议、跨处理器通信与外部生态

28.1 MAVLink 不只是"发位置的协议"

当前 src/modules/mavlink/ 的文件可以直接映射成子系统：

文件/目录	职责	应检查的协议问题
mavlink_main.cpp	实例、传输、发送与模块管理	串口/UDP、路由、带宽、实例配置
mavlink_receiver.cpp	消息接收与内部转换	目标身份、坐标、掩码、类型、时间
streams	各类遥测消息从 uORB 取数	频率、有效性、带宽优先级
mavlink_command_sender.cpp	外发命令与确认/重试相关处理	幂等、超时、目的组件
mavlink_mission.cpp	任务、围栏/集结点相关任务协议	数量、序号、任务类型、数据一致性
mavlink_parameters.cpp	参数读写与同步	参数类型、名称、确认与保存语义
mavlink_timesync.cpp	时间同步交互	时差、RTT、异常样本与时间基准
mavlink_ftp.cpp	文件传输服务	路径、分块、重试、会话与访问权限
mavlink_shell.cpp	远端 shell 通道	访问边界与串流处理
mavlink_log_handler.cpp	日志列表/下载等协议处理	文件大小、偏移、带宽
mavlink_ulog.cpp	ULog 在线流传输	链路吞吐、掉包和日志模式
mavlink_events.cpp	结构化事件传输	序号、丢失事件、元数据对应
mavlink_sign_control.cpp	签名相关控制	认证与完整性，不等于加密
open_drone_id_translations.cpp	Open Drone ID 数据转换	来源、格式、时间与设备实现
mavlink_rate_limiter.cpp	消息速率限制辅助	避免业务流挤占关键流

有协议处理代码，不等于任何链路都允许或适合开启全部服务。实际部署应按带宽、可信边界和运维需求配置。

28.2 串口配置为何牵涉多个层

text 复制代码

板级定义可用 UART 及设备节点
  -> 模块 YAML 声明可配置串口服务
  -> Tools/serial 生成配置与启动片段
  -> 用户选择端口、速率和协议角色
  -> rc.serial 等启动对应模块
  -> 驱动/协议模块使用端口

两个服务不能未经协调占用同一个 UART；总线共享和半双工等情形需要相应驱动支持。设备线接好但模块未启动、波特率正确但电平不匹配，都会表现为"没有数据"。

28.3 遥控协议到手动飞行意图

text 复制代码

接收机：SBUS / CRSF / DSM / GHST / 其他受支持输入
  -> 相应驱动 / rc_input
  -> 原始通道和接收状态
  -> rc_update：校准、通道映射、开关等处理
  -> manual_control：来源选择、有效性和操作动作
  -> Commander 模式/解锁请求 + 当前控制模式的目标生成

应分别校验端点/中位/死区、通道反向、failsafe 标志、接收中断和模式开关。接收机输出固定通道值并不必然表示链路有效，必须正确使用其失联标志与时间。

28.4 DDS 和 Zenoh 的架构位置

uXRCE-DDS 通常通过外部 Agent 进入 DDS 生态；Zenoh 使用其自己的通信栈与配置。两者都是对外数据通道选择，不是替代飞控内部所有 uORB 功能。

当前 Zenoh 目录有独立 dds_topics.yaml、发布者/订阅者和配置代码。相同消息类型名称并不保证两条链路的路由、序列化、QoS、自动发现和时间处理相同。

外部 ROS 2 节点应处理三种不同故障：传输断开、消息类型/版本不兼容、消息内容虽在更新但已无效。

28.5 muORB 和 qshell

muorb 用于特定多处理器部署中的消息通信，qshell 提供对应环境的命令交互支持。这类机制服务于"同一系统跨处理器运行"，与地面站数传和普通 ROS 2 网络连接不是同一个边界。

需要单独研究跨处理器消息所有权、时间同步、启动顺序、内存复制和其中一侧重启后的行为。

28.6 外部生态的责任划分

外部组件	通常负责	PX4 一侧负责
QGroundControl	配置、任务编辑、显示、日志获取等	参数、任务、状态和命令协议
MAVSDK 应用	通过 API 管理任务与动作	MAVLink 服务和飞行状态
ROS 2 应用	感知、定位、规划、业务节点	选定话题与模式/控制接口
Agent/桥接进程	相应传输与中间件桥接	客户端、话题定义和链路状态
VIO/SLAM	图像/雷达处理、地图、相对位姿	外部观测校验与融合
视频设备	采集、压缩、存储和视频传输	必要的触发、云台与元信息
RTK 基站/改正服务	改正数据生产和配送	接收链路、GNSS 设备和融合质量

整机开发要把外部组件也纳入启动、版本、故障和测试范围，而不是把飞控与伴随计算机之间的串口看作无条件可靠的黑盒。

二十九、健康检查、冗余、诊断与维护架构

29.1 Commander 的主要内部职责

text 复制代码

输入：估计有效性 / 电源 / 遥控 / 链路 / 任务 / 设备 / 外部检查
                     |
                     v
HealthAndArmingChecks：模块健康、允许解锁、模式所需能力
                     |
                     v
用户意图与模式管理：UserModeIntention / ModeManagement / ModeUtil
                     |
                     v
Failsafe：故障组合、延迟、优先级与可执行动作
                     |
                     v
输出：vehicle_status / vehicle_control_mode / actuator_armed / 事件等

另外还有 HomePosition、Safety、FailureDetector 和校准流程。模式管理既需要听取用户意图，又必须遵守当前可用能力。

29.2 当前健康检查覆盖的类别

HealthAndArmingChecks/checks/ 不是只检查"有没有 GPS"，包含以下实际类别：

类别	代表检查文件
惯性与基础传感器	accelerometer、gyro、magnetometer、baro、IMU consistency
导航感知	estimator、airspeed、distance sensor、optical flow
供电与执行器	battery、power、ESC、failure detector
人工与外部输入	manual control、RC calibration、RC/data link、Offboard
导航任务	home position、mission、navigator、rally point、geofence
模式与权限	mode、arm permission、external checks、system
资源与运维	CPU resource、logger、SD card、flight time
专用条件	VTOL、wind、traffic avoidance、parachute、Open Drone ID

检查代码存在不代表每个检查在所有机型都会要求同样条件。应阅读受影响模式、参数、状态和故障报告接口。

29.3 冗余分成四层

设备冗余：多 IMU、多气压计等，仍可能共用电源或总线。
估计冗余：多 EKF 实例使用不同传感器组合，选择器比较健康。
执行器冗余：更多电机/舵面有机会提供剩余控制能力。
系统冗余：处理器、电源、通信和独立保护链路。

只有第一层并不等于第四层。两个 IMU 同时受同一结构振动污染，仍然可能一致地给出错误测量。

29.4 自检、校准和在线估计

自检判断是否处于可运行范围。
校准估计相对固定或缓慢变化的安装/比例/偏置参数。
在线估计追踪运行中的状态与残余偏置。
温度补偿将温度变化与已识别误差关系应用到测量。

不能因为 EKF 能估计偏置，就省略基本传感器校准；也不能通过放宽解锁阈值来替代排查测量异常。

29.5 日志与资源监控的运行结构

logger 使用采集与写入分工，源码说明包含主线程与文件写线程，以减少文件写入对消息收集的影响。仍需考虑缓冲不足、存储抖动和掉日志。

29.6 硬故障、启动恢复和存储

hardfault_log 等命令管理硬故障记录，hardfault_stream 可将已保留信息传到地面。故障现场是否完整取决于平台、故障类型、存储与复位过程。

参数、任务、日志在断电时的行为不同，应分别验证：参数保存中断、任务上传中断、日志写入中断、存储不可用。不要以"一次正常上电能读到参数"代替断电一致性测试。

29.7 固件、引导程序与升级

构建输出通常需要打包元信息、板卡身份和目标配置。Bootloader 用来启动/更新应用，应用中的升级支持和上传工具属于另一部分。

完整升级设计包括：硬件识别、固件兼容、校验、刷写中断恢复、参数迁移、启动成功判据和回退方案。具体板卡是否支持双分区、安全启动或回滚不能从 PX4 总体架构推断，必须检查实现。

三十、构建生成体系与全部系统命令

30.1 从配置到固件的完整链

text 复制代码

Make 入口 / 构建目标名称
  -> boards/<vendor>/<board>/<variant>.px4board
  -> Kconfig 依赖与选项解析
  -> 顶层 CMake + 各模块/库 CMakeLists
  -> 生成消息、参数、事件、模块信息、启动配置等
  -> 编译平台、驱动、库、模块和相关依赖
  -> 链接、ROMFS 处理、资源/大小检查
  -> 固件打包或主机可执行程序

主要入口：、<CMakeLists.txt>、、。

模块 CMake 的 MAIN 是实际命令的重要线索。目录名 fw_lateral_longitudinal_control 对应运行命令 fw_lat_lon_control，events 对应 send_event，说明不能简单从目录名生成启动命令。

30.2 自动生成清单

源数据	生成或消费内容	入口
`.msg`、版本化消息	uORB 类型、元信息和接口相关产物	、<Tools/msg>
参数 YAML/支持的源定义	参数表、文档、可供外部读取的元数据	<Tools/module_config>、<src/lib/parameters>
模块 YAML、串口声明	串口配置与启动相关生成物	<Tools/serial>
机型脚本	自动启动选择、机型元数据等	<Tools/px4airframes>
事件定义和调用	事件元数据	<Tools/px4events>
输出功能/计时器/模块配置	`actuators.json` 等执行器元数据	<src/lib/metadata>
FlightTask 模板与目录	任务注册和选择相关生成代码	generate_flight_tasks.py
EKF 符号推导	状态与观测等生成代码	ekf_derivation

因此，新增一个控制模式可能同时涉及源码、构建、注册、消息与地面站元数据，不能只放一个 .cpp 文件。

30.3 全部 39 个一级系统命令目录

以下是源码命令家族清单，不是建议逐个执行。部分命令会修改参数、设备、存储或重启系统，实际用途需阅读其帮助和源码。

命令目录	主要用途
actuator_test	执行器测试请求与输出验证
bl_update	支持硬件上的 Bootloader 更新
bsondump	查看 BSON 格式内容
dmesg	系统日志/消息查看
dumpfile	文件内容检查/导出辅助
dyn	动态模块相关支持
failure	故障注入请求接口，受构建/配置限制
gpio	GPIO 诊断与控制
hardfault_log	硬故障记录管理
hist	查看已启用日志历史功能时的 PX4 消息历史
i2c_launcher	I²C 驱动启动辅助
i2cdetect	I²C 总线设备探测
io_bypass_control	专用直接 IO/PWM 控制路径；不等同标准受保护控制链
launch_kmod	特定保护构建下启动内核侧模块的内部辅助
led_control	指示灯控制
mft	板级 manifest/设备资源相关服务与状态
mft_cfg	manifest 配置读取和设置
microbench	微基准测试
mtd	MTD 存储设备相关操作
netman	网络配置与管理
nshterm	NSH 终端相关功能
param	参数查看、设置、保存、导入等
perf	性能计数器查看与管理
reboot	系统复位/重启
reflect	终端/USB 数据回送与负载测试
sd_bench	SD 卡性能测试
sd_stress	SD 卡压力测试
serial_passthru	串口透传
serial_test	串口通信测试
shutdown	系统关闭流程
system_time	系统时间查看/设置
tests	目标平台内置测试集合
top	任务与 CPU 运行状态
topic_listener	`listener` 话题内容查看
tune_control	提示音控制
uorb	uORB 状态、统计等入口
usb_connected	USB 连接状态查询支持
ver	版本、硬件与构建身份信息
work_queue	工作队列运行信息

30.4 开发工具与维护流程

工具类别	代表目录/文件	用途
开发环境	`Tools/setup/`、`Tools/docker_run.sh`	环境准备与容器运行
固件打包/上传	`Tools/package_firmware.py`、`Tools/px4_uploader.py`	打包与目标设备上传
MAVLink 交互	`Tools/mavlink_shell.py`、`Tools/mavlink_ulog_streaming.py`	远端控制台与日志流
消息/模块分析	`Tools/uorb_graph/`、`Tools/px4moduledoc/`	依赖图与模块文档生成
资源分析	`Tools/stack_usage/`、`Tools/itcm_check.py`、bloaty 配置	栈、内存和代码布局分析
EKF 分析	`Tools/ecl_ekf/`	估计器日志与分析辅助
相机业务	`Tools/geotag_images_ulog.py`	用日志辅助照片地理标记
规范与校验	clang-tidy、shellcheck、JSON/YAML 校验脚本	静态检查与配置正确性
参数/校准维护	参数迁移与校准处理脚本	数据格式转换和维护
仿真	`Tools/simulation/`、`Tools/HIL/`	启动、模型和仿真集成

工具脚本只是工具箱的一部分，运行前应查看输入输出和依赖，不应为了阅读源码就执行会安装系统包、刷写设备或上传数据的脚本。

30.5 三种配置文件各自管什么

Kconfig：声明可选功能、依赖和配置类型。
.px4board：为特定板卡/变体选择功能。
参数 YAML / 模块 YAML / 机型脚本：声明运行参数、设备配置、元数据和默认启动行为。

运行参数通常不能让一个根本未编译进固件的模块凭空出现。反过来，一个模块已编译，也可能因为启动条件不满足而不运行。

三十一、仿真后端、测试、回放与持续集成

31.1 仿真模块的全部一级子目录

子目录	角色
battery_simulator	电池状态仿真
gz_bridge	Gazebo 与 PX4 状态/传感器/执行器桥接
gz_msgs	Gazebo 消息相关依赖/定义
gz_plugins	Gazebo 插件支持
pwm_out_sim	仿真输出接口
sensor_agp_sim	辅助全局位置类传感器仿真
sensor_airspeed_sim	空速测量仿真
sensor_baro_sim	气压测量仿真
sensor_gps_sim	GNSS 测量仿真
sensor_mag_sim	磁场测量仿真
simulator_mavlink	MAVLink 仿真/HIL 接口
simulator_sih	SIH 动力学与仿真相关处理
system_power_simulator	系统电源状态仿真

这些子目录与外部仿真器构成不同组合，不是需要全部同时启动的串行流水线。

31.2 仿真中的消息来源所有权

仿真时 IMU/GNSS/气压等可能由虚拟设备产生，真实硬件测试则由真实驱动产生。必须明确：

哪个实例是真实测量，哪个是仿真测量。
估计器选择了哪个输入。
电机输出是否发往仿真而不是实际硬件。
真值消息是否仅用于评估，没有绕过估计器偷偷进入控制反馈。
仿真暂停、倍速、时间回跳或重启时模块如何处理。

31.3 锁步和时间一致性

采用锁步的仿真可以让仿真状态推进与飞控计算保持协调，减少主机性能造成的非物理时序。但具体锁步支持取决于后端和配置。

如果目标是测试真实链路延迟，不能因为锁步仿真中表现很好就忽略主机网络、CPU 调度与外部感知的实际耗时。应明确测试是在模拟时间还是实时墙钟条件下进行。

31.4 仿真模型之外还要建"故障模型"

每个接口至少考虑：偏置、白噪声、随机游走、量化、饱和、固定延迟、时变延迟、丢包、重复、断开和错误恢复。

不能只在状态真值上加一个扰动就认为测试了"GPS 丢失"。要把故障放到真实算法消费的测量/状态接口，验证有效性和模式逻辑是否真的走到了预期路径。

31.5 测试类型与位置

测试类型	主要位置	目的
算法单元测试	各模块/库中的 `*Test.cpp`	数学、边界和约束
模块/平台测试	`src/systemcmds/tests/` 等	平台 API、设备与运行行为
MAVSDK 集成	`test/mavsdk_tests/`	外部命令到飞行行为的端到端验证
ROS 集成	`test/ros_tests/`、`integrationtests/`、相关 launch	ROS 接口和任务流程
模糊测试	`test/fuzztest/` 与 CI 配置	异常输入、解析与算法健壮性
Failsafe 仿真	commander 测试与 `failsafe_sim.yml`	故障组合与状态机
编译矩阵	`build_all_targets.yml` 等	不同板卡和配置兼容
静态与格式检查	`.github/workflows/`、Tools	规范、潜在错误、许可证/依赖检查

通过一个 SITL 场景不能证明全部固件配置正确；全目标编译也不能证明某架飞机的闭环性能。

31.6 CI 的架构角色

当前仓库 CI 覆盖构建、编译平台、SITL、ROS、模糊测试、Failsafe、Flash/ITCM 分析、SBOM/许可证、文档和消息同步等。

它把"一个人电脑上能运行"变成可重复的检查流程。自研系统第一版可以从小矩阵开始：核心单元测试、一个 SITL 机型、关键故障任务、参数/消息格式检查和资源预算。

31.7 回放的正确使用

回放必须知道日志包含哪些测量、原始时间戳、参数和初始条件。若日志只记录低频融合结果，不能期望重建完整高频 IMU 融合过程。

比较新旧 EKF 时应固定输入和参数基线，记录创新、有效性、重置与误差；比较控制器时则需要闭环仿真，因为新控制会改变之后的运动与测量。

三十二、跨模块数据流与实际开发追踪方法

32.1 不只有飞行控制这一条主线

一个完整运行系统至少同时存在七条链：

链路	主数据流	主要失效风险
测量链	硬件 → 驱动 → sensors → 估计/控制	时间错、单位错、噪声、失效未上报
控制链	目标 → 外环 → 内环 → 分配 → 输出	不连续、饱和、时延、输出所有权冲突
权限链	状态/输入 → Commander → 控制模式/解锁	错误模式、过期指令、不合理故障动作
任务链	任务协议 → dataman → Navigator → 目标生成	任务不完整、高度基准错、执行状态错
配置链	参数来源 → 存储 → 更新通知 → 模块重新配置	参数版本错、运行状态与参数不一致
诊断链	状态/计数器/事件 → 日志/遥测 → 分析	记录不足、缓冲丢失、版本不可追溯
业务链	任务动作 → 相机/云台/载荷 → 反馈	将命令接受误认为物理动作完成

这些链的依赖关系解释了为什么"控制器公式正确"不等于整个无人机系统正确。

32.2 案例一：解锁命令

text 复制代码

遥控操作或外部命令
  -> manual_control / MAVLink 等相应输入路径
  -> Commander 命令与意图处理
  -> 检查身份、状态、模式、健康与解锁条件
  -> 更新解锁/系统状态或报告拒绝原因
  -> actuator_armed 等状态被输出层消费
  -> 输出层按当前状态决定是否允许正常执行器输出

阅读时分别查看命令确认、系统状态、输出状态与物理响应。命令处理返回成功不代表电机已经达到期望转速；解锁后也可能还处于地面或起飞前阶段。

32.3 案例二：上传并执行任务

text 复制代码

地面站发任务数量和任务项
  -> mavlink_mission 协议状态与确认
  -> dataman / 相关任务元信息
  -> 任务可执行性检查
  -> 模式切换允许并进入任务
  -> Navigator 管理当前任务项
  -> 对应机型的目标生成/引导
  -> 控制与执行
  -> 到达判断、动作反馈、下一项或任务结束

开发任务上传功能时，要测试上传中断、重复项、乱序、任务类型不匹配、旧任务覆盖、新任务未完成上传就请求执行等边界。

32.4 案例三：新定位设备加入系统

text 复制代码

设备输出
  -> 协议驱动/外部接口适配
  -> 单位、坐标、时间和协方差转换
  -> 标准观测话题
  -> EKF 输入缓冲与对应融合源
  -> 观测接受/拒绝、状态与有效性
  -> Commander 对各模式可用性重新评估
  -> 控制器使用状态，必要时响应估计重置

新增 UWB 或其他定位设备时，必须确认设备提供的是距离、局部位置、全局位置还是相对目标位置。它们对应的观测模型不同，不能都无条件塞进 GNSS 消息。

32.5 案例四：参数改变

text 复制代码

地面站 / param 命令
  -> 参数类型、范围与名称处理
  -> 参数内存值更新，按机制保存
  -> parameter_update
  -> 模块 updateParams / parameters_updated
  -> 重算增益、滤波器或其他配置
  -> 状态连续性处理
  -> 日志记录用于之后复现

并非所有参数都能无缝在线改变。几何、设备方向、滤波采样率、估计器配置等可能需要特殊处理或重启，应按参数定义和模块实现执行。

32.6 案例五：起飞后 GNSS 丢失

text 复制代码

GNSS 数据不再更新或不满足质量
  -> EKF 观测源超时/拒绝
  -> 评估其他有效观测：光流、视觉等
  -> 更新位置/速度有效性与估计状态
  -> HealthAndArmingChecks 提供模式能力与故障标志
  -> Failsafe 结合当前模式、参数、其他故障选择动作
  -> 目标和控制层级切换
  -> 事件与日志记录原因和结果

这不是固定地"GPS 一丢就返航"。如果已失去支撑返航的定位，返航本身可能不满足运行前提。应从状态有效性和故障仲裁理解真实行为。

32.7 案例六：多旋翼与固定翼过渡

text 复制代码

任务或人工请求过渡
  -> 模式与 VTOL 状态检查
  -> VTOL 专用过渡状态机
  -> 相应 MC/FW 目标、控制使能和输出协调
  -> 倾转/推力/舵面等效能更新
  -> 控制分配与真实执行器变化
  -> 空速、姿态、时间等完成条件
  -> 正常结束或触发对应失败/回退逻辑

应记录过渡请求、VTOL 状态、MC/FW 目标、倾转角、空速、分配剩余量和完成/失败原因，避免只凭飞机外形姿态判断过渡完成。

32.8 如何从一个话题追到完整调用链

以 vehicle_rates_setpoint 为例，在主机终端做只读检索：

bash 复制代码

rg -n 'vehicle_rates_setpoint' src/modules src/lib msg
rg -n 'ORB_ID\(vehicle_rates_setpoint\)' src/modules src/lib
rg -n 'registerCallback|ScheduleOnInterval|ScheduleDelayed' src/modules/mc_rate_control
rg -n 'Run\(|parameters_updated|setPidGains|publish' src/modules/mc_rate_control

然后人工区分：

include 只是引入类型，不代表发布或订阅。
Publication/Subscription 成员声明表达接口，但不代表每个模式都会执行。
.publish() 的条件和 topic 实例决定真实数据路径。
.update()、.copy()、队列读取决定消费时机。
机型启动参数可能选择 virtual 话题或不同实例。
输出层还可能根据模式/解锁状态抑制作用。

32.9 自动依赖图的用途和局限

仓库提供 <Tools/uorb_graph/create.py>，可从源码生成发布订阅依赖图。适合辅助找候选路径，但静态扫描不完全理解运行条件、宏、动态实例和新消息布局。

当前工具中的话题文件查找逻辑需要结合 msg/versioned/ 实际布局核对；不能把静态图没画出来的边直接认定不存在。

完整架构确认最好组合三类证据：

text 复制代码

静态源码：谁声明、谁发布、谁订阅
构建与启动：哪个实现被编译并启动
运行与日志：实际话题实例、更新率、模式和输出

32.10 为自己的无人机建立一张实际启用表

相比继续泛读整个仓库，选定机型后应该填写：

项目	你的实际选择
Git 提交与依赖版本	精确 commit / 子模块状态
板卡与变体	`vendor/board/variant`
操作系统与启动路径	NuttX/POSIX/具体部署
机型及几何	SYS_AUTOSTART、旋翼位置/方向/编号
传感器实例	型号、device_id、总线、安装方向
估计器	类型、实例、融合来源、延迟
控制路径	标准 MC/FW/VTOL 或自研替换位置
输出	Motor/Servo 功能到物理端口、协议
通信	每个端口的服务、速率、消息、权限
任务与载荷	导航模式、相机、云台、载荷状态机
故障配置	条件、延迟、动作和恢复策略
日志与验证	记录列表、测试场景、通过指标

这张表把"PX4 能做什么"转化为"我的这架飞机实际上如何工作"。

三十三、架构覆盖审计与后续维护

33.1 本次补充的可核对范围

以文首提交为基线，一级目录定义为对应父目录下的直接子目录，不包含 Kconfig 等普通文件，不按文件数量统计，也不递归计算第三方依赖的内部目录。

检查项	当前源码数量	本文对应章节	覆盖口径
`src/modules/` 一级子目录	57	第 26 章	每个目录独立列出职责与启用边界
`src/drivers/` 一级子目录	57	第 24 章	每个类别独立列出，芯片型号从目录继续下钻
`src/lib/` 一级子目录	61	第 25 章	每个基础库独立列出职责
`src/systemcmds/` 一级子目录	39	第 30 章	每个命令家族独立列出用途
`src/modules/simulation/` 一级子目录	13	第 31 章	每个仿真组件独立列出
`flight_mode_manager/tasks/` 一级子目录	14	第 27 章	包括基类与工具，不等于 14 个用户模式
原文独立公式块	43	第 1--21 章	全部改为纯文本代码块
原文 Mermaid 图	2	第 1、19 章	全部改为普通文本图

这份清单证明上述层级没有漏项；不把"逐项列出一级组件"包装为"全部寄存器、协议细节和机型均已逐行解释"。具体设备、构型和目标固件的深入解析，应沿各章节提供的源码入口继续。

基于 PX4 的全仓库架构、无人机算法解析与开发学习规划

阅读与公式显示说明

目录

一、先理解目标：什么是完整无人机系统

1.1 推荐的总体技术路线

1.2 一条典型的多旋翼自动飞行链路

二、当前仓库具有哪些能力

2.1 功能版图

2.2 不应误认为"PX4 已自动解决"的事情

2.3 版本阅读原则

三、先做整机需求与硬件设计

3.1 先写需求表，再选算法

3.2 动力与能源的第一轮计算

3.3 建议的硬件分层

3.4 硬件问题会直接变成算法问题

四、PX4 软件架构、启动与实时调度

4.1 仓库地图

4.2 启动链应该怎样读

4.3 uORB：内部模块的数据总线

4.4 模块外壳与纯算法分离

4.5 Work Queue 与实时执行

4.6 建议的初始时序预算

五、坐标系、时间与动力学

5.1 必须先固定的约定

5.2 高度有多种含义

5.3 四旋翼教学动力学

5.4 时间是状态估计和控制的一部分

六、传感器、校准与信号处理

6.1 数据进入 PX4 的流程

6.2 IMU：为什么它最重要

6.3 滤波怎样设计

6.4 其他传感器的作用与限制

6.5 驱动验收不是"有数值"就结束

七、状态估计与 EKF2

7.1 估计器与控制器各自做什么

7.2 当前 EKF2 的代码分层

7.3 状态向量：不要死记旧版本"几维 EKF"

7.4 EKF 的数学主线

7.5 EKF 的难点在工程条件

7.6 关键输出与诊断

7.7 自己实现估计器的顺序

八、光流、测距、视觉里程计与无 GPS 定位

8.1 光流究竟测到了什么

8.2 当前 PX4 光流数据链

8.3 消息字段必须正确理解

8.4 和源码对应的简化观测模型

8.5 光流调通流程

8.6 光流和 VIO/SLAM 如何选择

8.7 外部视觉接入

九、模式、状态机与指令权限

9.1 四个经常被混淆的概念

9.2 源码责任划分

9.3 FlightTask 的作用

9.4 自研状态机的最小范围

十、航线、航点、轨迹与自主规划

10.1 把"航线规划"分成五层

10.2 PX4 航点任务链路

10.3 当前源码也存在 Goto 目标平滑

10.4 自己设计航点执行器

10.5 从直线轨迹到平滑轨迹

10.6 覆盖航线：测绘和巡检的例子

10.7 全局规划如何选

10.8 局部避障必须包含"来得及停下"

10.9 规划器放在哪里

十一、位置和速度控制

11.1 输入和输出

11.2 为什么位置环外面不能再堆一层随意 PID

11.3 PositionControl::update() 按函数拆解

11.4 加速度为何能转成姿态

11.5 推力饱和与抗积分饱和

11.6 NaN 是接口语义，不只是错误

11.7 悬停推力估计与起飞管理

十二、姿态和角速度控制

12.1 级联关系

12.2 姿态控制源码

12.3 为什么不用欧拉角直接相减

12.4 角速度控制源码

12.5 积分为什么要和分配器通信

12.6 调参顺序与判据

十三、电机分配与执行器输出

11.3 `PositionControl::update()` 按函数拆解