diff --git a/EKF/common.h b/EKF/common.h index c36ac6d2163..e5dd0e3e072 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -298,6 +298,7 @@ struct parameters { float range_aid_innov_gate{1.0f}; ///< gate size used for innovation consistency checks for range aid fusion 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 range_stuck_threshold{0.1f}; ///< minimum variation in range finder reading required to declare a range finder 'unstuck' when readings recommence after being out of range (m) + int32_t range_signal_hysteresis_ms{1000}; ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (ms) // vision position fusion float ev_vel_innov_gate{3.0f}; ///< vision velocity fusion innovation consistency gate size (STD) diff --git a/EKF/range_finder_checks.cpp b/EKF/range_finder_checks.cpp index a7eaeb1554e..17dd693a5e0 100644 --- a/EKF/range_finder_checks.cpp +++ b/EKF/range_finder_checks.cpp @@ -80,7 +80,7 @@ void Ekf::updateRangeDataValidity() if (_range_sample_delayed.quality == 0) { _time_bad_rng_signal_quality = _imu_sample_delayed.time_us; _rng_hgt_valid = false; - } else if (_imu_sample_delayed.time_us - _time_bad_rng_signal_quality > RNG_BAD_SIG_HYST) { + } else if (_imu_sample_delayed.time_us - _time_bad_rng_signal_quality > (unsigned)_params.range_signal_hysteresis_ms) { _rng_hgt_valid = true; } @@ -139,4 +139,4 @@ void Ekf::updateRangeDataStuck() } else { _time_last_rng_ready = _range_sample_delayed.time_us; } -} \ No newline at end of file +}