diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/Sensor.hpp b/src/modules/ekf2/EKF/aid_sources/range_finder/Sensor.hpp deleted file mode 100644 index 8134418fab..0000000000 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/Sensor.hpp +++ /dev/null @@ -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 - * - */ - -#ifndef EKF_SENSOR_HPP -#define EKF_SENSOR_HPP - -#include - -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 diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp index 17c353d4d2..4e04fa24d3 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp +++ b/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) -{ - updateDtDataLpf(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); - 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()) { diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp index 194ca1a027..a44f6eba5a 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp +++ b/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 #include @@ -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 override { return _is_sample_ready && _is_sample_valid; } - bool isDataReady() const { return _is_sample_ready; } - bool isRegularlySendingData() const override { return _is_regularly_sending_data; } + bool isDataHealthy() const { return _is_sample_ready && _is_sample_valid; } + bool isRegularlySendingData() const { 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 diff --git a/src/modules/ekf2/test/test_SensorRangeFinder.cpp b/src/modules/ekf2/test/test_SensorRangeFinder.cpp index ecf69d83b5..37a5e66bc9 100644 --- a/src/modules/ekf2/test/test_SensorRangeFinder.cpp +++ b/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{}; - sample.rng = 1.f; - sample.time_us = 1e6; - sample.quality = 9; + const Dcmf attitude{Eulerf(0.f, 0.f, 0.f)}; + _range_finder.setSample(_good_sample); - _range_finder.setRange(sample.rng); - _range_finder.setDataReadiness(true); - _range_finder.setValidity(true); + _range_finder.setRange(1.23); + _range_finder.runChecks(_good_sample.time_us, attitude); 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()); - }