diff --git a/EKF/control.cpp b/EKF/control.cpp index c77da70579..07726c7a5d 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -761,8 +761,9 @@ void Ekf::controlAirDataFusion() } - // Always try to fuse airspeed data if available and we are in flight - if (_tas_data_ready && _control_status.flags.in_air) { + // Always try to fuse airspeed data if available and we are in flight and the filter is operating in a normal aiding mode + bool is_aiding = _control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos; + if (_tas_data_ready && _control_status.flags.in_air && is_aiding) { // If starting wind state estimation, reset the wind states and covariances before fusing any data if (!_control_status.flags.wind) { // activate the wind states @@ -793,9 +794,15 @@ void Ekf::controlBetaFusion() } - // Attempt synthetic sideslip fusion at the prescribed rate if we are in-flight and fusion has been enabled + // Perform synthetic sideslip fusion when in-air and sideslip fuson had been enabled externally in addition to the following criteria: + + // Suffient time has lapsed sice the last fusion bool beta_fusion_time_triggered = _time_last_imu - _time_last_beta_fuse > _params.beta_avg_ft_us; - if(beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air){ + + // The filter is operating in a mode where velocity states can be used + bool vel_states_active = _control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos; + + if(beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air && vel_states_active) { // If starting wind state estimation, reset the wind states and covariances before fusing any data if (!_control_status.flags.wind) { // activate the wind states