fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 16:57:35 +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-05-30 11:11:02 +10: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; _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 // Inhibit flow use if motion is un-suitable or we have good quality GPS
// Apply hysteresis to prevent rapid mode switching // Apply hysteresis to prevent rapid mode switching
float gps_err_norm_lim; float gps_err_norm_lim;
@@ -366,20 +368,20 @@ void Ekf::controlOpticalFlowFusion()
gps_err_norm_lim = 1.0f; 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 // Check if we are in-air and require optical flow to control position drift
// is necessary to control position drift
bool flow_required = _control_status.flags.in_air && 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 // 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 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; 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) { if ((preflight_motion_not_ok || flight_motion_not_ok) && !flow_required) {
_inhibit_flow_use = true; _inhibit_flow_use = true;
} }
} else if (_inhibit_flow_use && !_control_status.flags.opt_flow){
} else {
// allow use of optical flow if motion is suitable or we are reliant on it for flight navigation // 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 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; 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 // 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; _imu_del_ang_of += _imu_sample_delayed.delta_ang - _state.gyro_bias;
_delta_time_of += _imu_sample_delayed.delta_ang_dt; _delta_time_of += _imu_sample_delayed.delta_ang_dt;