diff --git a/EKF/common.h b/EKF/common.h index 7a749898bc..d4736361ef 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -283,6 +283,7 @@ struct parameters { int32_t range_aid{0}; ///< allow switching primary height source to range finder if certian conditions are met 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) // vision position fusion float ev_innov_gate{5.0f}; ///< vision estimator fusion innovation consistency gate size (STD) diff --git a/EKF/control.cpp b/EKF/control.cpp index b7c8c41fda..e0baca1956 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1184,7 +1184,7 @@ void Ekf::checkRangeDataValidity() _control_status.flags.in_air) { // require a variance of rangefinder values to check for "stuck" measurements - if (_rng_stuck_max_val - _rng_stuck_min_val > 1.0f) { + if (_rng_stuck_max_val - _rng_stuck_min_val > _params.range_stuck_threshold) { _time_last_rng_ready = _range_sample_delayed.time_us; _rng_stuck_min_val = 0.0f; _rng_stuck_max_val = 0.0f;