diff --git a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp index b7b45d5ced..e8cbe0e213 100644 --- a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp @@ -85,20 +85,20 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) measurement_var + bias_est.getBiasVar(), // observation variance math::max(_params.ekf2_baro_gate, 1.f)); // innovation gate - // Compensate for positive static pressure transients (negative vertical position innovations) - // caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations. + // Compensate for static pressure transients caused by rotor wash ground interaction + // by applying a temporary deadzone to baro innovations. + // The direction of the pressure transient depends on the baro location on the airframe, + // so the deadzone is applied symmetrically to both positive and negative innovations. if (_control_status.flags.gnd_effect && (_params.ekf2_gnd_eff_dz > 0.f)) { - const float deadzone_start = 0.0f; - const float deadzone_end = deadzone_start + _params.ekf2_gnd_eff_dz; + if (aid_src.innovation > _params.ekf2_gnd_eff_dz) { + aid_src.innovation -= _params.ekf2_gnd_eff_dz; - if (aid_src.innovation < -deadzone_start) { - if (aid_src.innovation <= -deadzone_end) { - aid_src.innovation += deadzone_end; + } else if (aid_src.innovation < -_params.ekf2_gnd_eff_dz) { + aid_src.innovation += _params.ekf2_gnd_eff_dz; - } else { - aid_src.innovation = -deadzone_start; - } + } else { + aid_src.innovation = 0.f; } } diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 982cb4c1af..f60f00cd7b 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -311,7 +311,7 @@ struct parameters { float baro_bias_nsd{0.13f}; ///< process noise for barometric height bias estimation (m/s/sqrt(Hz)) float ekf2_baro_gate{5.0f}; ///< barometric and GPS height innovation consistency gate size (STD) - float ekf2_gnd_eff_dz{5.0f}; ///< Size of deadzone applied to negative baro innovations when ground effect compensation is active (m) + float ekf2_gnd_eff_dz{5.0f}; ///< Size of deadzone applied to baro innovations when ground effect compensation is active (m) float ekf2_gnd_max_hgt{0.5f}; ///< Height above ground at which baro ground effect becomes insignificant (m) # if defined(CONFIG_EKF2_BARO_COMPENSATION) @@ -553,7 +553,7 @@ uint64_t mag_fault : 1; ///< 18 - true when the magnetometer has been declared faulty and is no longer being used uint64_t fuse_aspd : 1; ///< 19 - true when airspeed measurements are being fused uint64_t gnd_effect : - 1; ///< 20 - true when protection from ground effect induced static pressure rise is active + 1; ///< 20 - true when protection from ground effect induced static pressure transients is active uint64_t rng_stuck : 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough uint64_t gnss_yaw : diff --git a/src/modules/ekf2/params_barometer.yaml b/src/modules/ekf2/params_barometer.yaml index cefa503039..85a4c25049 100644 --- a/src/modules/ekf2/params_barometer.yaml +++ b/src/modules/ekf2/params_barometer.yaml @@ -42,7 +42,7 @@ parameters: EKF2_GND_EFF_DZ: description: short: Baro deadzone range for height fusion - long: Sets the value of deadzone applied to negative baro innovations. Deadzone + long: Sets the value of deadzone applied to baro innovations. Deadzone is enabled when EKF2_GND_EFF_DZ > 0. type: float default: 4.0 @@ -53,8 +53,8 @@ parameters: EKF2_GND_MAX_HGT: description: short: Height above ground level for ground effect zone - long: Sets the maximum distance to the ground level where negative baro innovations - are expected. + long: Sets the maximum distance to the ground level where baro ground effect + is expected. type: float default: 0.5 min: 0.0