PX4 Navigator 模块架构总览

一、Navigator 的定位

src/modules/navigator 是 PX4 的自动导航执行层。

它不负责：

• 姿态控制、角速度控制、电机混控、状态估计

它负责：

1. 接收上游状态与任务信息（来自 Commander 的 nav_state）
2. 根据当前导航状态选择合适的导航模式（Mission / Loiter / RTL 等）
3. 把高层任务语义翻译为下游位置控制器能执行的 position_setpoint_triplet
4. 输出任务执行结果、围栏状态、导航状态等 uORB 消息

一句话概括：Navigator 是 PX4 自动任务/返航/盘旋/降落/围栏/精降的统一导航执行器。

相关上游：Commander 核心调度，HomePosition 管理
相关下游：mc_pos_control 位置控制

---

二、纵向层级结构

L0 任务数据协议层     navigation.h / safe_point_land.hpp
L1 总控入口层         navigator.h / navigator_main.cpp
L2 模式生命周期层     navigator_mode.h / navigator_mode.cpp
L3 公共任务语义层     mission_block.h / mission_block.cpp
L4 Mission 执行框架层 mission_base.h / mission_base.cpp
L5 具体导航模式层     mission / loiter / takeoff / land / precland / vtol_takeoff / rtl*
L6 安全与验证层       geofence / GeofenceBreachAvoidance / mission_feasibility_checker
L7 参数定义层         navigator_params.c / mission_params.c / geofence_params.c / rtl_params.c

主执行链：

flowchart TB
    A["L0 数据协议层<br/>navigation.h"] --> B["L1 总控入口层<br/>Navigator::run()"]
    B --> C["L2 生命周期层<br/>NavigatorMode::run()"]
    C --> D["L5 具体模式层<br/>Mission/Loiter/RTL/..."]
    D --> E["L3 公共任务语义层<br/>MissionBlock"]
    E --> F["position_setpoint_triplet"]
    F --> G["下游位置控制器<br/>FlightTaskAuto / mc_pos_control"]

    B --> H["L6 围栏安全层<br/>Geofence / BreachAvoidance"]
    D --> I["L4 Mission 执行框架层<br/>MissionBase"]
    I --> J["L6 任务合法性检查层"]
    D --> K["RTL 策略树"]
    L["L7 参数定义层"] --> B
    L --> D
    L --> H
    L --> K

---

三、各层级文件说明

3.1 L0 — 任务数据协议层

文件	职责
navigation.h	定义 NAV_CMD、NAV_FRAME 枚举、mission_item_s 统一任务结构体、mission_fence_point_s 围栏点结构
safe_point_land.hpp	定义 loiter_point_s、land_approaches_s，服务 RTL 目的地和着陆进近描述

mission_item_s 是整个 Navigator 目录的核心中间语言，所有模式几乎都在围绕它进行：构造 → 修改 → 判断是否完成 → 翻译为 position_setpoint_s。

3.2 L1 — 总控入口层

Navigator 类是所有模式的总控调度器，持有：

• 所有模式对象：_mission、_loiter、_rtl、_takeoff、_land、_precland、_vtol_takeoff
• 安全对象：_geofence、_gf_breach_avoidance
• 共享输入状态：vehicle_status、vehicle_local_position、home_position 等
• 核心输出：position_setpoint_triplet

Navigator::run() 执行顺序：

1. 初始化/参数更新
2. 轮询核心 topic（位置、状态、任务、命令）
3. 执行 geofence_breach_check()（安全优先）
4. 根据 vehicle_status.nav_state 选择当前模式
5. 调用所有模式的生命周期函数
6. 发布输出（position_setpoint_triplet、mission_result 等）

3.3 L2 — 模式生命周期层

所有具体模式均继承 NavigatorMode，通过四种钩子函数执行：

钩子	时机	用途
on_activation()	首次切入时执行一次	初始化状态机、构造初始 mission item
on_active()	激活期间每轮执行	运行主状态机、更新 setpoint、检查完成条件
on_inactivation()	切出时执行一次	清理状态、记住暂停点
on_inactive()	未激活时后台执行	更新缓存、维护恢复数据（如 RTL 时间估计）

3.4 L3 — 公共任务语义层 MissionBlock

MissionBlock 是整个 Navigator 的任务语义内核：

函数	作用
is_mission_item_reached_or_completed()	定义各类 nav_cmd 的完成判据（位置到达、落地检测、高度匹配等）
mission_item_to_position_setpoint()	将 mission_item_s 翻译为 position_setpoint_s
issue_command()	将 DO_* 类任务项转换为 vehicle_command 发布
set_takeoff_item()	构造标准起飞任务项
set_land_item()	构造标准降落任务项
set_vtol_transition_item()	构造 VTOL 模式切换任务项

3.5 L4 — Mission 执行框架层 MissionBase

解决 mission 系模式的公共问题：

• 任务从哪读：mission_s（元信息）→ DatamanCache → mission_item_s（正文）
• 索引如何推进：处理 DO_JUMP、非位置命令、reverse mission 等复杂场景
• 暂停后如何恢复：记录 _inactivation_index，恢复时重新定位
• 上下文命令回放：恢复时重放 gimbal/camera/trigger/speed 等上下文命令

3.6 L5 — 具体导航模式层

模式	类型	说明
Mission	普通任务	在 MissionBase 框架上实现普通任务业务，插入起飞/着陆/VTOL/精降节点
Loiter	简单	设置或重定位一个悬停点，维持不变
Takeoff	简单	自动起飞，到达高度后进入 loiter
Land	简单	自动降落，落地后发布 mode_completed
PrecLand	复杂状态机	精确降落，依赖 landing_target_pose，包含搜索/横向接近/精降状态
VtolTakeoff	复杂状态机	VTOL 起飞的多阶段状态机（悬停→对齐→转换→爬升）
RTL	策略树	顶层返航策略选择器，分发到 RtlDirect / RtlDirectMissionLand / RtlMissionFast / RtlMissionFastReverse

3.7 L6 — 安全与验证层

模块	职责
Geofence	加载围栏数据，判定某点是否越界
GeofenceBreachAvoidance	基于速度/加速度预测未来位置，计算安全 loiter 点
MissionFeasibilityChecker	将 FeasibilityChecker + Navigator 上下文 + geofence 合并做执行前验证
FeasibilityChecker	纯规则验证：命令合法性、机型匹配、首航点距离、起降要求

---

四、Navigator 的输入与输出

4.1 输入 uORB 话题

话题	说明
vehicle_status.nav_state	Commander 决定的当前导航状态，决定选哪个模式
vehicle_local_position	EKF2 估计的本地 NED 位置/速度
vehicle_global_position	EKF2 估计的全局经纬度
home_position	起飞原点（用于 RTL 目的地计算）
mission	上传的自动任务元信息
vehicle_command	MAVLink 命令（用于接收 RTL/Takeoff/Land 等指令）
vehicle_land_detected	落地检测结果
landing_target_pose	精确降落目标位置（PrecLand 使用）

4.2 输出 uORB 话题

话题	下游消费者	关键字段
position_setpoint_triplet	FlightTaskAuto / mc_pos_control	前一/当前/下一 setpoint
mission_result	Commander / GCS	任务状态、当前序号
navigator_status	GCS 监控	当前模式
geofence_result	Commander	是否越界

---

五、与上下游的接口关系

flowchart LR
    Commander["Commander<br/>vehicle_status.nav_state"] --> Navigator["Navigator"]
    EKF2["EKF2<br/>vehicle_local/global_position"] --> Navigator
    MAVLink["MAVLink<br/>mission + vehicle_command"] --> Navigator
    Navigator --> Triplet["position_setpoint_triplet"]
    Triplet --> FMM["flight_mode_manager<br/>FlightTaskAuto"]
    FMM --> MPC["mc_pos_control<br/>vehicle_attitude_setpoint"]
    MPC --> ATT["mc_att_control"]

注意：Navigator 的输出不是直接送到 mc_pos_control，而是先经过 flight_mode_manager 中的 FlightTaskAuto 转换为 trajectory_setpoint + vehicle_constraints，再送入位置控制器。这一层转换负责轨迹平滑（位置→带加速度约束的轨迹）。

下一篇：navigation.h 与数据协议层
系列总目录：PX4 v1.16 源码解读总目录