AP_NavEKF3: fix vertical flyaways when rangefinder stops providing data

2020-05-20 12:23:25 +02:00 · 2020-05-20 12:23:25 +02:00 · 8615b18c26
parent 2b02c84ae4
commit 8615b18c26
1 changed files with 2 additions and 0 deletions
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
@ -48,6 +48,8 @@ void NavEKF3_core::readRangeFinder(void)
                }
                storedRngMeasTime_ms[sensorIndex][rngMeasIndex[sensorIndex]] = imuSampleTime_ms - 25;
                storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = sensor->distance_cm() * 0.01f;
+            } else {
+                continue;
            }

            // check for three fresh samples