From c212975abe83bf462bc512288ba30358f9a17865 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 6 Jan 2021 11:53:52 +0100 Subject: [PATCH] rng height: reset to baro using common logic The failsafe from rng height to baro height should occur after a reset to baro triggered by the controlHeightSensorTimeouts function and not in the fusion selector. Until now, the EKF was fusing baro measurements between rng updates if the range finder ODR was slower than the EKF update rate. This is not the case anymore as the height reset occurs after 5 seconds. The unit test has been extended to show this behavior. --- EKF/Sensor.hpp | 6 ++++++ EKF/control.cpp | 19 ++++++++++--------- EKF/sensor_range_finder.cpp | 3 +++ EKF/sensor_range_finder.hpp | 2 ++ test/test_EKF_fusionLogic.cpp | 15 ++++++++++++--- 5 files changed, 33 insertions(+), 12 deletions(-) diff --git a/EKF/Sensor.hpp b/EKF/Sensor.hpp index 0ff2e5cfad..a3fe91c8fe 100644 --- a/EKF/Sensor.hpp +++ b/EKF/Sensor.hpp @@ -70,6 +70,12 @@ public: * 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 diff --git a/EKF/control.cpp b/EKF/control.cpp index fb0847e09f..b35ca6f842 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -989,8 +989,8 @@ void Ekf::controlHeightFusion() } } - } else if (_baro_data_ready && !_baro_hgt_faulty) { - startBaroHgtFusion(); + } else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) { + // fuse baro data if there was a reset to baro fuse_height = true; } @@ -1055,16 +1055,17 @@ void Ekf::controlHeightFusion() updateBaroHgtOffset(); - if (isTimedOut(_time_last_hgt_fuse, 2 * RNG_MAX_INTERVAL) && _control_status.flags.rng_hgt - && (!_range_sensor.isDataHealthy())) { + if (_control_status.flags.rng_hgt + && isTimedOut(_time_last_hgt_fuse, 2 * RNG_MAX_INTERVAL) + && !_range_sensor.isDataHealthy() + && _range_sensor.isRegularlySendingData() + && !_control_status.flags.in_air) { // If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements // and are on the ground, then synthesise a measurement at the expected on ground value - if (!_control_status.flags.in_air) { - _range_sensor.setRange(_params.rng_gnd_clearance); - _range_sensor.setDataReadiness(true); - _range_sensor.setValidity(true); // bypass the checks - } + _range_sensor.setRange(_params.rng_gnd_clearance); + _range_sensor.setDataReadiness(true); + _range_sensor.setValidity(true); // bypass the checks fuse_height = true; } diff --git a/EKF/sensor_range_finder.cpp b/EKF/sensor_range_finder.cpp index 5fc8630d6a..b4ea510566 100644 --- a/EKF/sensor_range_finder.cpp +++ b/EKF/sensor_range_finder.cpp @@ -64,9 +64,12 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us) if (isSampleOutOfDate(current_time_us) || !isDataContinuous()) { _is_sample_valid = false; + _is_regularly_sending_data = false; return; } + _is_regularly_sending_data = true; + // Don't run the checks unless we have retrieved new data from the buffer if (_is_sample_ready) { _is_sample_valid = false; diff --git a/EKF/sensor_range_finder.hpp b/EKF/sensor_range_finder.hpp index f4c8bdb625..d0aa23e046 100644 --- a/EKF/sensor_range_finder.hpp +++ b/EKF/sensor_range_finder.hpp @@ -57,6 +57,7 @@ public: void runChecks(uint64_t current_time_us, const matrix::Dcmf &R_to_earth); bool isHealthy() const override { return _is_sample_valid; } bool isDataHealthy() const override { return _is_sample_ready && _is_sample_valid; } + bool isRegularlySendingData() const override { return _is_regularly_sending_data; } void setSample(const rangeSample &sample) { _sample = sample; @@ -115,6 +116,7 @@ private: bool _is_sample_ready{}; ///< true when new range finder data has fallen behind the fusion time horizon and is available to be fused bool _is_sample_valid{}; ///< true if range finder sample retrieved from buffer is valid + bool _is_regularly_sending_data{false}; ///< true if the interval between two samples is less than the maximum expected interval uint64_t _time_last_valid_us{}; ///< time the last range finder measurement was ready (uSec) /* diff --git a/test/test_EKF_fusionLogic.cpp b/test/test_EKF_fusionLogic.cpp index ca7ef3b37e..ad08da3ea8 100644 --- a/test/test_EKF_fusionLogic.cpp +++ b/test/test_EKF_fusionLogic.cpp @@ -337,17 +337,26 @@ TEST_F(EkfFusionLogicTest, doRangeHeightFusion) // WHEN: commanding range height and sending range data _ekf_wrapper.setRangeHeight(); _sensor_simulator.startRangeFinder(); - _sensor_simulator.runSeconds(2.5); - + _sensor_simulator.runSeconds(2.5f); // THEN: EKF should intend to fuse range height EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion()); + const float dt = 8e-3f; + for (int i = 0; i < 5; i++) { + _sensor_simulator.runSeconds(dt); + // THEN: EKF should intend to fuse range height, even if + // there is no new data at each EKF iteration (EKF rate > sensor rate) + EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion()); + } + // WHEN: stop sending range data _sensor_simulator.stopRangeFinder(); - _sensor_simulator.runSeconds(2.5); + _sensor_simulator.runSeconds(5.1); // THEN: EKF should stop to intend to use range height + // and fall back to baro height EXPECT_FALSE(_ekf_wrapper.isIntendingRangeHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); } TEST_F(EkfFusionLogicTest, doVisionHeightFusion)