From fca8d33c50528630ed977ecb5144acdb9b4d4931 Mon Sep 17 00:00:00 2001
From: "Dr.-Ing. Amilcar do Carmo Lucas" <amilcar.lucas@iav.de>
Date: Wed, 20 May 2020 12:23:25 +0200
Subject: [PATCH] AP_NavEKF3: fix vertical flyaways when rangefinder stops
 providing data

---
 libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
index fe7de03c2e..aa2a169813 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
@@ -54,6 +54,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