diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index b94be0610f..f9e8af441e 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -218,27 +218,6 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) updateDeadReckoningStatus(); } -void Ekf::controlDragFusion() -{ - if ((_params.fusion_mode & SensorFusionMask::USE_DRAG) && _drag_buffer && - !_control_status.flags.fake_pos && _control_status.flags.in_air) { - - if (!_control_status.flags.wind) { - // reset the wind states and covariances when starting drag accel fusion - _control_status.flags.wind = true; - resetWind(); - - } - - - dragSample drag_sample; - - if (_drag_buffer->pop_first_older_than(_time_delayed_us, &drag_sample)) { - fuseDrag(drag_sample); - } - } -} - void Ekf::controlAuxVelFusion() { if (_auxvel_buffer) { diff --git a/src/modules/ekf2/EKF/drag_fusion.cpp b/src/modules/ekf2/EKF/drag_fusion.cpp index 1ef10ffdb2..5e5675f16f 100644 --- a/src/modules/ekf2/EKF/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/drag_fusion.cpp @@ -42,6 +42,27 @@ #include +void Ekf::controlDragFusion() +{ + if ((_params.fusion_mode & SensorFusionMask::USE_DRAG) && _drag_buffer && + !_control_status.flags.fake_pos && _control_status.flags.in_air) { + + if (!_control_status.flags.wind) { + // reset the wind states and covariances when starting drag accel fusion + _control_status.flags.wind = true; + resetWind(); + + } + + + dragSample drag_sample; + + if (_drag_buffer->pop_first_older_than(_time_delayed_us, &drag_sample)) { + fuseDrag(drag_sample); + } + } +} + void Ekf::fuseDrag(const dragSample &drag_sample) { const float R_ACC = fmaxf(_params.drag_noise, 0.5f); // observation noise variance in specific force drag (m/sec**2)**2