diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 2f2249312a..b8d6a6279a 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -1047,18 +1047,21 @@ void Ekf::controlBetaFusion() void Ekf::controlDragFusion() { - if ((_params.fusion_mode & MASK_USE_DRAG) && - !_using_synthetic_position && - _control_status.flags.in_air && - !_mag_inhibit_yaw_reset_req) { + if ((_params.fusion_mode & MASK_USE_DRAG) && _drag_buffer && + !_using_synthetic_position && _control_status.flags.in_air && !_mag_inhibit_yaw_reset_req) { if (!_control_status.flags.wind) { // reset the wind states and covariances when starting drag accel fusion _control_status.flags.wind = true; resetWind(); - } else if (_drag_buffer && _drag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_drag_sample_delayed)) { - fuseDrag(); + } + + + dragSample drag_sample; + + if (_drag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &drag_sample)) { + fuseDrag(drag_sample); } } } diff --git a/src/modules/ekf2/EKF/drag_fusion.cpp b/src/modules/ekf2/EKF/drag_fusion.cpp index 6435faa69b..47cb2d7bbb 100644 --- a/src/modules/ekf2/EKF/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/drag_fusion.cpp @@ -44,7 +44,7 @@ #include -void Ekf::fuseDrag() +void Ekf::fuseDrag(const dragSample &drag_sample) { SparseVector24f<0,1,2,3,4,5,6,22,23> Hfusion; // Observation Jacobians Vector24f Kfusion; // Kalman gain vector @@ -89,7 +89,7 @@ void Ekf::fuseDrag() // perform sequential fusion of XY specific forces for (uint8_t axis_index = 0; axis_index < 2; axis_index++) { // measured drag acceleration corrected for sensor bias - const float mea_acc = _drag_sample_delayed.accelXY(axis_index) - _state.delta_vel_bias(axis_index) / _dt_ekf_avg; + const float mea_acc = drag_sample.accelXY(axis_index) - _state.delta_vel_bias(axis_index) / _dt_ekf_avg; // predicted drag force sign is opposite to predicted wind relative velocity const float drag_sign = (rel_wind_body(axis_index) >= 0.f) ? -1.f : 1.f; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 09ec1936fc..42bd0a2798 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -622,7 +622,7 @@ private: void fuseSideslip(); // fuse body frame drag specific forces for multi-rotor wind estimation - void fuseDrag(); + void fuseDrag(const dragSample &drag_sample); void fuseBaroHgt(); void fuseGpsHgt(); diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index aa7faac64f..c1cfb58118 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -295,7 +295,6 @@ protected: flowSample _flow_sample_delayed{}; extVisionSample _ev_sample_delayed{}; extVisionSample _ev_sample_delayed_prev{}; - dragSample _drag_sample_delayed{}; dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate) auxVelSample _auxvel_sample_delayed{};