diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index bdbf0d5326..b841c63b54 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -49,7 +49,7 @@ #include -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); diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index fa13ee24c5..c25a3d0a34 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/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); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index a992d63480..cad1d19e78 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/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);