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

AP_NavEKF2: Fix roundoff, and missing offset handling of getLLH

Browse Source
This commit is contained in:
Michael du Breuil 2018-09-13 13:39:21 -07:00 committed by Francisco Ferreira
parent 7e1ed948f5
commit 66e9286cf6
1 changed files with 4 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -316,10 +316,12 @@ bool NavEKF2_core::getHAGL(float &HAGL) const
bool NavEKF2_core::getLLH(struct Location &loc) const bool NavEKF2_core::getLLH(struct Location &loc) const
{ {
const AP_GPS &gps = AP::gps(); const AP_GPS &gps = AP::gps();
Location origin;
float posD;
if(validOrigin) { if(getPosD(posD) && getOriginLLH(origin)) {
// Altitude returned is an absolute altitude relative to the WGS-84 spherioid // Altitude returned is an absolute altitude relative to the WGS-84 spherioid
loc.alt = 100 * (int32_t)(ekfGpsRefHgt - (double)outputDataNew.position.z); loc.alt = origin.alt - posD*100;
loc.flags.relative_alt = 0; loc.flags.relative_alt = 0;
loc.flags.terrain_alt = 0; loc.flags.terrain_alt = 0;