ekf2: don't store _drag_sample_delayed

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);
 		}
 	}
 }

src/modules/ekf2/EKF/drag_fusion.cpp

@@ -44,7 +44,7 @@
 
 #include <mathlib/mathlib.h>
 
-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;

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();

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{};