fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 03:14:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: move flow buffer pop to controlOpticalFlowFusion

Browse Source
This commit is contained in:
Daniel Agar 2023-02-21 10:05:58 -05:00
parent ed8cef6cf0
commit 0711a34d0e
2 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -174,15 +174,6 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
}
if (_flow_buffer) {
// We don't fuse flow data immediately because we have to wait for the mid integration point to fall behind the fusion time horizon.
// This means we stop looking for new data until the old data has been fused, unless we are not fusing optical flow,
// in this case we need to empty the buffer
if (!_flow_data_ready || (!_control_status.flags.opt_flow && !_hagl_sensor_status.flags.flow)) {
_flow_data_ready = _flow_buffer->pop_first_older_than(imu_delayed.time_us, &_flow_sample_delayed);
}
}
if (_airspeed_buffer) {
_tas_data_ready = _airspeed_buffer->pop_first_older_than(imu_delayed.time_us, &_airspeed_sample_delayed);
}

9
src/modules/ekf2/EKF/optical_flow_control.cpp
View File

@ -40,6 +40,15 @@
void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
{
if (_flow_buffer) {
// We don't fuse flow data immediately because we have to wait for the mid integration point to fall behind the fusion time horizon.
// This means we stop looking for new data until the old data has been fused, unless we are not fusing optical flow,
// in this case we need to empty the buffer
if (!_flow_data_ready || (!_control_status.flags.opt_flow && !_hagl_sensor_status.flags.flow)) {
_flow_data_ready = _flow_buffer->pop_first_older_than(imu_delayed.time_us, &_flow_sample_delayed);
}
}
// Check if on ground motion is un-suitable for use of optical flow
if (!_control_status.flags.in_air) {
updateOnGroundMotionForOpticalFlowChecks();