From a9182259a55636d2b257b8fe57744c99c2431ec9 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Tue, 23 Aug 2016 20:58:26 +0200 Subject: [PATCH] fixed bug in beta fusion --- EKF/control.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 777db2fe93..e319bfe5de 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -778,13 +778,12 @@ void Ekf::controlAirDataFusion() void Ekf::controlBetaFusion() { 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 && _time_last_imu - _time_last_arsp_fuse > 5e6){ + if(beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air && _time_last_arsp_fuse > 5e6){ fuseSideslip(); } bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6; - if(sideslip_timed_out && _control_status.flags.fuse_beta){ + if(sideslip_timed_out && _control_status.flags.fuse_beta && _time_last_beta_fuse > 0.0f){ _control_status.flags.wind = false; }