AP_NavEKF2: fix vertical flyaways when rangefinder stops providing data

2020-05-20 12:23:05 +02:00 · 2020-05-20 12:23:05 +02:00 · 2b02c84ae4
parent 3f6b43e3aa
commit 2b02c84ae4
1 changed files with 2 additions and 0 deletions
--- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
@ -52,6 +52,8 @@ void NavEKF2_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