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

AP_NavEKF3: Fix roundoff, and missing offset handling of getLLH

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

7
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -313,10 +313,13 @@ bool NavEKF3_core::getHAGL(float &HAGL) const
bool NavEKF3_core::getLLH(struct Location &loc) const
{
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
loc.alt = 100 * (int32_t)(ekfGpsRefHgt - (double)outputDataNew.position.z);
loc.alt = origin.alt - posD*100;
loc.flags.relative_alt = 0;
loc.flags.terrain_alt = 0;