PX4 飞行模式编码与 MAVLink 映射

发表于 更新于 分类于 ， ， 阅读次数： 本文字数： 545 阅读时长 ≈ 2 分钟
解析 px4_custom_mode.h 的主/子模式枚举定义、32 位 custom_mode union 位布局，以及 PX4 内部 nav_state 与 MAVLink custom_mode 的双向映射链路。

一、文件定位

px4_custom_mode.h 负责：

  1. 定义 PX4 的 MAVLink custom_mode 编码格式（主模式 + 子模式枚举）
  2. 提供 nav_state → custom_mode 的确定性映射函数 get_px4_custom_mode()
  3. 供 MAVLink 下行心跳/模式消息以及 Commander 内部日志/事件复用

本文件不负责决定飞行模式本身。飞行模式的实际状态由 vehicle_status.nav_state 表示，由 Commander 的模式管理与 failsafe 状态机产生。

相关知识：Commander 模块框架

二、主模式枚举

1
2
3
4
5
6
7
8
9
10
11
12
enum PX4_CUSTOM_MAIN_MODE {
    PX4_CUSTOM_MAIN_MODE_MANUAL       = 1,  // 从 1 开始，0 表示 unknown
    PX4_CUSTOM_MAIN_MODE_ALTCTL,
    PX4_CUSTOM_MAIN_MODE_POSCTL,
    PX4_CUSTOM_MAIN_MODE_AUTO,
    PX4_CUSTOM_MAIN_MODE_ACRO,
    PX4_CUSTOM_MAIN_MODE_OFFBOARD,
    PX4_CUSTOM_MAIN_MODE_STABILIZED,
    PX4_CUSTOM_MAIN_MODE_RATTITUDE_LEGACY,
    PX4_CUSTOM_MAIN_MODE_SIMPLE,      // unused，保留备用
    PX4_CUSTOM_MAIN_MODE_TERMINATION
};

从 1 开始的原因：0 被约定为 unknown，保持 MAVLink 生态的兼容性。

三、Auto 子模式枚举

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
enum PX4_CUSTOM_SUB_MODE_AUTO {
    PX4_CUSTOM_SUB_MODE_AUTO_READY       = 1,
    PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF,
    PX4_CUSTOM_SUB_MODE_AUTO_LOITER,
    PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
    PX4_CUSTOM_SUB_MODE_AUTO_RTL,
    PX4_CUSTOM_SUB_MODE_AUTO_LAND,
    PX4_CUSTOM_SUB_MODE_AUTO_RESERVED_DO_NOT_USE,   // 已废弃（2020-03-05）
    PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET,
    PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND,
    PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF,
    PX4_CUSTOM_SUB_MODE_EXTERNAL1,
    // ...
    PX4_CUSTOM_SUB_MODE_EXTERNAL8,  // 外部拓展模式槽位
};

外部扩展模式 EXTERNAL1~8 为外部模式执行器/自定义执行器预留槽位，保持 MAVLink 层的稳定编码形式。

四、32 位 custom_mode 位布局

1
2
3
4
5
6
graph LR
    A["custom_mode (uint32_t)"]
    A --> B["reserved (8 bits, [31:24])"]
    A --> C["reserved (8 bits, [23:16])"]
    A --> D["main_mode (8 bits, [15:8])"]
    A --> E["sub_mode (8 bits, [7:0])"]

通过 union px4_custom_mode 同一段 32 位数据以多种视角解释：

1
2
3
4
5
6
7
8
9
union px4_custom_mode {
    struct {
        uint16_t reserved;
        uint8_t  main_mode;
        uint8_t  sub_mode;
    };
    uint32_t data;
    float    data_float;
};

内存编码示例（小端序）：

1
2
3
写 main_mode = 4：内存为 00 00 04 00
写 sub_mode  = 5：内存为 00 00 04 05
读 data = 0x05040000

按小端序规则：data = b0 + (b1<<8) + (b2<<16) + (b3<<24)，所以 data = 0x05040000。

编码函数 get_px4_custom_mode(nav_state) 将 PX4 内部状态映射到 MAVLink 编码：

1
2
3
4
5
6
7
8
9
nav_state（vehicle_status_s::NAVIGATION_STATE_*）
    |
    v get_px4_custom_mode()
    |
    v
px4_custom_mode（main_mode + sub_mode）
    |
    v
Heartbeat.cpp 读取 data 写入 HEARTBEAT.custom_mode 字段下行

映射形态分两类：

类型 示例
一对一：单一 nav_state 对应单一 main_mode MANUAL/ALTCTL/ACRO/OFFBOARD/STABILIZED/TERMINATION
一对多：主模式固定 + 子模式分散 AUTO_MISSION → AUTO + AUTO_MISSION；POSITION_SLOW → POSCTL + POSCTL_SLOW

DEFINE_GET_PX4_CUSTOM_MODE 宏门控的目的：本文件既作类型/枚举定义，又可选择性提供映射实现，避免所有 include 都引入 uORB 依赖与潜在编译耦合。

1
2
3
4
5
6
7
flowchart LR
    GCS["地面站 SET_MODE 命令"]
    --> MR["mavlink_receiver.cpp<br/>SET_MODE.custom_mode<br/>按 union 解析<br/>-> vcmd.param2/param3"]
    --> CMD["vehicle_command uORB"]
    --> HC["Commander::handle_command()<br/>反解 param1/2/3 → 模式选择"]
    --> UM["UserModeIntention::change()"]
    --> NS["vehicle_status.nav_state"]

handle_command() 中的参数含义：

参数 含义
param1 base_mode：MAVLink SET_MODE.base_mode 对应 MAV_MODE_FLAG 位图
param2 PX4 custom_main_mode（px4_custom_mode.main_mode）
param3 PX4 custom_sub_mode（px4_custom_mode.sub_mode）

Commander 侧直接将 float 强转回 uint8_t 提取 main_mode 和 sub_mode。

七、uORB 消息结构体名生成规则

PX4 的 uORB 消息生成器（Tools/msg/px_generate_uorb_topic_files.py）采用 PascalCase → snake_case 转换：

1
2
3
4
# 核心算法：在不是开头且后面是大写字母处插入下划线
re.sub(r'(?<!^)(?=[A-Z])', '_', file_base_name).lower()

# VehicleStatus.msg -> vehicle_status

生成内容：

  • vehicle_status.h：结构体 vehicle_status_s
  • ORB_DECLARE(vehicle_status) 与 ORB_DEFINE(vehicle_status, ...) 宏
  • C++ 封装类 uORB::Publication<vehicle_status_s> / uORB::Subscription vehicle_status_sub

八、总结

本文件的核心价值是编码层面的稳定性：nav_state → custom_mode.data 的映射是所有下行 MAVLink 消息对模式编码的唯一来源，保证地面站与飞控之间模式状态表达的一致性。

上一篇：参数系统与 Kconfig
下一篇：控制模式生成逻辑