From 33495e81437113bf301ff6cb5f1d1e5cf50dca0b Mon Sep 17 00:00:00 2001 From: Roman Date: Thu, 20 Apr 2017 08:34:40 +0200 Subject: [PATCH] ekf2: added parameters to specify horizontal speed and absolute altitude thresholds for range aid mode Signed-off-by: Roman --- src/modules/ekf2/ekf2_main.cpp | 4 ++++ src/modules/ekf2/ekf2_params.c | 24 ++++++++++++++++++++++++ 2 files changed, 28 insertions(+) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 088ee7f0de..70743006cb 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -265,6 +265,8 @@ private: control::BlockParamExtFloat _rng_pitch_offset; // range sensor pitch offset (rad) control::BlockParamExtInt _rng_aid; // enables use of a range finder even if primary height source is not range finder (EKF2_HGT_MODE != 2) + control::BlockParamExtFloat _rng_aid_hor_vel_max; // maximum allowed horizontal velocity for range aid + control::BlockParamExtFloat _rng_aid_height_max; // maximum allowed absolute altitude (AGL) for range aid // vision estimate fusion control::BlockParamExtFloat _ev_pos_noise; // default position observation noise for exernal vision measurements (m) @@ -396,6 +398,8 @@ Ekf2::Ekf2(): _rng_gnd_clearance(this, "EKF2_MIN_RNG", false, _params->rng_gnd_clearance), _rng_pitch_offset(this, "EKF2_RNG_PITCH", false, _params->rng_sens_pitch), _rng_aid(this, "EKF2_RNG_AID", false, _params->range_aid), + _rng_aid_hor_vel_max(this, "EKF2_RNG_A_VMAX", false, _params->max_vel_for_range_aid), + _rng_aid_height_max(this, "EKF2_RNG_A_HMAX", false, _params->max_hagl_for_range_aid), _ev_pos_noise(this, "EKF2_EVP_NOISE", false, _default_ev_pos_noise), _ev_ang_noise(this, "EKF2_EVA_NOISE", false, _default_ev_ang_noise), _ev_innov_gate(this, "EKF2_EV_GATE", false, _params->ev_innov_gate), diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 4b193c0e9e..6e93a93a66 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -1007,3 +1007,27 @@ PARAM_DEFINE_FLOAT(EKF2_MAGB_K, 0.2f); * @value 1 Range aid enabled */ PARAM_DEFINE_INT32(EKF2_RNG_AID, 0.0f); + +/** + * Maximum horizontal velocity allowed for range aid mode. + * + * If the vehicle horizontal speed exceeds this value then the estimator will not fuse range measurements + * to estimate it's height. This only applies when range aid mode is activated (EKF2_RNG_AID = enabled). + * + * @group EKF2 + * @min 0.1 + * @max 2 + */ +PARAM_DEFINE_FLOAT(EKF2_RNG_A_VMAX, 1.0f); + +/** + * Maximum absolute altitude (height above ground level) allowed for range aid mode. + * + * If the vehicle absolute altitude exceeds this value then the estimator will not fuse range measurements + * to estimate it's height. This only applies when range aid mode is activated (EKF2_RNG_AID = enabled). + * + * @group EKF2 + * @min 1.0 + * @max 10.0 + */ +PARAM_DEFINE_FLOAT(EKF2_RNG_A_HMAX, 5.0f);