From c4d162f9a086fa3357fdea2a8634acd525b64aab Mon Sep 17 00:00:00 2001 From: Claudio Micheli Date: Wed, 4 Nov 2020 18:35:39 +0100 Subject: [PATCH] EKF: range_finder parameterize range sensor quality hysteresis time Signed-off-by: Claudio Micheli Co-authored-by: bresch --- EKF/common.h | 1 + EKF/control.cpp | 4 +++- EKF/ekf.cpp | 1 + EKF/sensor_range_finder.hpp | 6 +++++- 4 files changed, 10 insertions(+), 2 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index d0a0d5bc4e..b396f332e0 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -291,6 +291,7 @@ struct parameters { float max_vel_for_range_aid{1.0f}; ///< maximum ground velocity for which we allow to use the range finder as height source (if range_aid == 1) int32_t range_aid{0}; ///< allow switching primary height source to range finder if certain conditions are met float range_aid_innov_gate{1.0f}; ///< gate size used for innovation consistency checks for range aid fusion + float range_valid_quality_s{1.0f}; ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s) 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 // vision position fusion diff --git a/EKF/control.cpp b/EKF/control.cpp index 265cccf556..5a267fa632 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -124,11 +124,13 @@ void Ekf::controlFusionModes() // Get range data from buffer and check validity const bool is_rng_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress()); _range_sensor.setDataReadiness(is_rng_data_ready); - _range_sensor.runChecks(_imu_sample_delayed.time_us, _R_to_earth); // update range sensor angle parameters in case they have changed _range_sensor.setPitchOffset(_params.rng_sens_pitch); _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); + _range_sensor.setQualityHysteresis(_params.range_valid_quality_s); + + _range_sensor.runChecks(_imu_sample_delayed.time_us, _R_to_earth); } if (_range_sensor.isDataHealthy()) { diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 7a51552217..50444868dc 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -80,6 +80,7 @@ void Ekf::reset() _terrain_initialised = false; _range_sensor.setPitchOffset(_params.rng_sens_pitch); _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); + _range_sensor.setQualityHysteresis(_params.range_valid_quality_s); _control_status.value = 0; _control_status_prev.value = 0; diff --git a/EKF/sensor_range_finder.hpp b/EKF/sensor_range_finder.hpp index 4d226a2da9..f4c8bdb625 100644 --- a/EKF/sensor_range_finder.hpp +++ b/EKF/sensor_range_finder.hpp @@ -83,6 +83,10 @@ public: _rng_valid_max_val = max_distance; } + void setQualityHysteresis(float valid_quality_threshold_s){ + _quality_hyst_us = uint64_t(valid_quality_threshold_s * 1e6f); + } + float getCosTilt() const { return _cos_tilt_rng_to_earth; } void setRange(float rng) { _sample.rng = rng; } @@ -146,7 +150,7 @@ private: * Quality check */ uint64_t _time_bad_quality_us{}; ///< timestamp at which range finder signal quality was 0 (used for hysteresis) - uint64_t _quality_hyst_us{1000000}; ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (us) + 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) }; } // namespace sensor