fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: move airspeed buffer pop to controlAirDataFusion

Browse Source
This commit is contained in:
Daniel Agar 2023-02-21 10:06:57 -05:00
parent 0711a34d0e
commit 241cee2bb7
3 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/ekf2/EKF/airspeed_fusion.cpp
View File

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

6
src/modules/ekf2/EKF/control.cpp
View File

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

2
src/modules/ekf2/EKF/ekf.h
View File

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