PX4 HomePosition 管理

发表于 更新于 分类于 ， ， 阅读次数： 本文字数： 500 阅读时长 ≈ 2 分钟
解析 Commander 中 HomePosition 组件的职责、输入输出接口、Home 点设置时机与质量门限，以及 Home 有效性如何反馈到 failsafe 链路影响 RTL 可用性。

一、功能定位

HomePosition 是 Commander 内部的一个状态组件，负责生成并维护 Home 点，通过 uORB 发布 home_position 话题。

Home 点在两套坐标系下均有表达：

  • 本地坐标系（NED）：用于本地导航计算
  • 全局坐标系（WGS84 经纬度）：用于 GPS 导航与 RTL 方向计算

home_position 话题被以下系统消费：

  • RTL 逻辑：确定返航目标点
  • 任务导航：相对 Home 点的高度/距离计算
  • Failsafe：home_position_invalid 标志决定 RTL 是否可用

相关知识：Commander 核心调度

二、输入/输出接口

输入

1
2
3
4
5
6
7
// uORB 订阅
ORB_ID(vehicle_gps_position)     -> sensor_gps_s       // raw GNSS
ORB_ID(vehicle_global_position)  -> vehicle_global_position_s  // 融合 global
ORB_ID(vehicle_local_position)   -> vehicle_local_position_s   // 融合 local + ref 经纬度

// 依赖的状态引用
const failsafe_flags_s &_failsafe_flags  // 由 HealthAndArmingChecks 维护

外部调用参数：

  • setHomePosition(force)：强制设置 Home 点
  • setManually(lat, lon, alt, yaw)：手动设置 Home 点（优先级高于自动设置）
  • update(set_automatically, check_if_changed)：周期更新检查

输出

1
2
3
4
5
6
7
8
9
10
11
12
// uORB 发布
ORB_ID(home_position) -> home_position_s
// 包含字段：
//   local NED 坐标（x, y, z）
//   全局坐标（lat, lon, alt）
//   航向 yaw
//   valid 标志

// 内部状态
_valid                      // 是否已设置过全局 HOME
_gps_position_for_home_valid // GNSS 缓存（global fused 无效时的 fallback）
_heading_reset_counter      // 跟踪 EKF heading reset 并修正 home yaw

三、Home 点设置时机

1
2
3
4
5
6
7
flowchart TD
    A["解锁（arm()）"] -->|"自动设置"| HP["HomePosition"]
    B["起飞后（in-air）"] -->|"若 COM_HOME_IN_AIR=1"| HP
    C["DO_SET_HOME MAVLink 命令"] -->|"手动覆盖"| HP
    D["run() 周期更新"] -->|"条件满足时自动更新"| HP
    
    HP --> PUB["发布 home_position 话题"]

手动设置（setManually）优先级高于自动设置，一旦手动设置后，自动设置不会再覆盖，除非调用 setHomePosition(force=true)。

GPS 质量门限

自动设置 Home 点时，需要满足以下 GNSS 精度要求：

  • EPH（水平位置误差）< 5 m
  • EPV（垂直位置误差）< 10 m

低于此精度的 GNSS 数据不用于 Home 点更新，防止 RTL 时使用不精确的返航点。

四、Home 有效性反馈链路

1
2
3
4
5
6
flowchart LR
    HP["home_position 话题<br/>home_position.valid"] --> HC["HomePositionCheck<br/>HealthAndArmingChecks 子系统"]
    HC --> FF["failsafe_flags.home_position_invalid"]
    FF --> MR["mode_req_home_position 位图<br/>（AUTO_RTL 等模式需要此项）"]
    MR --> FS["Failsafe 状态机<br/>决定 RTL 是否可用"]
    FS -->|"home_position_invalid=true 且需要 RTL"| FB["RTL 降级为 Land 或 Descend"]

这条链路保证了：当 GPS 精度不足导致 Home 点未设置或无效时，RTL 模式不会被错误地执行。Failsafe 状态机在尝试执行 RTL 时会检查 Home 是否有效，不可用时按 Failsafe 框架的级联回退机制降级。

五、heading_reset_counter 的作用

EKF 在某些情况下会触发 heading reset（例如 GPS 重新获取锁定后修正航向估计）。此时 Home 点的航向（yaw）记录也需要同步更新，否则 RTL 计算的返回方向会出现偏差。

_heading_reset_counter 跟踪 EKF 的 heading reset 事件，检测到 reset 后用最新的 heading 重新更新 Home 点的 yaw 分量。

上一篇：Commander 核心调度
返回总目录：PX4 v1.16 源码解读总目录