From d1d49857f98ed17f17d51417515d9a5e786b9f16 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Sat, 14 Jul 2018 07:51:04 +1000
Subject: [PATCH] EKF: Prevent stuck range checks running without new data

---
 EKF/control.cpp | 11 ++++++++---
 1 file changed, 8 insertions(+), 3 deletions(-)

diff --git a/EKF/control.cpp b/EKF/control.cpp
index e5c21b93c2..7c49d5ca3f 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -1138,15 +1138,20 @@ void Ekf::rangeAidConditionsMet()
 
 void Ekf::checkRangeDataValidity()
 {
-	// reset fault status
-	_rng_hgt_faulty = false;
-
 	// check if out of date
 	if ((_imu_sample_delayed.time_us - _range_sample_delayed .time_us) > 2 * RNG_MAX_INTERVAL) {
 		_rng_hgt_faulty = true;
 		return;
 	}
 
+	// Don't run the checks after this unless we have retrieved new data from the buffer
+	if (!_range_data_ready) {
+		return;
+	} else {
+		// reset fault status when we get new data
+		_rng_hgt_faulty = false;
+	}
+
 	// Check if excessively tilted
 	if (_R_rng_to_earth_2_2 < _params.range_cos_max_tilt) {
 		_rng_hgt_faulty = true;