From 03e11c4d1815e1b11912a0c0b846d3b3cf6319ad Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Wed, 19 Jul 2017 10:25:12 +0200 Subject: [PATCH] update ecl and add param for innovation consistency checks for range aid fusion (#7585) --- src/lib/ecl | 2 +- src/modules/ekf2/ekf2_main.cpp | 6 +++++- src/modules/ekf2/ekf2_params.c | 13 +++++++++++++ 3 files changed, 19 insertions(+), 2 deletions(-) diff --git a/src/lib/ecl b/src/lib/ecl index 18e81eba31..9b6e1ab2a7 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 18e81eba31d84c700ce619402536bbd0fad0132a +Subproject commit 9b6e1ab2a7bf1787842ea9f0acb4008363b5af06 diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index d865949ba2..e901d305cb 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -256,6 +256,8 @@ private: _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 + control::BlockParamExtFloat + _rng_aid_innov_gate; // gate size used for innovation consistency checks for range aid fusion // vision estimate fusion control::BlockParamExtFloat _ev_pos_noise; // default position observation noise for exernal vision measurements (m) @@ -388,6 +390,7 @@ Ekf2::Ekf2(): _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), + _rng_aid_innov_gate(this, "EKF2_RNG_A_IGATE", false, _params->range_aid_innov_gate), _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), @@ -933,7 +936,8 @@ void Ekf2::run() lpos.yaw = euler.psi(); float terrain_vpos; - lpos.dist_bottom_valid = _ekf.get_terrain_vert_pos(&terrain_vpos); + lpos.dist_bottom_valid = _ekf.get_terrain_valid(); + _ekf.get_terrain_vert_pos(&terrain_vpos); lpos.dist_bottom = terrain_vpos - pos[2]; // Distance to bottom surface (ground) in meters lpos.dist_bottom_rate = -velocity[2]; // Distance to bottom surface (ground) change rate lpos.surface_bottom_timestamp = now; // Time when new bottom surface found diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index b5919bc1ad..637cd5b0fc 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -1043,3 +1043,16 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_A_VMAX, 1.0f); * @max 10.0 */ PARAM_DEFINE_FLOAT(EKF2_RNG_A_HMAX, 5.0f); + +/** + * Gate size used for innovation consistency checks for range aid fusion + * + * A lower value means HAGL needs to be more stable in order to use range finder for height estimation + * in range aid mode + * + * @group EKF2 + * @unit SD + * @min 0.1 + * @max 5.0 + */ +PARAM_DEFINE_FLOAT(EKF2_RNG_A_IGATE, 1.0f);