From 613a784ce2fae3b7c44f18cc35f9417328893b1e Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 22 Sep 2025 18:18:42 -0800 Subject: [PATCH] change quality check to time based, refactor and clean up --- .../range_finder/range_height_control.cpp | 97 +++++++++++-------- .../range_finder/sensor_range_finder.cpp | 4 +- .../range_finder/sensor_range_finder.hpp | 52 ++++------ src/modules/ekf2/EKF/ekf.h | 4 +- src/modules/ekf2/EKF/estimator_interface.h | 5 +- 5 files changed, 82 insertions(+), 80 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index a641235201..239c415d69 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -46,22 +46,6 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) return; } - // TODO: why isn't this being done anywhere? - _aid_src_rng_hgt.fused = false; - - // Pop rangefinder measurement from buffer of samples into active sample - sensor::rangeSample sample = {}; - if (!_range_buffer->pop_first_older_than(imu_sample.time_us, &sample)) { - if (_range_sensor.timedOut(imu_sample.time_us)) { - stopRangeAltitudeFusion("sensor timed out"); - stopRangeTerrainFusion("sensor timed out"); - } - return; - } - - // Set the raw sample - _range_sensor.setSample(sample); - // TODO: move setting params to init function _range_sensor.setPitchOffset(_params.ekf2_rng_pitch); float cosine_max_tilt = 0.9659; // 10 degrees @@ -69,20 +53,54 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) _rng_consistency_check.setGate(_params.ekf2_rng_k_gate); _range_sensor.updateSensorToEarthRotation(_R_to_earth); - bool quality_ok = sample.quality > 0; + // TODO: why isn't this being done anywhere? + _aid_src_rng_hgt.fused = false; - // While on ground fake a measurement at ground level if the signal quality is bad - if (!quality_ok && !_control_status.flags.in_air) { - sample.quality = 100; - quality_ok = true; - sample.range = _range_sensor.getValidMinVal(); + // Pop rangefinder measurement from buffer of samples into active sample + sensor::rangeSample sample = {}; + + if (!_range_buffer->pop_first_older_than(imu_sample.time_us, &sample)) { + if (_range_sensor.timedOut(imu_sample.time_us)) { + stopRangeAltitudeFusion("sensor timed out"); + stopRangeTerrainFusion("sensor timed out"); + } + + return; } + // Set the raw sample + _range_sensor.setSample(sample); + + // Quality checks + if (sample.quality == 0) { + if (_control_status.flags.in_air) { + // Disable fusion after 1s of bad quality + uint64_t elapsed = sample.time_us - _range_time_last_good_sample; + + if (elapsed > 1'000'000) { + stopRangeAltitudeFusion("sensor quality bad"); + stopRangeTerrainFusion("sensor quality bad"); + } + + return; + + } else { + // While on ground fake a measurement at ground level if the signal quality is bad + sample.quality = 100; + sample.range = _range_sensor.getValidMinVal(); + _range_time_last_good_sample = sample.time_us; + } + + } else { + _range_time_last_good_sample = sample.time_us; + } + + // Tilt and range checks bool tilt_ok = _range_sensor.isTiltOk(); bool range_ok = sample.range <= _range_sensor.getValidMaxVal() && sample.range >= _range_sensor.getValidMinVal(); - // If quality, tilt, or value are outside of bounds -- throw away measurement - if (!quality_ok || !tilt_ok || !range_ok) { + // If tilt or value are outside of bounds -- throw away measurement + if (!tilt_ok || !range_ok) { stopRangeAltitudeFusion("pre checks failed"); stopRangeTerrainFusion("pre checks failed"); return; @@ -125,6 +143,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) // NOTE: terrain is not estimated in this mode if (_params.ekf2_hgt_ref == static_cast(HeightSensor::RANGE)) { fuseRangeAsHeightSource(); + } else { // Fuse Range data as aiding height source (Primary GPS or Baro) fuseRangeAsHeightAiding(); @@ -133,13 +152,8 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) void Ekf::fuseRangeAsHeightAiding() { - // ISSUES: 6/25/2025 - // - optical flow terrain fucks everything up, I've hardcoded disabled it - // - when rng fusion starts again, EKF Z state is corrupted (oscillation) - // - - - const char* kNotKinematicallyConsistentText = "not kinematically consistent"; - const char* kConditionsFailingText = "conditions failing"; + const char *kNotKinematicallyConsistentText = "not kinematically consistent"; + const char *kConditionsFailingText = "conditions failing"; // Stop fusion if rangefinder kinematic consistency fails if (!_control_status.flags.rng_kin_consistent) { @@ -148,7 +162,6 @@ void Ekf::fuseRangeAsHeightAiding() return; } - //// TERRAIN FUSION //// // Fuse Range into Terrain if: @@ -208,6 +221,7 @@ void Ekf::fuseRangeAsHeightAiding() fuseHaglRng(_aid_src_rng_hgt, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain); } +// TODO: remove this mode entirely void Ekf::fuseRangeAsHeightSource() { // When primary height source is RANGE, we always fuse it @@ -280,6 +294,7 @@ bool Ekf::rangeAidConditionsPassed() // Conditions have been valid for at least 1s conditions_passing = true; } + } else { if (_rng_aid_conditions_valid) { @@ -292,6 +307,7 @@ bool Ekf::rangeAidConditionsPassed() if (!is_below_max_speed) { ECL_INFO("!is_below_max_speed"); } + if (!is_in_range) { ECL_INFO("!is_in_range"); } @@ -345,12 +361,12 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src) const float innov_gate = math::max(_params.ekf2_rng_gate, 1.f); updateAidSourceStatus(aid_src, - _range_sensor.sample()->time_us, // sample timestamp - measurement, // observation - measurement_variance, // observation variance - getHagl() - measurement, // innovation - innovation_variance, // innovation variance - innov_gate); // innovation gate + _range_sensor.sample()->time_us, // sample timestamp + measurement, // observation + measurement_variance, // observation variance + getHagl() - measurement, // innovation + innovation_variance, // innovation variance + innov_gate); // innovation gate // z special case if there is bad vertical acceleration data, then don't reject measurement, // but limit innovation to prevent spikes that could destabilise the filter @@ -368,18 +384,19 @@ float Ekf::getRngVar() const return dist_var; } -void Ekf::stopRangeAltitudeFusion(const char* reason) +void Ekf::stopRangeAltitudeFusion(const char *reason) { if (_control_status.flags.rng_hgt) { ECL_INFO("STOP RNG Altitude fusion: %s", reason); _control_status.flags.rng_hgt = false; + if (_height_sensor_ref == HeightSensor::RANGE) { _height_sensor_ref = HeightSensor::UNKNOWN; } } } -void Ekf::stopRangeTerrainFusion(const char* reason) +void Ekf::stopRangeTerrainFusion(const char *reason) { if (_control_status.flags.rng_terrain) { ECL_INFO("STOP RNG Terrain fusion: %s", reason); 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 8dbd0f444b..dc072b19ce 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 @@ -73,8 +73,8 @@ void SensorRangeFinder::setPitchOffset(float new_pitch_offset) void SensorRangeFinder::setLimits(float min_distance, float max_distance) { - _rng_valid_min_val = min_distance; - _rng_valid_max_val = max_distance; + _min_distance = min_distance; + _max_distance = max_distance; } void SensorRangeFinder::updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth) 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 15eaf2c524..abae5760dc 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 @@ -50,12 +50,11 @@ namespace sensor { struct rangeSample { - uint64_t time_us{}; ///< timestamp of the measurement (uSec) - float range{}; ///< range (distance to ground) measurement (m) - int8_t quality{}; ///< Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality. + uint64_t time_us{}; // timestamp of the measurement (uSec) + float range{}; // range (distance to ground) measurement (m) + int8_t quality{}; // Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality. }; - class SensorRangeFinder { public: @@ -76,35 +75,25 @@ public: float range_cos_max_tilt{0.7071f}; // 45 degrees max tilt }; - void updateParameters(Parameters& parameters) { _parameters = parameters; }; - - bool timedOut(uint64_t time_now) const; - void setSample(const rangeSample &sample); - - // This is required because of the ring buffer // TODO: move the ring buffer here rangeSample *sample() { return &_sample; } + void setSample(const rangeSample &sample); void setPitchOffset(float new_pitch_offset); - void setCosMaxTilt(float cos_max_tilt) { _range_cos_max_tilt = cos_max_tilt; } - - float getCosTilt() const { return _cos_tilt_rng_to_earth; } - void setLimits(float min_distance, float max_distance); - // void setRange(float rng) { _sample.rng = rng; } - // float getRange() const { return _sample.rng; } - + float getCosTilt() const { return _cos_tilt_rng_to_earth; } float getDistBottom() const { return _sample.range * _cos_tilt_rng_to_earth; } + float getValidMinVal() const { return _min_distance; } + float getValidMaxVal() const { return _max_distance; } - float getValidMinVal() const { return _rng_valid_min_val; } - float getValidMaxVal() const { return _rng_valid_max_val; } - + void updateParameters(Parameters ¶meters) { _parameters = parameters; }; void updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth); bool isTiltOk() const { return _cos_tilt_rng_to_earth > _range_cos_max_tilt; } + bool timedOut(uint64_t time_now) const; private: @@ -112,21 +101,16 @@ private: Parameters _parameters{}; - /* - * Tilt check - */ - float _cos_tilt_rng_to_earth{1.f}; ///< 2,2 element of the rotation matrix from sensor frame to earth frame - float _range_cos_max_tilt{0.7071f}; ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data - float _pitch_offset_rad{3.14f}; ///< range finder tilt rotation about the Y body axis - float _sin_pitch_offset{0.0f}; ///< sine of the range finder tilt rotation about the Y body axis - float _cos_pitch_offset{-1.0f}; ///< cosine of the range finder tilt rotation about the Y body axis - - /* - * Range check - */ - float _rng_valid_min_val{}; ///< minimum distance that the rangefinder can measure (m) - float _rng_valid_max_val{}; ///< maximum distance that the rangefinder can measure (m) + // Tilt check + float _cos_tilt_rng_to_earth{1.f}; // 2,2 element of the rotation matrix from sensor frame to earth frame + float _range_cos_max_tilt{0.7071f}; // cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data + float _pitch_offset_rad{3.14f}; // range finder tilt rotation about the Y body axis + float _sin_pitch_offset{0.0f}; // sine of the range finder tilt rotation about the Y body axis + float _cos_pitch_offset{-1.0f}; // cosine of the range finder tilt rotation about the Y body axis + // Range check + float _min_distance{}; // minimum distance that the rangefinder can measure (m) + float _max_distance{}; // maximum distance that the rangefinder can measure (m) }; } // namespace sensor diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 1067b6c759..ed873749a7 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -783,8 +783,8 @@ private: void fuseRangeAsHeightAiding(); bool isConditionalRangeAidSuitable(); bool rangeAidConditionsPassed(); - void stopRangeAltitudeFusion(const char* reason); - void stopRangeTerrainFusion(const char* reason); + void stopRangeAltitudeFusion(const char *reason); + void stopRangeTerrainFusion(const char *reason); #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 68810a536c..780bdb2378 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -376,8 +376,9 @@ protected: uint64_t _time_last_range_buffer_push{0}; // Range aiding - bool _rng_aid_conditions_valid{false}; - uint64_t _time_rng_aid_conditions_valid{0}; + bool _rng_aid_conditions_valid{}; + uint64_t _time_rng_aid_conditions_valid{}; + uint64_t _range_time_last_good_sample{}; sensor::SensorRangeFinder _range_sensor{}; RangeFinderConsistencyCheck _rng_consistency_check;