夜雨聆风
前四篇拆完了整条执行链：传感器数据进来，EKF2算姿态位置，uORB传递消息，四层串级PID把位置指令变成电机PWM。但有一个问题一直没碰：谁告诉PID该干什么？
PID会悬停，但谁决定"现在悬停"？PID会飞航线，但谁决定"飞这条航线"？遥控器信号断了，谁决定返航还是降落？电池快没电了，谁决定紧急着陆？
答案是一个叫Commander的模块。它是PX4的决策中心——不负责具体怎么飞，负责决定飞什么模式、执行什么任务、什么时候该放弃任务保命。
如果说四层PID是飞机的肌肉和脊髓反射，Commander就是大脑皮层。它做的决策决定了PID的输入是什么，而PID只知道跟踪设定值，不知道为什么是这个设定值。
这篇拆Commander：状态机怎么跳转，飞行模式怎么切换，解锁/上锁的逻辑，以及那些在关键时刻保命的failsafe机制。
Commander在架构里的位置
先看Commander和前面四篇拆过的模块是什么关系：
遥控器/地面站
│
▼
Commander ←── Navigator（航线规划）
│ │
│ trajectory_setpoint
│ │
▼ ▼
FlightMode选择 位置控制器
│ │
▼ ▼
设定值生成 速度控制器
│ │
▼ ▼
attitude_sp 姿态控制器
│ │
▼ ▼
rates_sp 角速率控制器
│
▼
混控器→电机
Commander不直接碰控制环路。它做的事情是：
状态管理：飞机当前是DISARMED、ARMED还是IN_AIR？能不能切换？ 模式选择：当前飞Position模式、Altitude模式还是Manual模式？ 安全决策：遥控器断了怎么办？电池低了怎么办？GPS丢了怎么办？ 设定值路由：不同模式下，PID的设定值从哪来？遥控器杆量？Navigator航线？Land模式？
Commander跑在主循环里，频率约4-10Hz——比角速率控制器的250Hz慢很多，因为决策不需要那么快。但Commander的优先级很高，它必须在每个周期都跑完，不能被其他模块饿死。
三个主状态：DISARMED → ARMED → IN_AIR
Commander的核心是一个三态状态机。PX4把飞机的生命周期分成三个阶段：
DISARMED ──arm──► ARMED ──takeoff──► IN_AIR
▲ │ │
│ │disarm │
└──disarm───────┘◄──land/descend─────┘
三个状态的含义：
DISARMED → ARMED：解锁条件
解锁是飞控最关键的安全动作。PX4对解锁有一堆前置检查，不通过就拒绝解锁：
// src/modules/commander/Arming/PreFlightCheck.cpp
// 简化的解锁检查逻辑
boolPreFlightCheck::armCheck(vehicle_status_s &status)
{
bool can_arm = true;
// 1. 传感器健康检查
if (!sensor_health_check()) {
can_arm = false;
// 至少需要：一个陀螺仪、一个加速度计、一个磁力计
// 不需要GPS（取决于模式），但GPS不健康时限制只能Manual/Altitude
}
// 2. 电池检查
if (battery_remaining < ARM_BAT_MIN) {
can_arm = false;
// 默认5%以下不允许解锁
}
// 3. 遥控器校准检查
if (!rc_calibration_valid()) {
can_arm = false;
}
// 4. 全局位置检查（如果需要GPS模式）
if (requires_global_pos(nav_state) && !global_position_valid()) {
can_arm = false;
// Position模式必须有GPS，Manual/Altitude模式不需要
}
// 5. 地理围栏检查
if (geofence_breached()) {
can_arm = false;
}
// 6. 安全开关（硬件）
if (safety_button_pressed()) {
can_arm = false;
// 有些飞控板上有物理安全开关，必须先按一下
}
return can_arm;
}
每个检查失败都有对应的错误码，地面站（QGC）会显示具体原因。最常见的不解锁原因：GPS没搜到星、磁力计没校准、遥控器没对频。
还有一个很多人不知道的细节：PX4支持"强制解锁"（COM_ARM_WO_GPS=1），允许在没有GPS的情况下解锁。此时只能飞Manual或Altitude模式，不能飞Position模式。调试时很方便，飞行时不建议——没有GPS就没有位置保持能力。
ARMED → IN_AIR：起飞判定
飞机什么时候算"在天上"？不是电机转了就算，也不是离地就算。PX4用几个条件综合判断：
// src/modules/commander/Commander.cpp
boolCommander::is_in_air()
{
// 条件1：垂直速度 > 阈值（正在上升）
bool climbing = local_pos.vz < -0.5f; // NED坐标系Z向下，上升时vz<0
// 条件2：离地高度 > 阈值
bool above_ground = local_pos.z < -0.3f; // NED坐标Z向下，高度正值时z<0
// 条件3：推力显著大于0
bool thrust_up = thrust_sp > 0.1f;
// 综合判断：至少满足两个
int conditions_met = climbing + above_ground + thrust_up;
return conditions_met >= 2;
}
为什么不只用高度？因为在风浪中的船上起飞时，船在晃，高度测量有噪声。单一条件容易误判。多个条件综合，加上一个0.5秒的确认延迟，避免瞬态误触发。
IN_AIR → ARMED：降落判定
降落比起飞更复杂——飞机什么时候算"落地"？PX4用了一个很朴素的方法：
boolCommander::detect_landing()
{
// 条件1：垂直速度接近0（没有在动）
bool not_moving_vert = fabsf(local_pos.vz) < 0.2f;
// 条件2：高度接近0（在地面）
bool near_ground = fabsf(local_pos.z) < 0.3f;
// 条件3：推力很低（电机基本没出力）
bool low_thrust = thrust_sp < 0.1f;
// 必须同时满足，持续1秒
staticint landing_counter = 0;
if (not_moving_vert && near_ground && low_thrust) {
landing_counter++;
if (landing_counter > 50) { // 50Hz × 1秒
returntrue;
}
} else {
landing_counter = 0;
}
returnfalse;
}
1秒的确认延迟很重要。降落时飞机可能弹跳——触地后又短暂离地。如果一触地就判定着陆并上锁，弹跳时电机已经停转，飞机失控坠落。等1秒确认不再弹跳再上锁，安全得多。
飞行模式：Commander怎么选让PID干什么
PX4支持十几种飞行模式，但Commander同一时间只激活一个。不同模式下，设定值的来源和PID的配置完全不同。
核心模式分类
按自动化程度从低到高：
模式切换：设定值的过渡
模式切换时最危险的事情是设定值跳变。比如从Position模式切到Manual，Position模式下飞机在悬停（设定值=当前位置），Manual模式设定值直接来自遥控器杆量——如果杆在中位，设定值突然变成0角速率，飞机会猛烈修正姿态。
PX4的做法是在模式切换时做设定值平滑过渡：
// 模式切换时的设定值初始化
voidFlightMode::on_activation()
{
// 新模式的初始设定值 = 当前实际值
// 不是从0开始，而是从飞机当前状态开始
_pos_sp = _current_pos; // 位置设定值 = 当前位置
_vel_sp = _current_vel; // 速度设定值 = 当前速度
_att_sp = _current_att; // 姿态设定值 = 当前姿态
_rates_sp = _current_rates; // 角速率设定值 = 当前角速率
}
这个看起来简单，实际上是飞控安全的基石。没有这个初始化，任何模式切换都是一次设定值阶跃，四层PID会全力响应这个阶跃，产生剧烈的机动动作。
Hold模式：最简单的"自动驾驶"
Hold模式（也叫Loiter）是Position模式的自动版——不需要遥控器输入，飞机自动保持在当前位置。
voidFlightTaskHold::activate()
{
// 设定值 = 当前位置（锁定）
_position_setpoint = _current_position;
// 速度设定值 = 0（静止）
_velocity_setpoint.setZero();
// 如果有风偏，位置控制器+速度控制器会自动补偿
}
Hold模式没有导航逻辑，没有路径规划，就是"待在这别动"。但它是最常用的模式之一——遥控器松手就进Hold，GPS恢复后进Hold，Mission暂停也进Hold。
Failsafe：Commander的保命逻辑
Failsafe是Commander最关键的职责。飞行中出了问题，Commander必须在几百毫秒内做出决策：继续飞？返航？还是直接降落？
PX4的failsafe不是单一机制，而是一套多层级的响应链：
第一层：RC丢失
遥控器信号丢失是最常见的failsafe触发。
// src/modules/commander/Commander.cpp
// 简化的RC丢失处理逻辑
voidCommander::check_rc_failsafe()
{
// 遥控器数据超时（COM_RC_LOSS_T，默认0.5秒没收到信号）
if (hrt_elapsed_time(&_rc_last_timestamp) > _param_com_rc_loss_t.get() * 1e6f) {
_rc_lost = true;
// 触发failsafe前先Hold一段时间（COM_FAIL_ACT_T，默认0秒即立即执行）
// 这段时间内等RC恢复，恢复则取消failsafe
// 根据NAV_RCL_ACT配置决定动作
// 选项：Disabled / Loiter / Return / Land / Disarm / Terminate
switch ((link_loss_actions_t)_param_nav_rcl_act.get()) {
caselink_loss_actions_t::DISABLED: // 不处理
break;
caselink_loss_actions_t::LOITER: // 原地悬停
set_nav_state(vehicle_status_s::NAVIGATION_STATE_LOITER);
break;
caselink_loss_actions_t::RETURN: // 返航（默认）
set_nav_state(vehicle_status_s::NAVIGATION_STATE_RTL);
break;
caselink_loss_actions_t::LAND: // 降落
set_nav_state(vehicle_status_s::NAVIGATION_STATE_LAND);
break;
caselink_loss_actions_t::DISARM: // 立即上锁
disarm();
break;
caselink_loss_actions_t::TERMINATE: // 飞行终止（所有PWM输出设为failsafe值）
terminate_flight();
break;
}
} else {
_rc_lost = false;
}
}
第二层：数据链丢失
数据链是地面站和飞机之间的遥测链路。数据链丢了不直接影响飞行——遥控器还在，手动控制没问题。但如果飞的是Mission模式（依赖地面站上传航线），数据链断了就意味着航线更新不了。
PX4对数据链丢失的处理比RC丢失宽松：默认只是报警，不切换模式。因为手动控制仍然可用。
第三层：GPS丢失
GPS丢失对不同模式的影响不同，由COM_POSCTL_NAVL参数决定降级策略：
voidCommander::check_gps_failsafe()
{
if (_gps_lost) {
if (_param_com_posctl_navl.get() == 0) {
// 假设RC可用：降级到Altitude模式
if (_nav_state == NAVIGATION_STATE_POSCTL ||
_nav_state == NAVIGATION_STATE_ORBIT) {
set_nav_state(NAVIGATION_STATE_ALTCTL);
}
// Mission/Return也降级到Altitude（仍有手动控制能力）
} else {
// 假设RC不可用：降级到Land
if (_nav_state == NAVIGATION_STATE_AUTO_MISSION ||
_nav_state == NAVIGATION_STATE_RTL ||
_nav_state == NAVIGATION_STATE_POSCTL) {
set_nav_state(NAVIGATION_STATE_LAND);
// 无GPS且无RC，无法导航也无法手动控制，只能就地降落
}
}
}
}
注意这个降级逻辑：Position→Altitude→Land，是一个逐步失去控制能力的链条。GPS丢了还能维持高度控制，但连气压计都出了问题就只能在Manual模式下靠遥控器硬飞。
第四层：电池failsafe
电池是最不能等的failsafe。电压掉到临界值后，几秒内就会彻底断电。
PX4的电池failsafe由COM_LOW_BAT_ACT参数控制动作模式，三个阈值参数定义触发级别：
// 电池阈值参数（归一化0-1，不是百分比）
// BAT_LOW_THR = 0.15 (15%) — 低电量警告
// BAT_CRIT_THR = 0.07 (7%) — 严重低电量
// BAT_EMERGEN_THR = 0.05 (5%) — 紧急电量
voidCommander::check_battery_failsafe()
{
float battery_remaining = _battery.remaining;
// COM_LOW_BAT_ACT控制动作策略：
// 0 = Warning only
// 1 = Return（达到BAT_CRIT_THR时返航）
// 2 = Land（达到BAT_CRIT_THR时降落）
// 3 = 分级触发（推荐）：WARN→Return→Land按三个阈值分别触发
if ((low_bat_action_t)_param_com_low_bat_act.get() == low_bat_action_t::CASCADE) {
// 分级触发模式（推荐配置）
if (battery_remaining < _param_bat_emergen_thr.get()) {
// 紧急（5%）：Land
set_nav_state(NAVIGATION_STATE_LAND);
}
elseif (battery_remaining < _param_bat_crit_thr.get()) {
// 严重（7%）：Return
set_nav_state(NAVIGATION_STATE_RTL);
}
elseif (battery_remaining < _param_bat_low_thr.get()) {
// 低电（15%）：Warning only
mavlink_log_warning("LOW BATTERY");
}
}
else {
// 单一动作模式：达到BAT_CRIT_THR时执行指定动作
if (battery_remaining < _param_bat_crit_thr.get()) {
switch ((low_bat_action_t)_param_com_low_bat_act.get()) {
caselow_bat_action_t::RETURN:
set_nav_state(NAVIGATION_STATE_RTL);
break;
caselow_bat_action_t::LAND:
set_nav_state(NAVIGATION_STATE_LAND);
break;
caselow_bat_action_t::WARNING:
mavlink_log_warning("LOW BATTERY");
break;
}
}
}
}
三级电池failsafe的设计思路：低电警告、严重返航、紧急降落。推荐使用分级触发模式（COM_LOW_BAT_ACT=Cascade），这样15%先提醒你，7%自动返航，5%强制降落。
电池failsafe有一个容易被忽略的问题：电池百分比是怎么算的？PX4默认用电压估算，但锂电池的电压-容量曲线不是线性的——满电4.2V很快掉到3.7V，然后3.7V到3.3V之间很长一段"平台期"，最后3.3V以下又快速崩塌。这意味着电压法估算的百分比在平台期偏高，在崩塌期偏低。
如果用电流积分法（库仑计），精度会好很多，但需要额外的电流传感器。大多数飞控板有电流传感器，PX4也支持电流积分，默认两种方法取加权平均。
第五层：地理围栏
地理围栏是一个虚拟的飞行区域边界。飞机飞出这个区域，Commander强制返航或降落。
voidCommander::check_geofence()
{
// 检查是否违反围栏
if (geofence_breached(_current_position)) {
// 围栏违反动作
switch (_params.gf_action) {
case0: // 报警 only
break;
case1: // Loiter
set_nav_state(NAVIGATION_STATE_LOITER);
break;
case2: // Return
set_nav_state(NAVIGATION_STATE_RTL);
break;
case3: // Terminate（强制上锁，极端情况）
disarm();
break;
}
}
}
围栏可以设最大半径（圆形围栏）和最大高度（垂直围栏）。圆形围栏以起飞点为圆心。围栏的违反检测很简单：当前位置到起飞点的水平距离 > 最大半径，或者当前高度 > 最大高度。
有一个边界情况：飞机在围栏外解锁。这种情况Commander会拒绝解锁——你还没起飞就已经犯规了。
Failsafe优先级
多个failsafe同时触发时，谁说了算？PX4按动作严重程度处理，不是按触发类型排序：
None/Disabled < Warning < Hold < Return < Land < Disarm < Flight Termination
更严重的动作会覆盖较轻的。比如RC丢失设了Return，同时GPS丢失导致Land——Land比Return严重，所以执行Land。
实际执行时还要考虑模式可达性。比如RC丢失触发Return，但GPS同时丢了——Return模式需要GPS，无法执行，会降级为Land。这不是优先级覆盖，而是模式不可达时的自动降级。
还有一点：PX4触发failsafe时，会先进入Hold模式等待COM_FAIL_ACT_T秒（默认0秒即立即执行），给用户一个窗口发现并手动接管。在这个Hold窗口里，移动RC摇杆不会取消failsafe——必须通过切换模式来接管。
Return模式：回家的路
Return（RTL，Return To Launch）是最复杂的自动模式。它的执行分多个阶段，而且返航高度的判断比"爬到60m再飞回去"要复杂——PX4引入了"锥角"概念：
返航高度和锥角
多旋翼的返航高度由两个参数决定：RTL_RETURN_ALT（默认60m）和RTL_CONE_ANG（默认0度，即不启用锥角）。
锥角=0度（默认）：直接爬升到 RTL_RETURN_ALT（60m）再飞回去。如果已经高于60m，保持当前高度。锥角>0度：以Home点为顶点，画一个半锥角为 RTL_CONE_ANG的锥面。飞机如果在锥面内，可以沿着锥面斜线飞回去而不用爬到60m；在锥面外，则先爬到60m。锥角=90度：不爬升，保持在当前高度和 RTL_DESCEND_ALT（默认30m）中较高的那个飞回去。
锥角的设计是为了在Home点附近（比如飞出去只有20m远时触发RTL）不用爬到60m再飞回来——那样太浪费时间。锥角允许近距离时用较低的高度返回。
RTL的执行阶段
voidFlightTaskRTL::update()
{
switch (_rtl_state) {
case RTL_STATE_RISE: // 阶段1：爬升
_position_setpoint(2) = _rtl_altitude; // 目标高度
_position_setpoint(0) = _current_pos(0); // 水平位置不变
_position_setpoint(1) = _current_pos(1);
// 到达返航高度后 → 阶段2
if (reached_altitude()) {
_rtl_state = RTL_STATE_RETURN;
}
break;
case RTL_STATE_RETURN: // 阶段2：水平飞行
_position_setpoint(0) = _home_pos(0); // 目标：Home点
_position_setpoint(1) = _home_pos(1);
_position_setpoint(2) = _rtl_altitude; // 保持高度
// 到达Home上方后 → 阶段3
if (reached_home_xy()) {
_rtl_state = RTL_STATE_DESCEND;
}
break;
case RTL_STATE_DESCEND: // 阶段3：下降到RTL_DESCEND_ALT
_position_setpoint(0) = _home_pos(0);
_position_setpoint(1) = _home_pos(1);
_position_setpoint(2) = _rtl_descend_alt; // 降到30m
// 降到RTL_DESCEND_ALT → 阶段4（悬停等待）
if (reached_descend_alt()) {
_rtl_state = RTL_STATE_LOITER;
}
break;
case RTL_STATE_LOITER: // 阶段4：悬停等待
// 在RTL_DESCEND_ALT高度悬停RTL_LAND_DELAY秒
// RTL_LAND_DELAY = -1：无限悬停
// RTL_LAND_DELAY = 0（默认0.5s）：几乎直接降落
if (loiter_time_expired()) {
_rtl_state = RTL_STATE_LAND;
}
break;
case RTL_STATE_LAND: // 阶段5：降落
// 切换到Land模式的逻辑
break;
}
}
比简单的"爬升→飞行→降落"多了两个阶段：先降到30m悬停一会（放起落架的时间窗口），然后再降落。RTL_LAND_DELAY默认0.5秒——几乎直接降落，但如果你想给放起落架留时间，可以设长一些。设-1则永远悬停等你手动接管。
Land模式：降落的两种策略
PX4有两种降落策略：普通降落和精密降落。
普通降落
普通降落就是垂直下降到地面，然后检测着陆并上锁。
voidFlightTaskLand::update()
{
// 目标位置：当前水平位置不变，高度持续下降
_position_setpoint(0) = _current_pos(0); // 保持水平位置
_position_setpoint(1) = _current_pos(1);
_position_setpoint(2) = _land_start_alt - _descent_distance;
// 下降速度：MPC_LAND_SPEED（默认0.7m/s）
// 下降速度恒定，不会越降越快
_velocity_setpoint(2) = _params.land_speed;
// 接近地面时减速
float alt_above_ground = -_current_pos(2); // NED坐标Z向下
if (alt_above_ground < 2.0f) {
// 最后2米降到0.5m/s
float speed_scale = alt_above_ground / 2.0f;
_velocity_setpoint(2) = 0.5f * speed_scale;
}
}
接近地面减速是防止"地面效应"——多旋翼在接近地面时，气流被地面反射产生额外升力，导致飞机弹跳。减速可以让飞机温和触地。
精密降落
精密降落（PrecLand）用视觉或红外信标精确定位着陆点，可以实现厘米级降落精度。典型场景：降落在移动的起降平台上。
精密降落不是Commander的功能，而是一个独立的模块landing_target_estimator。但Commander会根据精密降落的状态决定是否启用它：
// 如果精密降落可用且目标被锁定，使用精密降落
if (precland_available && landing_target_locked) {
// 切换到精密降落模式
// 水平设定值来自视觉/信标，不再是保持当前位置
}
精密降落是PX4比较新的功能，代码在src/modules/landing_target_estimator/和src/modules/precland/。目前支持的硬件：IR-LOCK红外信标、AprilTag视觉标签。
解锁/上锁：Commander的最后一道门
Commander不仅管飞行中的状态切换，还管解锁和上锁。这是飞控最基本的安全功能——确保飞机不会意外启动电机。
解锁触发方式
PX4支持多种解锁方式：
遥控器开关：映射一个通道到arm开关 遥控器杆量：油门最低+偏航最右，持续1.5秒（经典方式） 地面站指令：QGC点击"Arm"按钮 MAVLink命令： MAV_CMD_COMPONENT_ARM_DISARMMission自动解锁：Mission模式第一条指令是takeoff时自动解锁
杆量解锁的逻辑：
voidCommander::check_stick_arm_disarm()
{
// 解锁条件：油门最低 + 偏航最右 + 持续1.5秒
if (throttle_stick < 0.05f && yaw_stick > 0.9f) {
if (!_arm_button_pressed) {
_arm_button_pressed = true;
_arm_button_timestamp = hrt_absolute_time();
}
if (hrt_elapsed_time(&_arm_button_timestamp) > 1500000) { // 1.5秒
arm();
_arm_button_pressed = false;
}
}
// 上锁条件：油门最低 + 偏航最左 + 持续1.5秒
elseif (throttle_stick < 0.05f && yaw_stick < -0.9f) {
// 同上逻辑，disarm()
}
else {
_arm_button_pressed = false;
}
}
1.5秒的延时是防止误触。但有人觉得太慢，改COM_DISARM_PRFLT参数缩短。不建议——1.5秒的误触保护救过很多飞机。
自动上锁
飞机降落后不需要手动上锁，PX4可以自动上锁：
// 降落后自动上锁（COM_DISARM_LAND参数）
// 默认0 = 禁用，设2.5表示着陆后2.5秒自动上锁
if (_params.auto_disarm_land > 0 && _landed) {
if (hrt_elapsed_time(&_landed_timestamp) > _params.auto_disarm_land * 1e6f) {
disarm();
}
}
自动上锁的好处是防止降落后忘了上锁，有人碰到遥控器导致飞机意外启动。坏处是如果降落不完美需要复飞，自动上锁会把电机停掉。建议设2-3秒，给自己留一点复飞的时间窗口。
Home点：比你想的更重要
Home点是返航的目标位置，也是地理围栏的圆心，还是很多failsafe的参考点。Home点的设定逻辑：
voidCommander::set_home_position()
{
// 首次解锁时设定Home点
if (!_home_set && _armed && _global_position_valid) {
_home_pos = _current_global_pos;
_home_alt = _current_altitude;
_home_set = true;
// 发布Home点（其他模块可以用）
_home_position_pub.publish(_home_pos);
}
}
Home点设定条件：
必须已解锁：没解锁不设Home 必须有GPS：没有经纬度没法设Home 只设一次：解锁后GPS精度稳定了才设，不会每秒更新
这意味着：如果起飞后GPS漂移了，Home点不变。好的一面是返航目标稳定；坏的一面是如果GPS起飞时有偏差，Home点就是偏的。建议起飞前等GPS精度收敛（HDOP<2）再解锁。
还有一个问题：室内没有GPS怎么办？PX4在无GPS模式下使用本地坐标系原点作为Home点。但本地坐标系的原点取决于EKF2的初始化——没有GPS时，EKF2在解锁时把当前位置设为原点。所以室内飞行的Home点仍然是解锁时的位置，只是精度不如GPS。
Commander的状态发布
Commander不仅要管理状态，还要把状态告诉其他模块。它通过uORB发布多个主题：
vehicle_status | ||
vehicle_status_flags | ||
home_position | ||
vehicle_control_mode | ||
actuator_armed |
其中vehicle_status是最重要的——几乎每个模块都会订阅它来决定自己的行为。比如Navigator看到nav_state=MISSION就开始执行航线，看到nav_state=RTL就开始计算返航路径。
Commander本身不产生控制指令，它只改变"导航状态"。具体的控制逻辑由FlightTask和Navigator实现，Commander负责选择哪个FlightTask激活。
代码阅读路径
Commander的代码比控制器更散，因为状态管理的逻辑天然是分支驱动的。按这个顺序读：
src/modules/commander/Commander.cpp— 主循环，run()方法。看状态机的主干逻辑。src/modules/commander/Arming/PreFlightCheck.cpp— 解锁前置检查。看为什么不让解锁。src/modules/commander/state_machine_helper.cpp— 状态转换函数。看DISARMED/ARMED/IN_AIR的跳转条件。src/modules/flight_mode_manager/— 飞行模式管理。看模式切换时设定值怎么过渡。src/modules/navigator/— Navigator模块。看Mission和RTL的路径规划。
Commander的主循环很长（2000+行），但逻辑是线性的：每个循环周期依次检查RC状态→传感器健康→电池→地理围栏→模式切换→状态发布。不用一次读完，按你关心的failsafe类型去找对应函数。
和ArduPilot的对比
最显著的差异在failsafe的架构：PX4集中管理，ArduPilot分散管理。集中管理的优点是逻辑清晰、调试方便——所有failsafe决策都在一个地方。分散管理的优点是模块独立——电池模块自己就能触发failsafe，不依赖Commander。
两种架构各有道理。PX4的选择更符合它"模块间通过uORB通信"的整体哲学——Commander订阅各种状态主题，集中决策。ArduPilot的选择更符合它"每个模块自给自足"的哲学——电池模块自己判断该不该failsafe。
最后
Commander拆完了，整个PX4源码深读系列从传感器到决策层的链路就完整了：
第一篇：数据流全貌——12个核心模块怎么串起来 第二篇：EKF2——大脑怎么把原始传感器数据变成姿态位置 第三篇：uORB——消息在内核里怎么飞 第四篇：四层串级PID——位置指令怎么一步步变成电机PWM 第五篇：Commander——谁在指挥PID干活，出了事谁保命
如果用一句话概括Commander：它不飞飞机，它决定飞机怎么飞。 所有的控制逻辑都在别的模块里，Commander只做选择——选模式、选设定值来源、选failsafe动作。但这些选择，决定了飞机是安全回家还是失控坠毁。
Commander的代码不好读，因为它是一个大状态机，充满了条件分支和时序逻辑。但它的设计思路很清晰：集中决策、状态驱动、安全优先。理解了这个思路，看代码就是在找对应的状态转换条件。
代码基于PX4 v1.14，Commander模块路径：src/modules/commander/，FlightTask路径：src/modules/flight_mode_manager/，Navigator路径：src/modules/navigator/
