ekf2: added parameters to specify horizontal speed and absolute altitude

thresholds for range aid mode

Signed-off-by: Roman <bapstroman@gmail.com>

src/modules/ekf2/ekf2_main.cpp

@@ -238,6 +238,8 @@ class Ekf2 : public control::SuperBlock
 	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)
@@ -359,6 +361,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),

src/modules/ekf2/ekf2_params.c

@@ -919,3 +919,27 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_PITCH, 0.0f);
  * @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);