PX4 控制模式生成逻辑

发表于 更新于 分类于 ， ， 阅读次数： 本文字数： 540 阅读时长 ≈ 2 分钟
解析 ModeUtil/control_mode.cpp 如何根据 nav_state、vehicle_type 与 offboard_control_mode 生成 vehicle_control_mode_s 控制器使能位图，阐明各控制环的开关逻辑。

一、功能定位

ModeUtil/control_mode.cpp 的职责：根据当前飞行模式（nav_state）、机型（vehicle_type）与 Offboard 输入类型（offboard_control_mode），生成一份 vehicle_control_mode_s 控制器使能位图，决定哪些控制环/任务应当运行。

这份使能位图是 Commander 输出给全系统的控制权配置，由 Commander::updateControlMode() 计算后发布。

相关前置：飞行模式编码与映射

二、输入与输出

输入

字段 类型 含义
nav_state vehicle_status_s::NAVIGATION_STATE_* 当前生效的飞行模式
vehicle_type vehicle_status_s::VEHICLE_TYPE_* 旋翼/固定翼/车辆
offboard_control_mode offboard 期望控制的 setpoint 类型 position/velocity/acceleration/attitude/body_rate/thrust_and_torque

输出

vehicle_control_mode_s 引用写入，包含若干 flag_control_*_enabled 布尔位：

1
2
3
4
5
6
7
8
9
10
11
struct vehicle_control_mode_s {
    bool flag_control_manual_enabled;     // 手动控制模式
    bool flag_control_auto_enabled;       // 自动控制模式
    bool flag_control_position_enabled;   // 位置控制环使能
    bool flag_control_velocity_enabled;   // 速度控制环使能
    bool flag_control_altitude_enabled;   // 高度控制环使能
    bool flag_control_attitude_enabled;   // 姿态控制环使能
    bool flag_control_rates_enabled;      // 角速率控制环使能
    bool flag_control_allocation_enabled; // 控制分配使能
    // ...
};

三、控制模式与控制环的对应关系

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
flowchart LR
    subgraph 控制环层级
        POS["位置控制<br/>mc_pos_control"]
        VEL["速度控制"]
        ATT["姿态控制<br/>mc_att_control"]
        RATE["角速率控制<br/>mc_rate_control"]
        ALLOC["控制分配<br/>control_allocator"]
    end

    subgraph 飞行模式
        POSCTL["POSCTL/AUTO_*"]
        ALTCTL["ALTCTL"]
        STAB["STABILIZED"]
        ACRO["ACRO"]
        OFFBOARD["OFFBOARD"]
    end

    POSCTL -->|"pos+vel+att+rate"| POS
    POSCTL --> VEL
    POSCTL --> ATT
    POSCTL --> RATE
    ALTCTL -->|"vel+att+rate"| VEL
    ALTCTL --> ATT
    ALTCTL --> RATE
    STAB -->|"att+rate"| ATT
    STAB --> RATE
    ACRO -->|"rate only"| RATE
    OFFBOARD -->|"根据 offboard_control_mode 动态选择"| POS
    
    RATE --> ALLOC

各模式激活的控制环（多旋翼为例）：

飞行模式 position velocity altitude attitude rates allocation
MANUAL ✗ ✗ ✗ ✗ ✗ ✓
ACRO ✗ ✗ ✗ ✗ ✓ ✓
STABILIZED ✗ ✗ ✗ ✓ ✓ ✓
ALTCTL ✗ ✗ ✓ ✓ ✓ ✓
POSCTL ✓ ✓ ✓ ✓ ✓ ✓
AUTO_MISSION ✓ ✓ ✓ ✓ ✓ ✓

四、Offboard 模式的特殊处理

Offboard 模式下，控制环使能取决于 offboard_control_mode 中声明的 setpoint 类型：

offboard_control_mode 激活的控制环
position=true 位置+速度+姿态+角速率
velocity=true 速度+姿态+角速率
acceleration=true 姿态+角速率
attitude=true 姿态+角速率
body_rate=true 角速率
thrust_and_torque=true 仅控制分配

这使得 Offboard 模式下上位机可以精确控制”接管到哪一层控制环”，灵活适配不同的外部控制策略。

五、conversions.hpp

conversions.hpp 提供 nav_state 到 events 体系的 navigation_mode_t 的转换：

  • 输入：nav_state
  • 输出：events 体系的 navigation_mode_t，用于事件/日志参数的稳定表达

与 control_mode.cpp 的区别：control_mode 用于控制运行时控制环开关；conversions 用于日志/事件的模式描述，两者均由 Commander 内部调用但服务于不同目的。

六、vehicle_control_mode 的消费者

vehicle_control_mode_s 发布后，被多个下游模块订阅：

模块 用途
mc_pos_control 判断是否执行位置/速度控制
mc_att_control 判断是否执行姿态控制，还是直接透传 rates
mc_rate_control 判断是否执行角速率控制
control_allocator 判断是否进行控制分配

每个模块在 Run() 开始时订阅 vehicle_control_mode，根据对应的 flag_control_*_enabled 决定本次执行的控制路径。

上一篇：飞行模式编码与映射
下一篇：模式需求位图