From 634db441f885c8bff57bd98c02c2ce7c2896c1d8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 9 Jul 2019 16:27:48 +1000 Subject: [PATCH] AP_NavEKF3: origin handling fixes from Francisco --- libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 6 +++++- libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 5 +++-- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index f8d78c2561..6dca3e65c3 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -604,7 +604,11 @@ void NavEKF3_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; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 38176bc7ae..93524000e7 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -219,14 +219,15 @@ bool NavEKF3_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