Px4&ros 踩坑

one5926 · 2026 年6 月 16 日 11:06

在PX4飞控系统中，飞行模式的优先级和切换逻辑是确保飞行安全的核心机制。当定点数据（如GPS或视觉定位）不稳定时，系统会根据预设的优先级和状态机逻辑自动处理。以下是详细的分析：

PX4的飞行模式通常按优先级从高到低排列如下（具体实现可能因固件版本略有差异，但逻辑一致）：

手动控制模式（Manual）
- Stabilized (自稳)：最高优先级。一旦检测到遥控器信号且处于该模式，飞控会立即响应，忽略自动导航指令。主要用于紧急接管。
- Acro / Manual：完全手动控制，姿态角速度环控制。
辅助飞行模式（Assisted）
- Altitude (定高)：保持当前高度，但水平位置由遥控器控制。
- Position (定点)：保持当前高度和水平位置。如果GPS或定位源丢失，通常会自动降级为 Altitude 模式。
自动飞行模式（Auto）
- Offboard：通过MAVLink接口（如MAVROS）接收外部控制指令（位置、速度、姿态）。这是你代码中主要使用的模式。
- Mission (航点)：执行预加载的航点任务。
- RTL (返航)：自动返回起飞点。
- Land (降落)：自动降落。

优先级逻辑：
手动模式 > 辅助模式 > 自动模式。
这意味着，如果飞行器处于 OFFBOARD 模式，但飞控检测到遥控器切换到了 Position 模式，飞控会立即放弃 OFFBOARD 指令，转为定点悬停。

当定点数据（如GPS信号差、视觉定位丢失或EKF协方差过大）不稳定时，PX4的EKF（扩展卡尔曼滤波）状态估计器会首先做出反应，进而触发模式降级或保护机制。

PX4内部维护着EKF的状态标志位（EST），如：

当定位数据质量下降，EKF的协方差矩阵（Covariance）会增大。如果超过阈值，相应的状态标志位会置为 0（不可靠）。

根据当前的飞行模式，系统会有不同的处理方式：

如果处于 Position (定点) 或 Loiter (悬停) 模式：
- 水平位置丢失：如果 EST.pos_horiz_rel 变为不可靠，飞控会自动降级为 Altitude (定高) 模式。此时无人机将保持当前高度，但不再锁定水平位置，可能会随风漂移，需飞行员立即接管。
- 垂直位置丢失：如果 EST.pos_vert 变为不可靠，飞控将无法保持定高，可能触发 Land (降落) 模式或导致坠机风险。
如果处于 Offboard 模式：
- 数据不稳定：PX4会检查EKF状态。如果定位状态不满足Offboard模式的要求（例如水平位置不可靠），飞控会拒绝执行位置控制指令，或者自动切换到 Position 模式（如果该模式仍可用）。

当定点数据不稳定时：