AP_NavEKF3: always apply ekfGpsRefHgt corrections

Co-authored-by: Pavlo Kolomiiets <[email protected]> #24039 When 3rd bit is set, Ardupilot doesn't use ekfGpsRefHgt to convert GPS alt to local alt. But it still updates ekfGpsRefHgt in correctEkfOriginHeight. And the innovation used in the latter is constantly non-zero.
ArduPilot · Apr 29, 2024 · 04f24e7 · 04f24e7
1 parent 00bf485
commit 04f24e7
Showing 1 changed file with 1 addition and 5 deletions.
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
@@ -684,11 +684,7 @@ void NavEKF3_core::readGpsData()
     if (validOrigin) {
         gpsDataNew.lat = gpsloc.lat;
         gpsDataNew.lng = gpsloc.lng;
-        if ((frontend->_originHgtMode & (1<<2)) == 0) {
-            gpsDataNew.hgt = (ftype)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt);
-        } else {
-            gpsDataNew.hgt = 0.01 * (gpsloc.alt - EKF_origin.alt);
-        }
+        gpsDataNew.hgt = (ftype)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt);
         storedGPS.push(gpsDataNew);
         // declare GPS in use
         gpsIsInUse = true;