From 04f24e7d5912356dd6d36c9a8e6c0b2418d0afe7 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 258bd6bcf588e4..c5068c4111104a 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;