further improve logic

Signed-off-by: RomanBapst <bapstroman@gmail.com>

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

@@ -76,25 +76,33 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us)
 	if (_is_sample_ready) {
 		_is_sample_valid = false;
 
-		if (_sample.quality == 0) {
+		_time_bad_quality_us = _sample.quality == 0 ? current_time_us : _time_last_valid_us;
-			_time_bad_quality_us = current_time_us;
 
-		} else if (current_time_us - _time_bad_quality_us > _quality_hyst_us) {
+		const bool quality_ok = isQualityOk(current_time_us);
-			// We did not receive bad quality data for some time
 
-			if (isTiltOk() && isDataInRange()) {
+		if (!quality_ok || !isTiltOk() || !isDataInRange()) {
-				updateStuckCheck();
+			return;
-				updateFogCheck(getDistBottom(), _sample.time_us);
+		}
 
-				if (!_is_stuck && !_is_blocked) {
+		updateStuckCheck();
-					_is_sample_valid = true;
+
-					_time_last_valid_us = _sample.time_us;
+		if (!_is_stuck) {
-				}
+			updateFogCheck(getDistBottom(), _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
+{
+	const bool bad_quality = current_time_us - _time_bad_quality_us > _quality_hyst_us;
+	return bad_quality;
+}
+
 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.

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

@@ -131,6 +131,7 @@ private:
 	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 @@ private:
 	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