From f33aaa4cc890bb24ab426b7fc82534b7d63c4203 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;