From 983234e29de46839b36d80ec61cb0ea0a6f1b9b6 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 15 Jan 2021 11:18:32 +0100 Subject: [PATCH] ekf2: add param to set rng check hysteresis --- src/modules/ekf2/EKF2.cpp | 1 + src/modules/ekf2/EKF2.hpp | 2 ++ src/modules/ekf2/ekf2_params.c | 11 +++++++++++ 3 files changed, 14 insertions(+) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 4958a529b1..e3fd2c7f03 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -117,6 +117,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_rng_a_vmax(_params->max_vel_for_range_aid), _param_ekf2_rng_a_hmax(_params->max_hagl_for_range_aid), _param_ekf2_rng_a_igate(_params->range_aid_innov_gate), + _param_ekf2_rng_qlty_t(_params->range_valid_quality_s), _param_ekf2_evv_gate(_params->ev_vel_innov_gate), _param_ekf2_evp_gate(_params->ev_pos_innov_gate), _param_ekf2_of_n_min(_params->flow_noise), diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index ba9122470d..d1a4b82cfd 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -389,6 +389,8 @@ private: _param_ekf2_rng_a_hmax, ///< maximum allowed absolute altitude (AGL) for range aid (m) (ParamExtFloat) _param_ekf2_rng_a_igate, ///< gate size used for innovation consistency checks for range aid fusion (STD) + (ParamExtFloat) + _param_ekf2_rng_qlty_t, ///< Minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s) // vision estimate fusion (ParamInt) diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index f2b86c3173..eb7a7266bc 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -1097,6 +1097,17 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_A_HMAX, 5.0f); */ PARAM_DEFINE_FLOAT(EKF2_RNG_A_IGATE, 1.0f); +/** + * Minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s) + * + * + * @group EKF2 + * @unit s + * @min 0.1 + * @max 5 +*/ +PARAM_DEFINE_FLOAT(EKF2_RNG_QLTY_T, 1.0f); + /** * Gate size for vision velocity estimate fusion *