PX4 Navigator navigation.h 与任务数据协议

一、任务数据协议层的定位

navigation.h 和 safe_point_land.hpp 是 Navigator 模块的 L0 数据协议层，不包含任何运行逻辑，只定义数据语义。后续所有模式（Mission/RTL/Loiter 等）都基于这里定义的结构体进行操作。

---

二、NAV_CMD — 导航命令枚举

NAV_CMD 定义了任务项的命令类型，决定了 MissionBlock 如何解释该任务项：

分类	典型值	语义
位置控制	NAV_CMD_WAYPOINT	飞往指定航点
起降	NAV_CMD_TAKEOFF / NAV_CMD_LAND	起飞 / 降落
盘旋	NAV_CMD_LOITER_UNLIMITED / LOITER_TIME_LIMIT / LOITER_TO_ALT	各类盘旋
VTOL	NAV_CMD_VTOL_TAKEOFF / VTOL_LAND / DO_VTOL_TRANSITION	VTOL 相关
动作	NAV_CMD_DO_CHANGE_SPEED / DO_SET_ROI_* / DO_JUMP	速度/ROI/跳转
围栏	NAV_CMD_NAV_FENCE_*	围栏点定义

is_mission_item_reached_or_completed() 中的第一步就是 switch(nav_cmd)，不同命令有截然不同的完成判据：

switch (navigator->get_mission_item()->nav_cmd) {
    case NAV_CMD_LAND:
        return vehicle_land_detected.landed;  // 以落地检测为准
    case NAV_CMD_TAKEOFF:
        return altitude_reached_and_yaw_ok;   // 高度+航向判断
    case NAV_CMD_LOITER_UNLIMITED:
        return false;  // 永不自动完成
    case NAV_CMD_DO_CHANGE_SPEED:
        return true;   // 立即完成（动作型命令）
    // ...
}

---

三、NAV_FRAME — 坐标系枚举

NAV_FRAME 指定任务项高度的参考框架：

值	含义
MAV_FRAME_GLOBAL	WGS84 绝对高度（MSL）
MAV_FRAME_GLOBAL_RELATIVE_ALT	相对于 Home 的高度（最常用）
MAV_FRAME_LOCAL_NED	本地 NED 坐标系
MAV_FRAME_GLOBAL_TERRAIN_ALT	相对地形的高度（需要地形数据）

MissionBlock::get_absolute_altitude_for_item() 根据 altitude_is_relative 字段和 frame 进行高度换算，统一转为 AMSL 绝对高度后再用于计算。

---

四、mission_item_s — 核心数据结构

mission_item_s 是整个 Navigator 的统一任务表达结构，所有导航模式都围绕它工作。

4.1 字段分层说明

struct mission_item_s {
    // ─── 空间位置层 ───────────────────────────────────────────
    double  lat{};          // 目标纬度（度）
    double  lon{};          // 目标经度（度）
    float   altitude{};     // 目标高度（m，绝对或相对，取决于 altitude_is_relative）

    // ─── 轨迹与接受判据层 ─────────────────────────────────────
    float   acceptance_radius{};   // 水平到达接受半径（m），控制航点切换松紧
    float   loiter_radius{};       // 盘旋半径（m，固定翼 loiter 必须）
    float   time_inside{};         // 在任务点内停留时间（s）
    float   yaw{};                 // 目标航向角（rad）

    // ─── 命令执行层 ───────────────────────────────────────────
    uint16_t nav_cmd{};            // 任务命令类型（NAV_CMD_*）
    float    params[7]{};          // MAVLink 命令参数（通用扩展字段）

    // ─── 跳转/流程控制层 ──────────────────────────────────────
    uint16_t do_jump_mission_index{};   // DO_JUMP 跳转目标索引
    uint16_t do_jump_repeat_count{};    // DO_JUMP 重复次数
    uint16_t do_jump_current_count{};   // DO_JUMP 当前执行次数

    // ─── 着陆/围栏复用层 ──────────────────────────────────────
    uint8_t  land_precision{};          // 着陆精度类型（0=标准，1/2=精确降落）
    uint8_t  vertex_count{};            // 围栏多边形顶点数
    float    circle_radius{};           // 围栏圆形半径（m）

    // ─── 行为修饰层 ───────────────────────────────────────────
    uint8_t  frame{};                   // 坐标系（NAV_FRAME）
    bool     loiter_exit_xtrack{};      // 是否从切线方向离开盘旋
    bool     force_heading{};           // 是否强制控制航向
    bool     altitude_is_relative{};    // true=相对Home高度，false=AMSL绝对高度
    bool     autocontinue{};            // 完成后是否自动进入下一任务项
    bool     vtol_back_transition{};    // 是否属于 VTOL 回转换链路
};

4.2 关键字段使用位置

字段	主要使用函数	说明
nav_cmd	is_mission_item_reached_or_completed(), mission_item_to_position_setpoint()	第一个 switch 分支点
acceptance_radius	is_mission_item_reached_or_completed()	普通航点水平到达判据
loiter_radius	mission_item_to_position_setpoint()	固定翼 Loiter、RTL 等盘旋
yaw	is_mission_item_reached_or_completed(), VTOL heading align	航向到达判据
altitude_is_relative	get_absolute_altitude_for_item(), MissionFeasibilityChecker	高度换算
autocontinue	MissionBase::on_active(), advance_mission()	任务自动推进控制
vtol_back_transition	is_mission_item_reached_or_completed(), VTOL landing	VTOL 回转换判定

---

五、mission_item_to_position_setpoint() 翻译过程

这是从任务语义到控制量的核心翻译函数，将 mission_item_s 转为 position_setpoint_s：

flowchart TD
    MS["mission_item_s"] --> CMD{"nav_cmd 类型"}
    CMD -->|WAYPOINT/LOITER| POS["填充 lat/lon/alt<br/>设置 type=POSITION<br/>写入 acceptance_radius"]
    CMD -->|TAKEOFF| TKO["设置 type=TAKEOFF<br/>填充起飞高度"]
    CMD -->|LAND| LND["设置 type=LAND<br/>填充落地位置"]
    CMD -->|DO_*| CMD2["直接发 vehicle_command<br/>不生成 setpoint"]
    POS --> SP["position_setpoint_s"]
    TKO --> SP
    LND --> SP
    SP --> TRIP["position_setpoint_triplet.current"]

position_setpoint_s 的核心字段：

字段	含义
type	setpoint 类型（POSITION / VELOCITY / LOITER / TAKEOFF / LAND / IDLE）
lat / lon / alt	目标全局位置
loiter_radius / loiter_direction	盘旋参数
yaw	目标航向
acceptance_radius	到达半径
cruising_speed	巡航速度限制

---

六、mission_fence_point_s — 围栏数据

围栏数据存储于 dataman，通过 mission_fence_point_s 表达：

• 多边形顶点（NAV_FRAME_GLOBAL坐标）：构成包含/排除多边形
• 圆形区域：中心 + 半径
• Rally 点：安全备降点

Geofence::checkPointAgainstAllGeofences() 读取 dataman 中的围栏数据，判断目标点是否在有效飞行区域内。

---

七、PositionYawSetpoint — 轻量化位置航向结构

PositionYawSetpoint 是一个简化结构，专用于 RTL、safe point、着陆进近等不需要完整 mission_item_s 复杂语义的场景：

struct PositionYawSetpoint {
    float lat{};      // 纬度（度）
    float lon{};      // 经度（度）
    float alt{};      // 高度（m，AMSL）
    float yaw{};      // 目标航向（rad）
};

safe_point_land.hpp 中的 land_approaches_s 进一步描述着陆进近的几何形状（入场角、最终进近路径等），主要由 RTL 模式在选择着陆方式时使用。

---

八、小结

理解 navigation.h 的关键在于：

1. mission_item_s 是 Navigator 内部的统一语言，所有模式构造目标时最终都填充它的字段
2. nav_cmd 是最核心的分支依据，它决定了完成判据和 setpoint 翻译方式
3. 高度字段需要结合 altitude_is_relative 和 frame 才能正确解释
4. autocontinue 控制任务是否自动推进，这是 mission 模式连续执行的基础

上一篇：Navigator 模块架构总览
下一篇：Navigator 主循环与模式调度
系列总目录：PX4 v1.16 源码解读总目录