From c57c5c06d47e661f44b616c3b6fe72163caaa15d Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Mon, 29 Apr 2024 14:48:00 +1000
Subject: [PATCH] AP_NavEKF3: always apply ekfGpsRefHgt corrections

Co-authored-by: Pavlo Kolomiiets <pkolomiets@gmail.com>

https://github.com/ArduPilot/ardupilot/issues/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.
---
 libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 6 +-----
 1 file changed, 1 insertion(+), 5 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
index 258bd6bcf588e..c5068c4111104 100644
--- 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;