diff --git a/EKF/control.cpp b/EKF/control.cpp index b5742c3cbf..85bc12f341 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -877,11 +877,10 @@ void Ekf::controlAirDataFusion() { // control activation and initialisation/reset of wind states required for airspeed fusion - // If both airspeed and sideslip fusion have timed out then we no longer have valid wind estimates + // If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6; bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6; if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) { - // if the airspeed or sideslip measurements have timed out for 10 seconds we declare the wind estimate to be invalid _control_status.flags.wind = false; } @@ -911,7 +910,7 @@ void Ekf::controlBetaFusion() { // control activation and initialisation/reset of wind states required for synthetic sideslip fusion fusion - // If both airspeed and sideslip fusion have timed out then we no longer have valid wind estimates + // If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6; bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6; if(_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) {