Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: origin handling fixes from Francisco

This commit is contained in:
Andrew Tridgell 2019-07-09 16:27:22 +10:00
parent 3dfaa5f021
commit 2427440e13
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

@ -580,7 +580,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
if (validOrigin) {
gpsDataNew.pos = EKF_origin.get_distance_NE(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);
// declare GPS available for use
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
// resetHeightDatum, but if we do then the best option is
// to maintain the old error
ekfGpsRefHgt += (int32_t)(100.0f * oldHgt);
EKF_origin.alt += (int32_t)(100.0f * oldHgt);
} else {
// 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 = 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