ekf2: move airspeed buffer pop to controlAirDataFusion

src/modules/ekf2/EKF/airspeed_fusion.cpp

@@ -49,7 +49,7 @@
 
 #include <mathlib/mathlib.h>
 
-void Ekf::controlAirDataFusion()
+void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 {
 	// control activation and initialisation/reset of wind states required for airspeed fusion
 
@@ -66,6 +66,10 @@ void Ekf::controlAirDataFusion()
 		return;
 	}
 
+	if (_airspeed_buffer) {
+		_tas_data_ready = _airspeed_buffer->pop_first_older_than(imu_delayed.time_us, &_airspeed_sample_delayed);
+	}
+
 	if (_tas_data_ready) {
 		updateAirspeed(_airspeed_sample_delayed, _aid_src_airspeed);
 

src/modules/ekf2/EKF/control.cpp

@@ -174,10 +174,6 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 		_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
 	}
 
-	if (_airspeed_buffer) {
-		_tas_data_ready = _airspeed_buffer->pop_first_older_than(imu_delayed.time_us, &_airspeed_sample_delayed);
-	}
-
 	// run EKF-GSF yaw estimator once per imu_delayed update after all main EKF data samples available
 	runYawEKFGSF(imu_delayed);
 
@@ -185,7 +181,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 	controlMagFusion();
 	controlOpticalFlowFusion(imu_delayed);
 	controlGpsFusion();
-	controlAirDataFusion();
+	controlAirDataFusion(imu_delayed);
 	controlBetaFusion(imu_delayed);
 	controlDragFusion();
 	controlHeightFusion(imu_delayed);

src/modules/ekf2/EKF/ekf.h

@@ -887,7 +887,7 @@ private:
 	void run3DMagAndDeclFusions(const Vector3f &mag);
 
 	// control fusion of air data observations
-	void controlAirDataFusion();
+	void controlAirDataFusion(const imuSample &imu_delayed);
 
 	// control fusion of synthetic sideslip observations
 	void controlBetaFusion(const imuSample &imu_delayed);