From 1301b9797ac3cb445f50209f87b784dcdad7402d Mon Sep 17 00:00:00 2001 From: priseborough Date: Fri, 28 Apr 2017 11:04:20 +1000 Subject: [PATCH] AP_NavEKF3: Fix solution validity reporting bug Fixes bug that causes relative position status to report as false when using optical flow and using range finder as the primary height sensor. --- libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp index 770e063cd2..31c2a968cc 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp @@ -40,8 +40,8 @@ void NavEKF3_core::SelectFlowFusion() // Perform Data Checks // Check if the optical flow data is still valid flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000); - // check is the terrain offset estimate is still valid - gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000); + // check is the terrain offset estimate is still valid - if we are using range finder as the main height reference, the ground is assumed to be at 0 + gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000) || (activeHgtSource == HGT_SOURCE_RNG); // Perform tilt check bool tiltOK = (prevTnb.c.z > frontend->DCM33FlowMin); // Constrain measurements to zero if takeoff is not detected and the height above ground