From b3bb625c710c878ce2f2124b16aa226694b865cf Mon Sep 17 00:00:00 2001
From: Elia Tarasov <elias.tarasov@gmail.com>
Date: Fri, 15 Mar 2019 08:59:44 +0300
Subject: [PATCH] add parameter to set ground effect distance
---
src/modules/ekf2/ekf2_main.cpp | 5 ++++-
src/modules/ekf2/ekf2_params.c | 13 +++++++++++++
2 files changed, 17 insertions(+), 1 deletion(-)
diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index 5d19dd3059..dbdbbbc798 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -339,6 +339,8 @@ private:
_baro_innov_gate, ///< barometric height innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_GND_EFF_DZ>)
_gnd_effect_deadzone, ///< barometric deadzone range for negative innovations (m)
+ (ParamExtFloat<px4::params::EKF2_GND_MAX_HGT>)
+ _gnd_effect_max_hgt, ///< maximum height above the ground level for expected negative baro innovations (m)
(ParamExtFloat<px4::params::EKF2_GPS_P_GATE>)
_posNE_innov_gate, ///< GPS horizontal position innovation consistency gate size (STD)
(ParamExtFloat<px4::params::EKF2_GPS_V_GATE>) _vel_innov_gate, ///< GPS velocity innovation consistency gate size (STD)
@@ -540,6 +542,7 @@ Ekf2::Ekf2():
_baro_noise(_params->baro_noise),
_baro_innov_gate(_params->baro_innov_gate),
_gnd_effect_deadzone(_params->gnd_effect_deadzone),
+ _gnd_effect_max_hgt(_params->gnd_effect_max_hgt),
_posNE_innov_gate(_params->posNE_innov_gate),
_vel_innov_gate(_params->vel_innov_gate),
_tas_innov_gate(_params->tas_innov_gate),
@@ -1394,7 +1397,7 @@ void Ekf2::run()
}
// update ground effect flag based on terrain estimation
- if (lpos.dist_bottom_valid && lpos.dist_bottom < _gnd_effect_deadzone.get()) {
+ if (lpos.dist_bottom_valid && lpos.dist_bottom < _gnd_effect_max_hgt.get()) {
_ekf.set_gnd_effect_flag(true);
}
diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c
index 0022ce8350..cd6bdaa5d2 100644
--- a/src/modules/ekf2/ekf2_params.c
+++ b/src/modules/ekf2/ekf2_params.c
@@ -552,6 +552,19 @@ PARAM_DEFINE_FLOAT(EKF2_BARO_GATE, 5.0f);
*/
PARAM_DEFINE_FLOAT(EKF2_GND_EFF_DZ, 0.0f);
+/**
+ * Height above ground level for ground effect zone
+ *
+ * Sets the maximum distance to the ground level where negative baro innovations are expected.
+ *
+ * @group EKF2
+ * @min 0.0
+ * @max 5.0
+ * @unit M
+ * @decimal 1
+ */
+PARAM_DEFINE_FLOAT(EKF2_GND_MAX_HGT, 0.5f);
+
/**
* Gate size for GPS horizontal position fusion
*
--
GitLab