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

ekf2: setEkfGlobalOrigin() reset baro and hgt sensor offsets if necessary

Browse Source 
- handle uninitalized _gps_alt_ref
 - add basic lat/lon/alt sanity checks
This commit is contained in:
Daniel Agar
2022-06-16 00:59:54 -04:00
committed by GitHub
parent fc3d88bb67
commit 1980b5c5e8
5 changed files with 71 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/gps_checks.cpp
+4 -1
View File
@@ -78,7 +78,10 @@ bool Ekf::collect_gps(const gpsMessage &gps)
}
// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
_gps_alt_ref = 1e-3f * (float)gps.alt + _state.pos(2);
if (!PX4_ISFINITE(_gps_alt_ref)) {
_gps_alt_ref = 1e-3f * (float)gps.alt + _state.pos(2);
}
_NED_origin_initialised = true;
_earth_rate_NED = calcEarthRateNED((float)math::radians(_pos_ref.getProjectionReferenceLat()));