From a75a383ef24ccfeddba46bcd84f1d0a74e590345 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Fri, 7 Oct 2016 08:00:39 +1100
Subject: [PATCH] AP_NavEKF2: Update protection for out of focus flow data

---
 libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
index 2126faa0ba..a46a930bc1 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
@@ -43,8 +43,10 @@ void NavEKF2_core::SelectFlowFusion()
     gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000);
     // Perform tilt check
     bool tiltOK = (prevTnb.c.z > frontend->DCM33FlowMin);
-    // Constrain measurements to zero if we are on the ground
-    if (frontend->_fusionModeGPS == 3 && !takeOffDetected) {
+    // Constrain measurements to zero if takeoff is not detected and the height above ground
+    // is insuffient to achieve acceptable focus. This allows the vehicle to be picked up
+    // and carried to test optical flow operation
+    if (!takeOffDetected && ((terrainState - stateStruct.position.z) < 0.5f)) {
         ofDataDelayed.flowRadXYcomp.zero();
         ofDataDelayed.flowRadXY.zero();
         flowDataValid = true;