From f0ea858e4c37aea16e5713df50ae841b81d747ab Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Wed, 7 Jan 2015 17:45:22 +1100
Subject: [PATCH] AP_NavEKF: Make LLH output report last known position in
const pos mode
---
libraries/AP_NavEKF/AP_NavEKF.cpp | 6 +++++-
1 file changed, 5 insertions(+), 1 deletion(-)
diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 5d3e0c5f4..4021a728f 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -3644,7 +3644,11 @@ bool NavEKF::getLLH(struct Location &loc) const
loc.alt = _ahrs->get_home().alt - state.position.z*100;
loc.flags.relative_alt = 0;
loc.flags.terrain_alt = 0;
- location_offset(loc, state.position.x, state.position.y);
+ if (constPosMode) {
+ location_offset(loc, lastKnownPositionNE.x, lastKnownPositionNE.y);
+ } else {
+ location_offset(loc, state.position.x, state.position.y);
+ }
return true;
}
--
GitLab