Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: origin handling fixes from Francisco

Browse Source
This commit is contained in:
Andrew Tridgell 2019-07-07 10:00:18 +10:00 committed by Randy Mackay
parent bca5dbaaa8
commit d6edab35e4
2 changed files with 8 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -563,7 +563,11 @@ void NavEKF2_core::readGpsData()
// convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
if (validOrigin) { if (validOrigin) {
gpsDataNew.pos = location_diff(EKF_origin, gpsloc); gpsDataNew.pos = location_diff(EKF_origin, gpsloc);
gpsDataNew.hgt = (float)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt); if ((frontend->_originHgtMode & (1<<2)) == 0) {
gpsDataNew.hgt = (float)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt);
} else {
gpsDataNew.hgt = 0.01 * (gpsloc.alt - EKF_origin.alt);
}
storedGPS.push(gpsDataNew); storedGPS.push(gpsDataNew);
// declare GPS available for use // declare GPS available for use
gpsNotAvailable = false; gpsNotAvailable = false;

5
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -222,14 +222,15 @@ bool NavEKF2_core::resetHeightDatum(void)
// if we don't have GPS lock then we shouldn't be doing a // if we don't have GPS lock then we shouldn't be doing a
// resetHeightDatum, but if we do then the best option is // resetHeightDatum, but if we do then the best option is
// to maintain the old error // to maintain the old error
ekfGpsRefHgt += (int32_t)(100.0f * oldHgt); EKF_origin.alt += (int32_t)(100.0f * oldHgt);
} else { } else {
// if we have a good GPS lock then reset to the GPS // if we have a good GPS lock then reset to the GPS
// altitude. This ensures the reported AMSL alt from // altitude. This ensures the reported AMSL alt from
// getLLH() is equal to GPS altitude, while also ensuring // getLLH() is equal to GPS altitude, while also ensuring
// that the relative alt is zero // that the relative alt is zero
ekfGpsRefHgt = AP::gps().location().alt*0.01; EKF_origin.alt = AP::gps().location().alt;
} }
ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt;
} }
// set the terrain state to zero (on ground). The adjustment for // set the terrain state to zero (on ground). The adjustment for