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