ekf: sensor_range_finder: reduce abstractions, bring back to functional view

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

@@ -1,84 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file Sensor.hpp
- * Abstract class for sensors
- *
- * @author Mathieu Bresciani <brescianimathieu@gmail.com>
- *
- */
-
-#ifndef EKF_SENSOR_HPP
-#define EKF_SENSOR_HPP
-
-#include <cstdint>
-
-namespace estimator
-{
-namespace sensor
-{
-
-class Sensor
-{
-public:
-	virtual ~Sensor() {};
-
-	/*
-	 * run sanity checks on the current data
-	 * this has to be called immediately after
-	 * setting new data
-	 */
-	virtual void runChecks() {};
-
-	/*
-	 * return true if the sensor is healthy
-	 */
-	virtual bool isHealthy() const = 0;
-
-	/*
-	 * return true if the delayed sample is healthy
-	 * and can be fused in the estimator
-	 */
-	virtual bool isDataHealthy() const = 0;
-
-	/*
-	 * return true if the sensor data rate is
-	 * stable and high enough
-	 */
-	virtual bool isRegularlySendingData() const = 0;
-};
-
-} // namespace sensor
-} // namespace estimator
-#endif // !EKF_SENSOR_HPP

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

@@ -49,24 +49,23 @@ namespace sensor
 
 void SensorRangeFinder::runChecks(const uint64_t current_time_us, const matrix::Dcmf &R_to_earth, bool in_air)
 {
-	_in_air = in_air;
+	_is_sample_valid = true;
 
-	updateSensorToEarthRotation(R_to_earth);
-	updateValidity(current_time_us);
-}
-
-void SensorRangeFinder::updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth)
-{
 	// calculate 2,2 element of rotation matrix from sensor frame to earth frame
 	// this is required for use of range finder and flow data
 	_cos_tilt_rng_to_earth = R_to_earth(2, 0) * _sin_pitch_offset + R_to_earth(2, 2) * _cos_pitch_offset;
-}
 
-void SensorRangeFinder::updateValidity(uint64_t current_time_us)
+	// Calculate a first order IIR low-pass filtered time of arrival between samples using a 2 second time constant.
-{
+	float alpha = 0.5f * _dt_update;
-	updateDtDataLpf(current_time_us);
+	_dt_data_lpf = _dt_data_lpf * (1.0f - alpha) + alpha * (current_time_us - _sample.time_us);
 
-	if (isSampleOutOfDate(current_time_us) || !isDataContinuous()) {
+	// Apply spike protection to the filter state.
+	_dt_data_lpf = fminf(_dt_data_lpf, 4e6f);
+
+	bool is_continuous = _dt_data_lpf < 2e6f;
+	bool is_out_of_date = (current_time_us - _sample.time_us) > 2 * RNG_MAX_INTERVAL;
+
+	if (is_out_of_date || !is_continuous) {
 		_is_sample_valid = false;
 		_is_regularly_sending_data = false;
 		return;
@@ -78,8 +77,23 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us)
 	if (_is_sample_ready) {
 		_is_sample_valid = false;
 
+		bool quality_ok = false;
 
-		if (!isQualityOk(current_time_us) || !isTiltOk() || !isDataInRange()) {
+		// Mark quality as OK while on the ground
+		if (!in_air) {
+			_sample.rng = _rng_valid_min_val; // set to min val while on ground
+			quality_ok = true;
+
+		} else {
+			_time_bad_quality_us = _sample.quality == 0 ? current_time_us : _time_bad_quality_us;
+			quality_ok = current_time_us - _time_bad_quality_us > _quality_hyst_us;
+		}
+
+		bool in_range = (_sample.rng >= _rng_valid_min_val) && (_sample.rng <= _rng_valid_max_val);
+
+		bool tilt_ok = _cos_tilt_rng_to_earth > _range_cos_max_tilt;
+
+		if (!quality_ok || !tilt_ok || !in_range) {
 			return;
 		}
 
@@ -93,39 +107,6 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us)
 	}
 }
 
-bool SensorRangeFinder::isQualityOk(uint64_t current_time_us)
-{
-	// Mark quality as OK while on the ground
-	if (!_in_air) {
-		_sample.rng = _rng_valid_min_val; // set to min val while on ground
-		return true;
-	}
-
-	_time_bad_quality_us = _sample.quality == 0 ? current_time_us : _time_bad_quality_us;
-
-	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.
-	float alpha = 0.5f * _dt_update;
-	_dt_data_lpf = _dt_data_lpf * (1.0f - alpha) + alpha * (current_time_us - _sample.time_us);
-
-	// Apply spike protection to the filter state.
-	_dt_data_lpf = fminf(_dt_data_lpf, 4e6f);
-}
-
-inline bool SensorRangeFinder::isSampleOutOfDate(uint64_t current_time_us) const
-{
-	return (current_time_us - _sample.time_us) > 2 * RNG_MAX_INTERVAL;
-}
-
-inline bool SensorRangeFinder::isDataInRange() const
-{
-	return (_sample.rng >= _rng_valid_min_val) && (_sample.rng <= _rng_valid_max_val);
-}
-
 void SensorRangeFinder::updateStuckCheck()
 {
 	if (!isStuckDetectorEnabled()) {

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

@@ -41,8 +41,6 @@
 #ifndef EKF_SENSOR_RANGE_FINDER_HPP
 #define EKF_SENSOR_RANGE_FINDER_HPP
 
-#include "Sensor.hpp"
-
 #include <matrix/math.hpp>
 #include <lib/mathlib/math/filter/MedianFilter.hpp>
 
@@ -60,17 +58,15 @@ struct rangeSample {
 static constexpr uint64_t RNG_MAX_INTERVAL =
 	200e3;  ///< Maximum allowable time interval between range finder measurements (uSec)
 
-class SensorRangeFinder : public Sensor
+class SensorRangeFinder
 {
 public:
 	SensorRangeFinder() = default;
-	~SensorRangeFinder() override = default;
+	~SensorRangeFinder() = default;
 
 	void runChecks(uint64_t current_time_us, const matrix::Dcmf &R_to_earth, bool in_air = true);
-	bool isHealthy() const override { return _is_sample_valid; }
+	bool isDataHealthy() const { return _is_sample_ready && _is_sample_valid; }
-	bool isDataHealthy() const override { return _is_sample_ready && _is_sample_valid; }
+	bool isRegularlySendingData() const { return _is_regularly_sending_data; }
-	bool isDataReady() const { return _is_sample_ready; }
-	bool isRegularlySendingData() const override { return _is_regularly_sending_data; }
 	bool isStuckDetectorEnabled() const { return _stuck_threshold > 0.f; }
 
 	void setSample(const rangeSample &sample)
@@ -115,21 +111,11 @@ public:
 	float getDistBottom() const { return _sample.rng * _cos_tilt_rng_to_earth; }
 
 	void setDataReadiness(bool is_ready) { _is_sample_ready = is_ready; }
-	void setValidity(bool is_valid) { _is_sample_valid = is_valid; }
 
 	float getValidMinVal() const { return _rng_valid_min_val; }
 	float getValidMaxVal() const { return _rng_valid_max_val; }
 
 private:
-	void updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth);
-
-	void updateValidity(uint64_t current_time_us);
-	void updateDtDataLpf(uint64_t current_time_us);
-	bool isSampleOutOfDate(uint64_t current_time_us) const;
-	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);
 	void updateStuckCheck();
 	void updateFogCheck(const float dist_bottom, const uint64_t time_us);
 
@@ -174,7 +160,6 @@ private:
 	 */
 	uint64_t _time_bad_quality_us{};	///< timestamp at which range finder signal quality was 0 (used for hysteresis)
 	uint64_t _quality_hyst_us{};		///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (us)
-	bool _in_air{};
 
 	/*
 	 * Fog check

src/modules/ekf2/test/test_SensorRangeFinder.cpp

@@ -103,26 +103,20 @@ void SensorRangeFinderTest::testTilt(const Eulerf &euler, bool should_pass)
 
 	if (should_pass) {
 		EXPECT_TRUE(_range_finder.isDataHealthy());
-		EXPECT_TRUE(_range_finder.isHealthy());
 
 	} else {
 		EXPECT_FALSE(_range_finder.isDataHealthy());
-		EXPECT_FALSE(_range_finder.isHealthy());
 	}
 }
 
 TEST_F(SensorRangeFinderTest, setRange)
 {
-	rangeSample sample{};
+	const Dcmf attitude{Eulerf(0.f, 0.f, 0.f)};
-	sample.rng = 1.f;
+	_range_finder.setSample(_good_sample);
-	sample.time_us = 1e6;
-	sample.quality = 9;
 
-	_range_finder.setRange(sample.rng);
+	_range_finder.setRange(1.23);
-	_range_finder.setDataReadiness(true);
+	_range_finder.runChecks(_good_sample.time_us, attitude);
-	_range_finder.setValidity(true);
 	EXPECT_TRUE(_range_finder.isDataHealthy());
-	EXPECT_TRUE(_range_finder.isHealthy());
 }
 
 TEST_F(SensorRangeFinderTest, goodData)
@@ -134,7 +128,6 @@ TEST_F(SensorRangeFinderTest, goodData)
 
 	// THEN: the data can be used for aiding
 	EXPECT_TRUE(_range_finder.isDataHealthy());
-	EXPECT_TRUE(_range_finder.isHealthy());
 }
 
 TEST_F(SensorRangeFinderTest, tiltExceeded)
@@ -177,7 +170,6 @@ TEST_F(SensorRangeFinderTest, rangeMaxExceeded)
 
 	// THEN: the data should be marked as unhealthy
 	EXPECT_FALSE(_range_finder.isDataHealthy());
-	EXPECT_FALSE(_range_finder.isHealthy());
 }
 
 TEST_F(SensorRangeFinderTest, rangeMinExceeded)
@@ -192,7 +184,6 @@ TEST_F(SensorRangeFinderTest, rangeMinExceeded)
 
 	// THEN: the data should be marked as unhealthy
 	EXPECT_FALSE(_range_finder.isDataHealthy());
-	EXPECT_FALSE(_range_finder.isHealthy());
 }
 
 TEST_F(SensorRangeFinderTest, outOfDate)
@@ -208,7 +199,6 @@ TEST_F(SensorRangeFinderTest, outOfDate)
 
 	// THEN: the data should be marked as unhealthy
 	EXPECT_FALSE(_range_finder.isDataHealthy());
-	EXPECT_FALSE(_range_finder.isHealthy());
 }
 
 TEST_F(SensorRangeFinderTest, rangeStuck)
@@ -229,7 +219,6 @@ TEST_F(SensorRangeFinderTest, rangeStuck)
 	}
 
 	EXPECT_FALSE(_range_finder.isDataHealthy());
-	EXPECT_FALSE(_range_finder.isHealthy());
 
 	new_sample.quality = 100;
 
@@ -244,13 +233,11 @@ TEST_F(SensorRangeFinderTest, rangeStuck)
 	// because the sensor is "stuck"
 	if (_range_finder.isStuckDetectorEnabled()) {
 		EXPECT_FALSE(_range_finder.isDataHealthy());
-		EXPECT_FALSE(_range_finder.isHealthy());
 
 	} else {
 		// If stuck detector is disabled then the
 		// data should instantly be marked as healthy
 		EXPECT_TRUE(_range_finder.isDataHealthy());
-		EXPECT_TRUE(_range_finder.isHealthy());
 	}
 
 	// BUT WHEN: we continue to send samples but with changing distance
@@ -264,7 +251,6 @@ TEST_F(SensorRangeFinderTest, rangeStuck)
 	// THEN: the data should be marked as healthy
 	// because the sensor is not "stuck" anymore
 	EXPECT_TRUE(_range_finder.isDataHealthy());
-	EXPECT_TRUE(_range_finder.isHealthy());
 }
 
 TEST_F(SensorRangeFinderTest, qualityHysteresis)
@@ -278,13 +264,11 @@ TEST_F(SensorRangeFinderTest, qualityHysteresis)
 	_range_finder.setSample(new_sample);
 	_range_finder.runChecks(new_sample.time_us, attitude);
 	EXPECT_FALSE(_range_finder.isDataHealthy());
-	EXPECT_FALSE(_range_finder.isHealthy());
 
 	new_sample.quality = _good_sample.quality;
 	_range_finder.setSample(new_sample);
 	_range_finder.runChecks(new_sample.time_us, attitude);
 	EXPECT_FALSE(_range_finder.isDataHealthy());
-	EXPECT_FALSE(_range_finder.isHealthy());
 
 	// AND: we need to put enough good data to pass the hysteresis
 	const uint64_t dt = 3e5;
@@ -298,7 +282,6 @@ TEST_F(SensorRangeFinderTest, qualityHysteresis)
 
 	// THEN: the data is again declared healthy
 	EXPECT_TRUE(_range_finder.isDataHealthy());
-	EXPECT_TRUE(_range_finder.isHealthy());
 }
 
 TEST_F(SensorRangeFinderTest, continuity)
@@ -314,7 +297,6 @@ TEST_F(SensorRangeFinderTest, continuity)
 	// THEN: the data should be marked as unhealthy
 	// Note that it also fails the out-of-date test here
 	EXPECT_FALSE(_range_finder.isDataHealthy());
-	EXPECT_FALSE(_range_finder.isHealthy());
 
 	// AND WHEN: the data rate is acceptable
 	dt_sensor_us = 3e5;
@@ -324,11 +306,9 @@ TEST_F(SensorRangeFinderTest, continuity)
 	// THEN: it should still fail until the filter converge
 	// to the new datarate
 	EXPECT_FALSE(_range_finder.isDataHealthy());
-	EXPECT_FALSE(_range_finder.isHealthy());
 
 	updateSensorAtRate(_good_sample, duration_us, dt_update_us, dt_sensor_us);
 	EXPECT_TRUE(_range_finder.isDataHealthy());
-	EXPECT_TRUE(_range_finder.isHealthy());
 }
 
 TEST_F(SensorRangeFinderTest, distBottom)
@@ -359,7 +339,6 @@ TEST_F(SensorRangeFinderTest, blockedByFog)
 	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
@@ -369,7 +348,6 @@ TEST_F(SensorRangeFinderTest, blockedByFog)
 
 	// THEN: the data should be marked as unhealthy
 	EXPECT_FALSE(_range_finder.isDataHealthy());
-	EXPECT_FALSE(_range_finder.isHealthy());
 
 	// WHEN: the sensor is not blocked by fog anymore
 	sample.rng = 5.f;
@@ -378,7 +356,6 @@ TEST_F(SensorRangeFinderTest, blockedByFog)
 
 	// THEN: the data should be marked as healthy again
 	EXPECT_TRUE(_range_finder.isDataHealthy());
-	EXPECT_TRUE(_range_finder.isHealthy());
 
 	// WHEN: the sensor is is not jumping to a value below 2m
 	while (sample.rng > _min_range) {
@@ -390,6 +367,4 @@ TEST_F(SensorRangeFinderTest, blockedByFog)
 
 	// THEN: the data should still be marked as healthy
 	EXPECT_TRUE(_range_finder.isDataHealthy());
-	EXPECT_TRUE(_range_finder.isHealthy());
-
 }