From 17d9018a9136e29b575227ac2a52619563dc97c0 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 29 Sep 2022 11:33:21 +1000
Subject: [PATCH] AP_NavEKF3: fixed EKF3 origin alt inconsistency

always use common origin, and adjust output posD for difference
between public and local lane origin heights
---
 libraries/AP_NavEKF3/AP_NavEKF3.cpp         | 4 ++++
 libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 6 ++++++
 2 files changed, 10 insertions(+)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp
index 892c25756d..631a63e7ff 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp
@@ -1375,6 +1375,10 @@ bool NavEKF3::getOriginLLH(struct Location &loc) const
     if (!core) {
         return false;
     }
+    if (common_origin_valid) {
+        loc = common_EKF_origin;
+        return true;
+    }
     return core[primary].getOriginLLH(loc);
 }
 
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
index 6f3b99ac84..aced7b8b02 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
@@ -268,6 +268,12 @@ bool NavEKF3_core::getPosD(float &posD) const
         posD = outputDataNew.position.z + posOffsetNED.z + 0.01f * (float)EKF_origin.alt - (float)ekfGpsRefHgt;
     }
 
+    // adjust posD for difference between our origin and the public_origin
+    Location local_origin;
+    if (getOriginLLH(local_origin)) {
+        posD += (public_origin.alt - local_origin.alt) * 0.01;
+    }
+
     // Return the current height solution status
     return filterStatus.flags.vert_pos;