PX4 Navigator 主循环与模式调度

发表于 更新于 分类于 ， ， 阅读次数： 本文字数： 476 阅读时长 ≈ 2 分钟
解析 PX4 Navigator 主循环 run() 的完整执行流程，说明如何根据 nav_state 选择并驱动各导航模式，以及围栏安全检查、RTL 策略树、MissionBase 执行框架的运行机制。

一、Navigator::run() 主循环

Navigator 运行在 WorkQueue 上，主循环执行顺序如下：

1
2
3
4
5
6
7
8
9
10
11
12
flowchart TD
    A["run 开始"] --> B["参数更新<br/>params_update"]
    B --> C["轮询核心 uORB topic<br/>vehicle_status / local_pos / global_pos<br/>home_pos / mission / command / land_detected"]
    C --> D["geofence_breach_check<br/>围栏安全检查 - 优先"]
    D --> E["根据 nav_state<br/>选择当前模式对象"]
    E --> F["NavigatorMode::run active<br/>驱动当前模式生命周期"]
    F --> G{"triplet 有更新?"}
    G -->|是| H["发布 position_setpoint_triplet"]
    G -->|否| I["不发布"]
    H --> J["发布辅助 topic<br/>mission_result / navigator_status<br/>geofence_result / navigator_status"]
    I --> J
    J --> K["run 结束 等待下次回调"]

二、模式选择逻辑

根据 vehicle_status.nav_state（由 Commander 设定，见飞行模式编码）选择对应的模式对象：

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
35
// Navigator::run() 中的模式选择（简化）
NavigatorMode *navigation_mode_new = nullptr;

switch (_vehicle_status.nav_state) {
    case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
        navigation_mode_new = &_mission;
        break;
    case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
        navigation_mode_new = &_loiter;
        break;
    case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
        navigation_mode_new = &_rtl;
        break;
    case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
        navigation_mode_new = &_takeoff;
        break;
    case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
        navigation_mode_new = &_land;
        break;
    case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
        navigation_mode_new = &_precland;
        break;
    // ...
}
// 若模式发生切换，触发 on_inactivation() + on_activation()
if (navigation_mode_new != _navigation_mode) {
    if (_navigation_mode) _navigation_mode->run(false);  // 触发 on_inactivation
    _navigation_mode = navigation_mode_new;
}
// 驱动当前模式
if (_navigation_mode) _navigation_mode->run(true);  // 触发 on_active
// 驱动所有非活跃模式（后台维护）
for (auto *mode : _navigation_mode_array) {
    if (mode != _navigation_mode) mode->run(false);  // 触发 on_inactive
}

三、NavigatorMode 生命周期机制

NavigatorMode::run(bool active) 统一管理四种钩子：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
void NavigatorMode::run(bool active) {
    if (active) {
        if (!_active) {
            _active = true;
            on_activation();  // 首次激活
        } else {
            on_active();      // 持续激活
        }
    } else {
        if (_active) {
            _active = false;
            on_inactivation();  // 切出
        } else {
            on_inactive();      // 后台维护
        }
    }
}

这一机制保证了：

  • 模式切入/切出时一定执行一次初始化/清理
  • 非活跃模式在后台持续维护状态（如 RTL 持续估计剩余时间）

四、MissionBase — 任务执行框架

Mission 系模式（Mission、RtlMissionFast 等）都使用 MissionBase 框架管理任务：

1
2
3
4
5
6
7
8
9
10
11
flowchart TD
    MB["MissionBase::on_active"] --> A["updateMavlinkMission<br/>从 mission topic 同步元信息"]
    A --> B["updateDatamanCache<br/>从 dataman 读取任务项"]
    B --> C["loadCurrentMissionItem<br/>加载当前 mission_item_s"]
    C --> D["生成 current/next triplet<br/>setActiveMissionItems"]
    D --> E{"当前任务是否完成?"}
    E -->|否| F["维持当前 setpoint"]
    E -->|是| G["advance_mission<br/>推进到下一任务项"]
    G --> H{"autocontinue?"}
    H -->|是| D
    H -->|否| I["等待用户命令"]

任务数据读取链：

1
2
3
4
5
6
7
uORB: mission_s（元信息：count, current_seq）
    ↓ updateMavlinkMission()
DatamanCache（异步预取）
    ↓ updateDatamanCache()
DatamanClient（按需读取）
    ↓ loadCurrentMissionItem()
当前 mission_item_s

五、MissionBlock — 公共任务语义

所有模式在生成任务时都通过 MissionBlock 进行：

1
2
3
4
5
6
7
flowchart LR
    Mode["具体模式<br/>(Mission/Loiter/RTL)"] -->|构造/修改| MS["mission_item_s"]
    MS --> MB["MissionBlock<br/>公共语义解释器"]
    MB -->|位置型任务| SP["position_setpoint_s"]
    MB -->|动作型任务| CMD["vehicle_command"]
    SP --> TRIP["position_setpoint_triplet.current"]
    TRIP --> FMM["flight_mode_manager<br/>FlightTaskAuto"]

具体实现中，模式通过设置 _mission_item（MissionBlock 的成员）并调用 mission_item_to_position_setpoint() 来生成 triplet：

1
2
3
4
5
6
7
8
9
10
11
12
// 以 Loiter 为例：
void Loiter::set_loiter_position() {
    _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
    _mission_item.lat     = _navigator->get_global_position()->lat;
    _mission_item.lon     = _navigator->get_global_position()->lon;
    _mission_item.altitude = _navigator->get_global_position()->alt + _navigator->get_loiter_min_alt();
    _mission_item.acceptance_radius = _navigator->get_acceptance_radius();

    // 通过 MissionBlock 翻译并写入 triplet
    mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
    _navigator->set_position_setpoint_triplet_updated();
}

六、RTL 策略树

RTL 不是单一状态机，而是顶层策略选择器：

1
2
3
4
5
6
7
8
9
10
11
flowchart TD
    RTL["RTL::on_activation"] --> Decide["setRtlTypeAndDestination<br/>findRtlDestination"]
    Decide --> Type{"RTL_TYPE 参数"}
    Type -->|0: 直接返航| Direct["RtlDirect<br/>直飞→盘旋→降落"]
    Type -->|1: 直飞任务降落段| DML["RtlDirectMissionLand<br/>直飞到任务降落起点→沿任务落地"]
    Type -->|2: 任务正向快速返回| MF["RtlMissionFast<br/>沿任务正向到降落段"]
    Type -->|3: 任务反向回溯| MFR["RtlMissionFastReverse<br/>沿任务反向回溯→接入降落"]
    Direct --> Exec["执行返航状态机<br/>更新 triplet"]
    DML --> Exec
    MF --> Exec
    MFR --> Exec

RtlDirect 的核心状态机（最常用）：

状态 动作
CLIMBING 爬升到返航高度（RTL_RETURN_ALT）
MOVE_TO_LOITER 直飞到 Home/safe point 上方
LOITER_DOWN 盘旋下降到 RTL_DESCEND_ALT
LOITER_HOLD 等待 RTL_LAND_DELAY 秒
MOVE_TO_LAND 飞向精确降落点（若使能精降）
LAND 执行降落
IDLE 完成

七、围栏安全链

围栏检查优先于所有模式执行，在每次主循环中调用：

1
2
3
4
5
6
7
8
9
10
11
12
13
flowchart TD
    Check["geofence_breach_check"] --> Predict{"GF_PREDICT 开启?"}
    Predict -->|是| PredPos["GeofenceBreachAvoidance<br/>基于速度/加速度预测未来位置"]
    Predict -->|否| CurPos["用当前位置检查"]
    PredPos --> GF["Geofence<br/>检查是否越界<br/>(距离/高度/多边形/圆形)"]
    CurPos --> GF
    GF --> Result{"是否越界?"}
    Result -->|否| NoAction["继续正常执行"]
    Result -->|是| Action{"GF_ACTION 参数"}
    Action -->|1: Warning| Warn["发布警告<br/>继续执行"]
    Action -->|2: Hold| Hold["注入 Loiter 到安全点"]
    Action -->|3: RTL| RTLA["触发 RTL"]
    Action -->|4: Land| Land["触发就地降落"]

八、小结

Navigator 主循环的核心机制：

  1. 以 nav_state 为中枢：Commander 通过 nav_state 驱动模式切换，Navigator 只执行选定的模式
  2. 生命周期统一调度：NavigatorMode::run() 保证切入/切出/后台的干净执行
  3. MissionBlock 语义统一：所有模式产生的任务最终通过同一翻译层转化，保证一致性
  4. 安全优先：围栏检查在模式执行之前，任何位置违规都会优先触发保护行为

上一篇：navigation.h 与数据协议层
下一篇：围栏与精降复杂状态机
系列总目录：PX4 v1.16 源码解读总目录