From 2f554a26dda4769bec9ac8839d4d0a9e4ae98707 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:05 +0200
Subject: [PATCH] AP_NavEKF2: fix vertical flyaways when rangefinder stops
 providing data

---
 libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
index a5db1f2232..6e4ee699a2 100644
--- 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