From d701cfcb75a523df4bba497a053182a16442ac0d Mon Sep 17 00:00:00 2001 From: priseborough Date: Sat, 29 Nov 2014 20:26:35 +1100 Subject: [PATCH] AP_NavEKF: Output terrain relative PosD when relying on Optical Flow --- libraries/AP_NavEKF/AP_NavEKF.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 7f1f93d159..ac87d7a42b 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -3576,7 +3576,14 @@ void NavEKF::getVelNED(Vector3f &vel) const // return false if no position is available bool NavEKF::getPosNED(Vector3f &pos) const { - pos = state.position; + 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 - flowStates[1]; + } else { + pos.z = state.position.z; + } return true; }