PX4导航遇到GPS数据丢失的处理和相关控制参数

PX4导航时GPS数据丢失的处理

1. GPS数据丢失的检测与驱动层处理

• PX4的检测：GPS驱动通过poll()系统调用轮询接收数据，当超过1秒无有效数据时标记为不健康（_healthy=false）
• 重连：驱动会关闭并重启串口，在参数中设置了自动协议的话，飞控会自动循环切换协议（UBX/MTK/NMEA）尝试重连
• 数据传输：GPS1端口使用轮询机制（pollOrRead()函数），不是串口中断或DMA，在上层中断发送GPS数据时，飞控可以正常识别到gps数据的丢失

2. EKF2状态估计器降级

GPS丢失时，EKF2通过以下步骤降级位置估计：

// gps_control.cpp: 检测GPS超时if(_imu_sample_delayed.time_us-_gps_sample_delayed.time_us>(uint64_t)1e6){stopGpsFusion();// 停止GPS融合_warning_events.flags.gps_data_stopped=true;}// ekf_helper.cpp: 停止GPS融合voidEkf::stopGpsFusion(){if(_control_status.flags.gps){stopGpsPosFusion();stopGpsVelFusion();}// 停止航向融合（如果启用）if(_control_status.flags.gps_yaw){stopGpsYawFusion();}}// 切换到气压计高度融合voidEkf::stopGpsPosFusion(){_control_status.flags.gps=false;if(_control_status.flags.gps_hgt){startBaroHgtFusion();// 启动气压计高度融合}}

该文件所在位置：

modules\ekf2\EKF\gps_control.cpp

降级结果：

• 停止GPS位置、速度和航向融合
• 切换至气压计高度融合
• 依赖IMU进行惯性导航，但位置误差随时间漂移
• 尝试使用视觉/光流等辅助定位源（如果可用）

3. 导航模式的GPS依赖与切换逻辑

3.1 任务模式 (Mission)

• GPS依赖：任务有效性验证严格依赖home位置有效性

• 处理逻辑：

  // mission.cpp: 任务有效性检查if((!_home_inited&&_navigator->home_global_position_valid())||force){MissionFeasibilityChecker_missionFeasibilityChecker(_navigator);_navigator->get_mission_result()->valid=_missionFeasibilityChecker.checkMissionFeasible(_mission,...);}// mission_feasibility_checker.cpp: 检查home有效性constboolhome_valid=_navigator->home_global_position_valid();constboolhome_alt_valid=_navigator->home_alt_valid();if(!home_alt_valid){failed=true;// 任务标记为无效}

  该文件所在位置：

  modules\navigator\mission.cpp

• 结果：GPS丢失→home位置无效→任务标记为无效→自动切换至悬停模式

相关参数：

文件路径：\px4_autopilot_ws\src\modules\navigator\mission_params.c

参数控制：

3.2 返航模式 (RTL)

• GPS依赖：返航目标点设置依赖home位置有效性

• 处理逻辑：

  // rtl.cpp: 查找返航目标点voidRTL::find_RTL_destination(){if(!_navigator->home_global_position_valid()){return;// 无法设置返航目标点}_destination.set(home_landing_position);}

  该文件所在位置：

  modules\navigator\mission.cpp

• 结果：GPS丢失→无法确定home位置→无法设置返航目标→自动切换至悬停模式

文件路径：\px4_autopilot_ws\src\modules\navigator\rtl_params.c

参数控制：

3.3 悬停模式 (Loiter)

• GPS依赖：不直接依赖GPS，基于EKF2提供的位置估计维持位置

• 处理逻辑：

  // loiter.cpp: 设置悬停位置voidLoiter::set_loiter_position(){// 基于当前位置设置悬停点if(_navigator->get_vstatus()->vehicle_type==vehicle_status_s::VEHICLE_TYPE_ROTARY_WING){setLoiterItemFromCurrentPositionWithBreaking(&_mission_item);}else{setLoiterItemFromCurrentPosition(&_mission_item);}}

  该文件所在位置：

  modules\navigator\loiter.cpp

• 结果：GPS丢失后继续尝试维持当前位置，但精度随EKF2位置估计漂移而降低

主要文件路径：\px4_autopilot_ws\src\modules\navigator\navigator_params.c

参数控制：

其他导航参数

文件路径：\px4_autopilot_ws\src\modules\navigator\navigator_params.c

系统级导航参数

文件路径：\px4_autopilot_ws\src\modules\commander\commander_params.c

跟随目标参数

文件路径：\px4_autopilot_ws\src\modules\navigator\follow_target_params.c

4. 系统状态变化

• 标志位更新：

  • vehicle_status_s::global_position_valid→ false
  • vehicle_status_s::gps_position_valid→ false
  • home_position_s::valid_alt→ false
  • home_position_s::valid_hpos→ false
  • EKF2::warning_events::gps_data_stopped→ true

• home位置有效性：

  // navigator.h: home位置有效性判断boolhome_global_position_valid(){return(_home_pos.valid_alt&&_home_pos.valid_hpos);}

  该文件所在位置：

  modules\navigator\navigator.h

5. 模式切换

1. GPS丢失→ GPS驱动标记_healthy=false
2. EKF2检测到超时→ 停止GPS融合，切换至气压计高度融合
3. home位置失效→home_global_position_valid()返回false
4. 任务验证失败→ 任务标记为无效
5. 导航模式切换→ 从任务/返航模式自动切换至悬停模式
6. 系统状态更新→ 相关标志位更新，通知其他模块

6. 依赖关系

GPS数据 → EKF2融合 → home位置有效性 → 导航模式决策 ↑ ↑ ↑ | | | 超时检测 位置/高度有效 任务/返航可行性

总结

• 位置估计：GPS融合降级：惯性导航+气压计高度
• 导航模式：从任务/返航切换到悬停，避免无GPS时盲目执行航线
• 系统状态：通过标志位清晰反映GPS状态，确保各模块协同处理
• 用户反馈：通过mavlink日志和事件系统提示GPS丢失状态

无人机在GPS丢失时，自动维持相对安全的状态，等待GPS信号恢复或由用户接管控制。