From ae725421bccd2556c9f4abcaa1812a35c6304e9d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Aug 2021 18:02:14 +1000 Subject: [PATCH] AP_NavEKF3: fixed output LLH to account for IMU offset thanks to Paul and Randy --- libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 79f4e0bd6f..3ce87c58db 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -285,7 +285,8 @@ bool NavEKF3_core::getLLH(struct Location &loc) const // The EKF is able to provide a position estimate loc.lat = EKF_origin.lat; loc.lng = EKF_origin.lng; - loc.offset(outputDataNew.position.x, outputDataNew.position.y); + loc.offset(outputDataNew.position.x + posOffsetNED.x, + outputDataNew.position.y + posOffsetNED.y); return true; } else { // We have been be doing inertial dead reckoning for too long so use raw GPS if available @@ -295,7 +296,8 @@ bool NavEKF3_core::getLLH(struct Location &loc) const // Return the EKF estimate but mark it as invalid loc.lat = EKF_origin.lat; loc.lng = EKF_origin.lng; - loc.offset(outputDataNew.position.x, outputDataNew.position.y); + loc.offset(outputDataNew.position.x + posOffsetNED.x, + outputDataNew.position.y + posOffsetNED.y); return false; } } @@ -306,7 +308,8 @@ bool NavEKF3_core::getLLH(struct Location &loc) const } else { loc.lat = EKF_origin.lat; loc.lng = EKF_origin.lng; - loc.offset(lastKnownPositionNE.x, lastKnownPositionNE.y); + loc.offset(lastKnownPositionNE.x + posOffsetNED.x, + lastKnownPositionNE.y + posOffsetNED.y); return false; } }