Browse Source

AP_NavEKF3: Fix roundoff, and missing offset handling of getLLH

mission-4.1.18
Michael du Breuil 7 years ago committed by Francisco Ferreira
parent
commit
1991f223ac
  1. 7
      libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp

7
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp

@ -313,10 +313,13 @@ bool NavEKF3_core::getHAGL(float &HAGL) const @@ -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;

Loading…
Cancel
Save