From d701cfcb75a523df4bba497a053182a16442ac0d Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
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;
 }