PX4 Commander 核心调度

发表于 更新于 分类于 ， ， 阅读次数： 本文字数： 414 阅读时长 ≈ 1 分钟
深入解析 Commander.cpp 的核心函数：arm/disarm 完整流程、自动锁定逻辑 handleAutoDisarm()、模式意图与故障安全合成函数 handleModeIntentionAndFailsafe()，以及 MAVLink 命令处理 handle_command()。

一、Commander 主类结构

Commander 继承 ModuleBase<Commander> 与 ModuleParams，以单进程内控制平面的形式持有所有子系统成员：

1
2
3
4
5
6
7
8
9
10
11
12
13
class Commander : public ModuleBase<Commander>, public ModuleParams {
    // 子系统成员（均以值/引用持有）
    Failsafe              _failsafe;
    FailureDetector       _failure_detector;
    HealthAndArmingChecks _health_and_arming_checks;
    HomePosition          _home_position;
    ModeManagement        _mode_management;
    UserModeIntention     _user_mode_intention;
    Safety                _safety;
    WorkerThread          _worker_thread;
    MulticopterThrowLaunch _throw_launch;
    // ...
};

主循环 100 Hz，状态发布 2 Hz 或状态变化时立即发布。

前置知识：模式需求位图，Commander 模块框架

二、run() 中央调度顺序

1
2
3
4
5
6
7
8
9
每次循环：
  1. 更新系统状态与输入源
     （电源/landed/安全按钮/VTOL/HOME/LINK/RC/Offboard……）
  2. 更新失败检测、模式管理
  3. ★ failsafe/模式意图合成（handleModeIntentionAndFailsafe）
  4. 跑健康检查（10 Hz）
  5. 处理命令/动作请求
  6. 把三大状态按 2 Hz 或状态变化立即发布
     actuator_armed → vehicle_control_mode → vehicle_status

三、arm() 完整流程

触发来源：

  • action_request：RC 手势/开关/按钮生成的动作请求指令
  • vehicle_command：MAVLink COMPONENT_ARM_DISARM 等命令
  • 其他内部路径：任务启动等
1
2
3
4
5
6
7
8
9
flowchart TD
    A["arm() 入口"] --> B["检查 arming_state 不是 ARMED"]
    B --> C["HealthAndArmingChecks::canArm() 强制跑一次"]
    C --> D{"通过检查?"}
    D -->|"No"| E["返回 TRANSITION_DENIED<br/>记录拒绝原因"]
    D -->|"Yes"| F["验证油门位置<br/>（RC 模式需要油门最低）"]
    F --> G["_vehicle_status.arming_state = ARMED"]
    G --> H["发布 actuator_armed.armed=true"]
    H --> I["返回 TRANSITION_CHANGED"]

四、disarm() 完整流程

空中门禁加强制路径：

1
2
3
4
5
6
7
8
9
flowchart TD
    A["disarm() 入口"] --> B{"forced = true?"}
    B -->|"Yes（kill/lockdown/failsafe）"| C["跳过空中门禁检查"]
    B -->|"No（正常 disarm）"| D{"is_landed?"}
    D -->|"No（空中）"| E["返回 TRANSITION_DENIED<br/>不允许空中手动锁定"]
    D -->|"Yes（地面）"| F["执行锁定"]
    C --> F
    F --> G["_vehicle_status.arming_state = STANDBY"]
    G --> H["发布 actuator_armed.armed=false"]

触发 disarm 的来源：

  • kill switch/lockdown 自动上锁：disarm(..., true)（forced=true）
  • failsafe action disarm：handleModeIntentionAndFailsafe() 中调用

五、handleAutoDisarm()

着陆后自动上锁与地面预解锁超时自动上锁：

1
2
3
4
5
6
7
8
graph TD
    A["handleAutoDisarm()"] --> B{"landed_amid_mission?"}
    B -->|"Yes（任务中短暂landed）"| C["豁免 COM_DISARM_LAND<br/>避免任务中误判"]
    B -->|"No"| D{"landed 超过 COM_DISARM_LAND 秒?"}
    D -->|"Yes"| E["自动上锁（非强制）"]
    
    A --> F{"kill/lockdown 持续超过阈值?"}
    F -->|"Yes"| G["强制上锁（forced=true）<br/>绕过空中门禁"]

迟滞参数来源：_auto_disarm_killed 事件由 updateParameters() 根据 COM_KILL_DISARM 参数初始化。

六、handleModeIntentionAndFailsafe()

这是整个 Commander 最核心的函数，将用户意图模式、模式替代、failsafe 状态机选出的 action 合成最终的 vehicle_status.nav_state：

1
2
3
4
5
6
7
8
9
10
flowchart TD
    UI["UserModeIntention<br/>用户想要的 nav_state"] --> Merge
    FS["Failsafe 状态机<br/>选出最严重 Action"] --> Merge
    MM["ModeManagement<br/>getNavStateReplacementIfValid()"] --> Merge
    
    Merge["合成逻辑"] --> OUT1["nav_state_user_intention<br/>（user 想要的，下行心跳用）"]
    Merge --> OUT2["nav_state<br/>（实际生效的）"]
    
    Merge -->|"Action=Disarm"| DIS["直接执行 disarm()"]
    Merge -->|"Action=Terminate"| TERM["强制 nav_state=TERMINATION"]

两个输出的用途：

  • nav_state_user_intention：下行 MAVLink CURRENT_MODE 时同时发送 intended_custom_mode
  • nav_state：真正驱动所有模块行为的当前模式

ModeManagement 的 replacement 是强制插入层——无论用户意图还是 failsafe action 映射出来的模式，都需要通过 getNavStateReplacementIfValid() 处理，允许外部模式执行器替代/模式不可用替代等机制生效。

Disarm 与 Terminate 属于超越模式的系统动作，直接驱动上锁或将 nav_state 强制为 TERMINATION。

七、handle_command()

handle_command() 是 Commander 对 vehicle_command 的命令分发入口，参数来自 MAVLinkReceiver 转发，也可能来自其他模块内部发布：

1
2
3
4
5
6
7
8
9
vehicle_command 话题
  -> handle_command() 目标过滤（system/component）
  -> 按 cmd.command 分支执行：
     - DO_SET_MODE：切换飞行模式
     - COMPONENT_ARM_DISARM：arm/disarm
     - 校准命令：卸载到 WorkerThread
     - 重启命令
     - failsafe 相关动作
  -> vehicle_command_ack 回复 ACK

八、完整数据流汇总

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
graph LR
    subgraph 输入
        RC["RC/action_request"]
        MAVLINK["vehicle_command<br/>MAVLink"]
        SENSORS["传感器/估计器话题"]
    end

    subgraph Commander 内部
        HAC["HealthAndArmingChecks<br/>-> failsafe_flags"]
        CM["control_mode<br/>-> vehicle_control_mode"]
        UMI["UserModeIntention<br/>-> user nav_state"]
        FSM["Failsafe<br/>-> selected action"]
        MGT["ModeManagement<br/>-> replacement nav_state"]
        HP["HomePosition<br/>-> home_position"]
    end

    subgraph 输出
        AA["actuator_armed"]
        VCM["vehicle_control_mode"]
        VS["vehicle_status"]
        FDS["failure_detector_status"]
    end

    RC --> UMI
    MAVLINK --> HAC
    MAVLINK --> UMI
    SENSORS --> HAC
    HAC --> FSM
    UMI --> FSM
    FSM --> MGT
    MGT --> VS
    CM --> VCM
    VS --> AA
    VS --> FDS

上一篇：模式需求位图
下一篇：HomePosition 管理