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 · c57c5c0 · c57c5c0
1 parent ff7b583
commit c57c5c0
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;