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

EKF: Rework optical flow selection logic

Browse Source 
Fixes a race condition caused by the _is_dead_reckoning test.
Only checks flow selection when flow is available.
This commit is contained in:
Paul Riseborough
2018-06-09 22:23:55 +02:00
committed by Lorenz Meier
parent e383b6a272
commit ee2dc7d790
1 changed files with 44 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+8 -9
View File
@@ -357,6 +357,8 @@ void Ekf::controlOpticalFlowFusion()
_time_good_motion_us = _imu_sample_delayed.time_us;
}
// New optical flow data has fallen behind the fusion time horizon and is ready to be fused
if (_flow_data_ready) {
// Inhibit flow use if motion is un-suitable or we have good quality GPS
// Apply hysteresis to prevent rapid mode switching
float gps_err_norm_lim;
@@ -366,20 +368,20 @@ void Ekf::controlOpticalFlowFusion()
gps_err_norm_lim = 1.0f;
}
// If we are in-air and doing inertial dead reckoning or are using bad GPS, then optical flow use
// is necessary to control position drift
// Check if we are in-air and require optical flow to control position drift
bool flow_required = _control_status.flags.in_air &&
(_is_dead_reckoning || (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim)));
(_is_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
|| (_control_status.flags.opt_flow && !_control_status.flags.gps && !_control_status.flags.ev_pos) // is completely reliant on optical flow
|| (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad
if (!_inhibit_flow_use) {
if (!_inhibit_flow_use && _control_status.flags.opt_flow) {
// inhibit use of optical flow if motion is unsuitable and we are not reliant on it for flight navigation
bool preflight_motion_not_ok = !_control_status.flags.in_air && ((_imu_sample_delayed.time_us - _time_good_motion_us) > (uint64_t)1E5);
bool flight_motion_not_ok = _control_status.flags.in_air && !_range_aid_mode_enabled;
if ((preflight_motion_not_ok || flight_motion_not_ok) && !flow_required) {
_inhibit_flow_use = true;
}
} else {
} else if (_inhibit_flow_use && !_control_status.flags.opt_flow){
// allow use of optical flow if motion is suitable or we are reliant on it for flight navigation
bool preflight_motion_ok = !_control_status.flags.in_air && ((_imu_sample_delayed.time_us - _time_bad_motion_us) > (uint64_t)5E6);
bool flight_motion_ok = _control_status.flags.in_air && _range_aid_mode_enabled;
@@ -401,9 +403,6 @@ void Ekf::controlOpticalFlowFusion()
}
}
if (_flow_data_ready) {
// New optical flow data has fallen behind the fusion time horizon and is ready to be fused
// Accumulate autopilot gyro data across the same time interval as the flow sensor
_imu_del_ang_of += _imu_sample_delayed.delta_ang - _state.gyro_bias;
_delta_time_of += _imu_sample_delayed.delta_ang_dt;