From b95bc9076baa2b002d84e85202633ca900119100 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sat, 2 May 2020 11:45:06 +0900
Subject: [PATCH] AP_NavEKF2: fix getLLH when no GPS

---
 libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
index bd5ce7ce86..5387d8e424 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
@@ -351,7 +351,11 @@ bool NavEKF2_core::getLLH(struct Location &loc) const
                 // correct for IMU offset (EKF calculations are at the IMU position)
                 loc.lat = EKF_origin.lat;
                 loc.lng = EKF_origin.lng;
-                loc.offset((lastKnownPositionNE.x + posOffsetNED.x), (lastKnownPositionNE.y + posOffsetNED.y));
+                if (PV_AidingMode == AID_NONE) {
+                    loc.offset((lastKnownPositionNE.x + posOffsetNED.x), (lastKnownPositionNE.y + posOffsetNED.y));
+                } else {
+                    loc.offset((outputDataNew.position.x + posOffsetNED.x), (outputDataNew.position.y + posOffsetNED.y));
+                }
                 return false;
             }
         }