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

ekf2_terr: refactor terrain estimator - flow aiding

Browse Source
This commit is contained in:
bresch
2022-02-08 16:28:42 +01:00
committed by Daniel Agar
parent 33fd1849e0
commit 2fb7b35a8b
3 changed files with 66 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
+1 -12
View File
@@ -134,19 +134,9 @@ void Ekf::controlFusionModes()
// 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) {
if (!_flow_data_ready || (!_control_status.flags.opt_flow && !_hagl_sensor_status.flags.flow)) {
_flow_data_ready = _flow_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed);
}
// check if we should fuse flow data for terrain estimation
if (!_flow_for_terrain_data_ready && _flow_data_ready && _control_status.flags.in_air) {
// TODO: WARNING, _flow_data_ready can be modified in controlOpticalFlowFusion
// due to some checks failing
// only fuse flow for terrain if range data hasn't been fused for 5 seconds
_flow_for_terrain_data_ready = isTimedOut(_time_last_hagl_fuse, (uint64_t)5E6);
// only fuse flow for terrain if the main filter is not fusing flow and we are using gps
_flow_for_terrain_data_ready &= (!_control_status.flags.opt_flow && _control_status.flags.gps);
}
}
if (_ext_vision_buffer) {
@@ -420,7 +410,6 @@ void Ekf::controlOpticalFlowFusion()
} else {
// don't use this flow data and wait for the next data to arrive
_flow_data_ready = false;
_flow_for_terrain_data_ready = false; // TODO: find a better place
}
}