From e1d9d503e375681be4d17e6e057adf464de26ad5 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Tue, 23 Aug 2016 20:15:03 +0200 Subject: [PATCH] updated logic of when to fuse beta. Now synthetic sideslip measurements are fused after 5 seconds of cruise flight with airspeed sensor activated --- EKF/control.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index a454bcd0a5..777db2fe93 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -751,8 +751,11 @@ void Ekf::controlRangeFinderFusion() void Ekf::controlAirDataFusion() { // control activation and initialisation/reset of wind states + + bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6; + // wind states are required to do airspeed fusion - if (_time_last_imu - _time_last_arsp_fuse > 10e6 || _time_last_arsp_fuse == 0) { + if ((airspeed_timed_out && !_control_status.flags.fuse_beta) || _time_last_arsp_fuse == 0) { // if the airspeed measurements have timed out for 10 seconds we declare the wind estimate to be invalid _control_status.flags.wind = false; @@ -776,10 +779,15 @@ 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){ + if(beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air && _time_last_imu - _time_last_arsp_fuse > 5e6){ fuseSideslip(); } - _control_status.flags.wind = true; // Look into this + + bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6; + if(sideslip_timed_out && _control_status.flags.fuse_beta){ + _control_status.flags.wind = false; + } + }