From a75a383ef24ccfeddba46bcd84f1d0a74e590345 Mon Sep 17 00:00:00 2001 From: priseborough 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;