diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index ca407713815..69b6105d427 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -337,6 +337,10 @@ private: (ParamExtFloat) _baro_noise, ///< observation noise for barometric height fusion (m) (ParamExtFloat) _baro_innov_gate, ///< barometric height innovation consistency gate size (STD) + (ParamInt) + _enable_gnd_effect, ///< Controls barometric deadzone fusion, 0 disables, 1 enables + (ParamExtFloat) + _gnd_effect_deadzone, ///< barometric deadzone range for negative innovations (m) (ParamExtFloat) _posNE_innov_gate, ///< GPS horizontal position innovation consistency gate size (STD) (ParamExtFloat) _vel_innov_gate, ///< GPS velocity innovation consistency gate size (STD) @@ -537,6 +541,7 @@ Ekf2::Ekf2(): _pos_noaid_noise(_params->pos_noaid_noise), _baro_noise(_params->baro_noise), _baro_innov_gate(_params->baro_innov_gate), + _gnd_effect_deadzone(_params->gnd_effect_deadzone), _posNE_innov_gate(_params->posNE_innov_gate), _vel_innov_gate(_params->vel_innov_gate), _tas_innov_gate(_params->tas_innov_gate), @@ -1250,7 +1255,8 @@ void Ekf2::run() if (vehicle_land_detected_updated) { if (orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &vehicle_land_detected) == PX4_OK) { _ekf.set_in_air_status(!vehicle_land_detected.landed); - //_ekf.set_gnd_effect_flag(vehicle_land_detected.in_ground_effect); + if(_enable_gnd_effect.get()) + _ekf.set_gnd_effect_flag(vehicle_land_detected.in_ground_effect); } } diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index f9f46fb0ab2..a008d75e52e 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -538,6 +538,29 @@ PARAM_DEFINE_FLOAT(EKF2_MAG_YAWLIM, 0.25f); */ PARAM_DEFINE_FLOAT(EKF2_BARO_GATE, 5.0f); +/** + * Boolean determining if barometer deadzone near ground should be accounted. + * + * A value of 1 indicates that deadzone is active + * + * @group EKF2 + * @boolean + */ +PARAM_DEFINE_INT32(EKF2_GND_EFF_EN, 0); + +/** + * Baro deadzone range for height fusion + * + * Sets the value of deadzone applied to negative baro innovations when ground effect compensation is active + * + * @group EKF2 + * @min 0.5 + * @max 10.0 + * @unit M + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_GND_EFF_DZ, 5.0f); + /** * Gate size for GPS horizontal position fusion *