diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 3f29fbbb79..b94be0610f 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -218,37 +218,6 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) updateDeadReckoningStatus(); } -void Ekf::controlBetaFusion(const imuSample &imu_delayed) -{ - _control_status.flags.fuse_beta = _params.beta_fusion_enabled && _control_status.flags.fixed_wing - && _control_status.flags.in_air && !_control_status.flags.fake_pos; - - if (_control_status.flags.fuse_beta) { - - // Perform synthetic sideslip fusion at regular intervals when in-air and sideslip fusion had been enabled externally: - const bool beta_fusion_time_triggered = isTimedOut(_aid_src_sideslip.time_last_fuse, _params.beta_avg_ft_us); - - if (beta_fusion_time_triggered) { - - updateSideslip(_aid_src_sideslip); - _innov_check_fail_status.flags.reject_sideslip = _aid_src_sideslip.innovation_rejected; - - // If starting wind state estimation, reset the wind states and covariances before fusing any data - if (!_control_status.flags.wind) { - // activate the wind states - _control_status.flags.wind = true; - // reset the timeout timers to prevent repeated resets - _aid_src_sideslip.time_last_fuse = imu_delayed.time_us; - resetWind(); - } - - if (Vector2f(Vector2f(_state.vel) - _state.wind_vel).longerThan(7.f)) { - fuseSideslip(_aid_src_sideslip); - } - } - } -} - void Ekf::controlDragFusion() { if ((_params.fusion_mode & SensorFusionMask::USE_DRAG) && _drag_buffer && diff --git a/src/modules/ekf2/EKF/sideslip_fusion.cpp b/src/modules/ekf2/EKF/sideslip_fusion.cpp index 9125b7b038..60f08299c1 100644 --- a/src/modules/ekf2/EKF/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/sideslip_fusion.cpp @@ -47,6 +47,37 @@ #include +void Ekf::controlBetaFusion(const imuSample &imu_delayed) +{ + _control_status.flags.fuse_beta = _params.beta_fusion_enabled && _control_status.flags.fixed_wing + && _control_status.flags.in_air && !_control_status.flags.fake_pos; + + if (_control_status.flags.fuse_beta) { + + // Perform synthetic sideslip fusion at regular intervals when in-air and sideslip fusion had been enabled externally: + const bool beta_fusion_time_triggered = isTimedOut(_aid_src_sideslip.time_last_fuse, _params.beta_avg_ft_us); + + if (beta_fusion_time_triggered) { + + updateSideslip(_aid_src_sideslip); + _innov_check_fail_status.flags.reject_sideslip = _aid_src_sideslip.innovation_rejected; + + // If starting wind state estimation, reset the wind states and covariances before fusing any data + if (!_control_status.flags.wind) { + // activate the wind states + _control_status.flags.wind = true; + // reset the timeout timers to prevent repeated resets + _aid_src_sideslip.time_last_fuse = imu_delayed.time_us; + resetWind(); + } + + if (Vector2f(Vector2f(_state.vel) - _state.wind_vel).longerThan(7.f)) { + fuseSideslip(_aid_src_sideslip); + } + } + } +} + void Ekf::updateSideslip(estimator_aid_source1d_s &sideslip) const { // reset flags