From 67903a29e26a94b9506030bc4b71ca75e7ac3c69 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 2 May 2020 11:51:54 +0900 Subject: [PATCH] AP_NavEKF3: fix getLLH when no GPS --- libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 5a41c2dbaa..e9ccaa0b56 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -347,7 +347,11 @@ bool NavEKF3_core::getLLH(struct Location &loc) const // if no GPS fix, provide last known position before entering the mode loc.lat = EKF_origin.lat; loc.lng = EKF_origin.lng; - loc.offset(lastKnownPositionNE.x, lastKnownPositionNE.y); + if (PV_AidingMode == AID_NONE) { + loc.offset(lastKnownPositionNE.x, lastKnownPositionNE.y); + } else { + loc.offset(outputDataNew.position.x, outputDataNew.position.y); + } return false; } }