Fix deadlock in fog detection logic and improved unit test

src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp

@@ -76,25 +76,27 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us)
 	if (_is_sample_ready) {
 		_is_sample_valid = false;
 
-		if (_sample.quality == 0) {
-			_time_bad_quality_us = current_time_us;
+		_time_bad_quality_us = _sample.quality == 0 ? current_time_us : _time_bad_quality_us;
 
-		} else if (current_time_us - _time_bad_quality_us > _quality_hyst_us) {
-			// We did not receive bad quality data for some time
+		if (!isQualityOk(current_time_us) || !isTiltOk() || !isDataInRange()) {
+			return;
+		}
 
-			if (isTiltOk() && isDataInRange()) {
-				updateStuckCheck();
-				updateFogCheck(getDistBottom(), _sample.time_us);
+		updateStuckCheck();
+		updateFogCheck(getDistBottom(), _sample.time_us);
 
-				if (!_is_stuck && !_is_blocked) {
-					_is_sample_valid = true;
-					_time_last_valid_us = _sample.time_us;
-				}
-			}
+		if (!_is_stuck && !_is_blocked) {
+			_is_sample_valid = true;
+			_time_last_valid_us = _sample.time_us;
 		}
 	}
 }
 
+bool SensorRangeFinder::isQualityOk(uint64_t current_time_us) const
+{
+	return current_time_us - _time_bad_quality_us > _quality_hyst_us;
+}
+
 void SensorRangeFinder::updateDtDataLpf(uint64_t current_time_us)
 {
 	// Calculate a first order IIR low-pass filtered time of arrival between samples using a 2 second time constant.
@@ -149,7 +151,7 @@ void SensorRangeFinder::updateStuckCheck()
 
 void SensorRangeFinder::updateFogCheck(const float dist_bottom, const uint64_t time_us)
 {
-	if (_max_fog_dist > 0.f && time_us - _time_last_valid_us < 1e6) {
+	if (_max_fog_dist > 0.f) {
 
 		const float median_dist = _median_dist.apply(dist_bottom);
 		const float factor = 2.f; // magic hardcoded factor

src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp

@@ -131,6 +131,7 @@ class SensorRangeFinder : public Sensor
 	bool isDataContinuous() const { return _dt_data_lpf < 2e6f; }
 	bool isTiltOk() const { return _cos_tilt_rng_to_earth > _range_cos_max_tilt; }
 	bool isDataInRange() const;
+	bool isQualityOk(uint64_t current_time_us) const;
 	void updateStuckCheck();
 	void updateFogCheck(const float dist_bottom, const uint64_t time_us);
 
@@ -184,6 +185,7 @@ class SensorRangeFinder : public Sensor
 	float _max_fog_dist{0.f};	//< maximum distance at which the range finder could detect fog (m)
 	math::MedianFilter<float, 5> _median_dist{};
 	float _prev_median_dist{0.f};
+
 };
 
 } // namespace sensor

src/modules/ekf2/test/test_SensorRangeFinder.cpp

@@ -354,14 +354,13 @@ TEST_F(SensorRangeFinderTest, blockedByFog)
 	const Dcmf attitude{Eulerf(0.f, 0.f, 0.f)};
 	const uint64_t dt_update_us = 10e3;
 	uint64_t dt_sensor_us = 3e5;
-	uint64_t duration_us = 5e5;
+	uint64_t duration_us = 2e6;
 
 	updateSensorAtRate(_good_sample, duration_us, dt_update_us, dt_sensor_us);
 	// THEN: the data should be marked as healthy
 	EXPECT_TRUE(_range_finder.isDataHealthy());
 	EXPECT_TRUE(_range_finder.isHealthy());
 
-
 	// WHEN: sensor is then blocked by fog
 	// range jumps to value below 2m
 	uint64_t t_now_us = _range_finder.getSampleAddress()->time_us;
@@ -374,6 +373,7 @@ TEST_F(SensorRangeFinderTest, blockedByFog)
 
 	// WHEN: the sensor is not blocked by fog anymore
 	sample.rng = 5.f;
+	sample.time_us = _range_finder.getSampleAddress()->time_us;
 	updateSensorAtRate(sample, duration_us, dt_update_us, dt_sensor_us);
 
 	// THEN: the data should be marked as healthy again