From 66e9286cf6cd4ac867a3149ef30305f9c96e39ac Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Thu, 13 Sep 2018 13:39:21 -0700
Subject: [PATCH] AP_NavEKF2: Fix roundoff, and missing offset handling of
 getLLH

---
 libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
index d3241e1019..e49a3cd216 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
@@ -316,10 +316,12 @@ bool NavEKF2_core::getHAGL(float &HAGL) const
 bool NavEKF2_core::getLLH(struct Location &loc) const
 {
     const AP_GPS &gps = AP::gps();
+    Location origin;
+    float posD;
 
-    if(validOrigin) {
+    if(getPosD(posD) && getOriginLLH(origin)) {
         // Altitude returned is an absolute altitude relative to the WGS-84 spherioid
-        loc.alt =  100 * (int32_t)(ekfGpsRefHgt - (double)outputDataNew.position.z);
+        loc.alt =  origin.alt - posD*100;
         loc.flags.relative_alt = 0;
         loc.flags.terrain_alt = 0;