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

参数控制：

参数名称	行号	类型	默认值	描述
MIS_TAKEOFF_ALT	58	FLOAT	2.5f	起飞最小高度
MIS_TAKEOFF_REQ	68	INT32	0	是否要求起飞后才能执行任务
MIS_LTRMIN_ALT	83	FLOAT	-1.0f	任务着陆前悬停最小高度
MIS_DIST_1WP	99	FLOAT	900	从起飞点到第一个航点的最大距离
MIS_DIST_WPS	115	FLOAT	900	连续航点间的最大距离
MIS_MNT_YAW_CTL	129	INT32	0	任务中的挂载偏航控制
MIS_YAW_TMT	146	FLOAT	-1.0f	任务中的偏航控制超时
MIS_YAW_ERR	158	FLOAT	12.0f	任务中的偏航误差阈值

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

参数控制：

参数名称	行号	类型	默认值	描述
RTL_RETURN_ALT	59	FLOAT	60	返航爬升高度
RTL_DESCEND_ALT	75	FLOAT	30	返航下降悬停高度
RTL_LAND_DELAY	90	FLOAT	0.0f	下降后悬停到着陆的延迟时间
RTL_MIN_DIST	106	FLOAT	10.0f	返航最小距离阈值
RTL_TYPE	119	INT32	0	返航目标选择类型
RTL_CONE_ANG	138	INT32	45	返航爬升锥体半角
RTL_PLD_MD	150	INT32	0	返航中的挂载模式
RTL_LOITER_RAD	164	FLOAT	80.0f	返航悬停半径
RTL_HDG_MD	176	INT32	0	返航航向模式
RTL_TIME_FACTOR	190	FLOAT	1.1f	返航时间因子
RTL_TIME_MARGIN	205	INT32	100	返航时间余量

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

参数控制：

参数名称	文件路径	行号	类型	默认值	描述
NAV_LOITER_RAD	navigator_params.c	55	FLOAT	80.0f	悬停半径
RC_MAP_LOITER_SW	rc_update/params.c	1386	INT32	0	遥控器悬停开关映射
RC_LOITER_TH	rc_update/params.c	1906	FLOAT	0.75f	遥控器悬停阈值
RTL_LOITER_RAD	rtl_params.c	164	FLOAT	80.0f	返航悬停半径
VTO_LOITER_ALT	vtol_takeoff_params.c	53	FLOAT	80	VTOL起飞悬停高度

其他导航参数

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

参数名称	行号	类型	默认值	描述
NAV_ACC_RAD	70	FLOAT	10.0f	航点到达半径
NAV_FW_ALT_RAD	84	FLOAT	10.0f	固定翼高度到达半径
NAV_FW_ALTL_RAD	98	FLOAT	5.0f	固定翼着陆高度到达半径
NAV_MC_ALT_RAD	112	FLOAT	0.8f	多旋翼高度到达半径
NAV_TRAFF_AVOID	128	INT32	1	是否启用交通规避
NAV_TRAFF_A_RADM	141	FLOAT	500	交通规避最大半径
NAV_TRAFF_A_RADU	155	FLOAT	10	交通规避最小半径
NAV_AH_LAT	167	INT32	-265847810	防碰撞基准点纬度
NAV_AH_LON	179	INT32	1518423250	防碰撞基准点经度
NAV_AH_ALT	192	FLOAT	600.0f	防碰撞基准点高度
NAV_FORCE_VT	200	INT32	1	是否强制垂直起飞

系统级导航参数

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

参数名称	行号	类型	默认值	描述
COM_ARM_MIS_REQ	712	INT32	0	解锁前是否要求有效任务
NAV_DLL_ACT	861	INT32	0	数据链路丢失时的动作
NAV_RCL_ACT	880	INT32	2	RC信号丢失时的动作

跟随目标参数

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

参数名称	行号	类型	默认值	描述
NAV_MIN_FT_HT	55	FLOAT	8.0f	跟随目标最小高度
NAV_FT_DST	66	FLOAT	8.0f	跟随目标距离
NAV_FT_FS	77	INT32	1	跟随目标前向速度缩放
NAV_FT_RS	90	FLOAT	0.5f	跟随目标旋转速度缩放

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信号恢复或由用户接管控制。