From a0957a83f8112d816c604b52ea55e985f5beed7f Mon Sep 17 00:00:00 2001 From: priseborough <p_riseborough@live.com.au> Date: Wed, 7 Jan 2015 17:27:28 +1100 Subject: [PATCH] AP_NavEKF: Fix bug in reported position and velocity The last known position was being output on the velocities when in constant position mode. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 241335965..5d3e0c5f4 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -3520,18 +3520,19 @@ void NavEKF::getAccelNED(Vector3f &accelNED) const { void NavEKF::getVelNED(Vector3f &vel) const { vel = state.velocity; - if (constPosMode) { - vel.x = lastKnownPositionNE.x; - vel.y = lastKnownPositionNE.y; - } } // return the last calculated NED position relative to the reference point (m). // return false if no position is available bool NavEKF::getPosNED(Vector3f &pos) const { - pos.x = state.position.x; - pos.y = state.position.y; + if (constPosMode) { + pos.x = lastKnownPositionNE.x; + pos.y = lastKnownPositionNE.y; + } else { + pos.x = state.position.x; + pos.y = state.position.y; + } // If relying on optical flow, then output ground relative position so that the vehicle does terain following if (_fusionModeGPS == 3) { pos.z = state.position.z - terrainState; -- GitLab