|
|
@ -877,11 +877,10 @@ void Ekf::controlAirDataFusion() |
|
|
|
{ |
|
|
|
{ |
|
|
|
// control activation and initialisation/reset of wind states required for airspeed fusion
|
|
|
|
// 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 airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6; |
|
|
|
bool sideslip_timed_out = _time_last_imu - _time_last_beta_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 (_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; |
|
|
|
_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
|
|
|
|
// 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 sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6; |
|
|
|
bool airspeed_timed_out = _time_last_imu - _time_last_arsp_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)) { |
|
|
|
if(_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) { |
|
|
|