fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-13 23:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

HomePosition: only evaluate vehicle_gps_position if copy is successful

Browse Source
This commit is contained in:
Matthias Grob
2024-10-12 09:18:22 +02:00
parent 67fb4cfec8
commit 26845d9746
1 changed files with 14 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/HomePosition.cpp
+14 -13
View File
@@ -310,21 +310,22 @@ void HomePosition::update(bool set_automatically, bool check_if_changed)
if (_vehicle_gps_position_sub.updated()) {
sensor_gps_s vehicle_gps_position;
_vehicle_gps_position_sub.copy(&vehicle_gps_position);
_gps_lat = vehicle_gps_position.latitude_deg;
_gps_lon = vehicle_gps_position.longitude_deg;
_gps_alt = vehicle_gps_position.altitude_msl_m;
_gps_eph = vehicle_gps_position.eph;
_gps_epv = vehicle_gps_position.epv;
if (_vehicle_gps_position_sub.copy(&vehicle_gps_position)) {
_gps_lat = vehicle_gps_position.latitude_deg;
_gps_lon = vehicle_gps_position.longitude_deg;
_gps_alt = vehicle_gps_position.altitude_msl_m;
_gps_eph = vehicle_gps_position.eph;
_gps_epv = vehicle_gps_position.epv;
const hrt_abstime now = hrt_absolute_time();
const bool time = (now < vehicle_gps_position.timestamp + 1_s);
const bool fix = vehicle_gps_position.fix_type >= kHomePositionGPSRequiredFixType;
const bool eph = vehicle_gps_position.eph < kHomePositionGPSRequiredEPH;
const bool epv = vehicle_gps_position.epv < kHomePositionGPSRequiredEPV;
const bool evh = vehicle_gps_position.s_variance_m_s < kHomePositionGPSRequiredEVH;
_gps_position_for_home_valid = time && fix && eph && epv && evh;
const hrt_abstime now = hrt_absolute_time();
const bool time = (now < vehicle_gps_position.timestamp + 1_s);
const bool fix = vehicle_gps_position.fix_type >= kHomePositionGPSRequiredFixType;
const bool eph = vehicle_gps_position.eph < kHomePositionGPSRequiredEPH;
const bool epv = vehicle_gps_position.epv < kHomePositionGPSRequiredEPV;
const bool evh = vehicle_gps_position.s_variance_m_s < kHomePositionGPSRequiredEVH;
_gps_position_for_home_valid = time && fix && eph && epv && evh;
}
}
const vehicle_local_position_s &lpos = _local_position_sub.get();