diff --git a/EKF/control.cpp b/EKF/control.cpp index 85bc12f341..132b230aff 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -947,19 +947,22 @@ void Ekf::controlBetaFusion() void Ekf::controlDragFusion() { - if (_params.fusion_mode & MASK_USE_DRAG && _control_status.flags.in_air) { - if (!_control_status.flags.wind) { - // reset the wind states and covariances when starting drag accel fusion - _control_status.flags.wind = true; - resetWindStates(); - resetWindCovariance(); + if (_params.fusion_mode & MASK_USE_DRAG ) { + if (_control_status.flags.in_air) { + if (!_control_status.flags.wind) { + // reset the wind states and covariances when starting drag accel fusion + _control_status.flags.wind = true; + resetWindStates(); + resetWindCovariance(); - } else if (_drag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_drag_sample_delayed)) { - fuseDrag(); + } else if (_drag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_drag_sample_delayed)) { + fuseDrag(); + + } + } else { + _control_status.flags.wind = false; } - } else { - _control_status.flags.wind = false; } }