From 879771420381738e29a3058556a8ef79f24f830f Mon Sep 17 00:00:00 2001 From: priseborough Date: Sat, 20 Aug 2016 11:31:09 +1000 Subject: [PATCH] AP_NavEKF2: Fix bug in handling of invalid range finder data --- libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 06e1bf74d9..780b74164c 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -82,6 +82,9 @@ void NavEKF2_core::readRangeFinder(void) // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it storedRange.push(rangeDataNew); + // indicate we have updated the measurement + rngValidMeaTime_ms = imuSampleTime_ms; + } else if (!takeOffDetected && ((imuSampleTime_ms - rngValidMeaTime_ms) > 200)) { // before takeoff we assume on-ground range value if there is no data rangeDataNew.time_ms = imuSampleTime_ms; @@ -96,10 +99,10 @@ void NavEKF2_core::readRangeFinder(void) // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it storedRange.push(rangeDataNew); + // indicate we have updated the measurement + rngValidMeaTime_ms = imuSampleTime_ms; + } - - rngValidMeaTime_ms = imuSampleTime_ms; - } } }