Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: ensure that both AMSL and relative alt are correct after a reset

Browse Source 
this forces the EKF origin to the GPS alt on a height datum reset if
we have GPS lock. If we don't do this then the reported AMSL alt will
drift over time away from the GPS alt when we reset while on the
ground
This commit is contained in:
Andrew Tridgell 2019-07-02 05:55:13 +10:00
parent f52ccd9982
commit 2fc28e2eae
1 changed files with 13 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -215,7 +215,19 @@ bool NavEKF2_core::resetHeightDatum(void)
stateStruct.position.z = 0.0f;
// adjust the height of the EKF origin so that the origin plus baro height before and after the reset is the same
if (validOrigin) {
ekfGpsRefHgt += (double)oldHgt;
const AP_GPS &gps = AP::gps();
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
// if we have a good GPS lock then reset to the GPS
// altitude. This ensures the reported AMSL alt from
// getLLH() is equal to GPS altitude, while also ensuring
// that the relative alt is zero
ekfGpsRefHgt = gps.location().alt*0.01;
} else {
// if we don't have GPS lock then we shouldn't be doing a
// resetHeightDatum, but if we do then the best option is
// to maintain the old error
ekfGpsRefHgt += oldHgt;
}
}
// adjust the terrain state
terrainState += oldHgt;