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

EKF: rework optical flow selection logic

Browse Source 
Make use of range aiding movement check clearer.
Fix logic errors that caused rapid toggling of flow use when on ground.
This commit is contained in:
Paul Riseborough 2018-05-29 17:45:59 +10:00 committed by Lorenz Meier
parent 487e6a0901
commit e383b6a272
1 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
EKF/control.cpp
View File

@ -365,21 +365,25 @@ void Ekf::controlOpticalFlowFusion()
} else {
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
bool flow_required = _control_status.flags.in_air &&
(_is_dead_reckoning || (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim)));
if (!_inhibit_flow_use) {
// check if we should inhibit use of optical flow
bool movement_not_ok = ((_imu_sample_delayed.time_us - _time_good_motion_us) > (uint64_t)1E5) // on-ground check
&& !_range_aid_mode_enabled; // in-air check
if (movement_not_ok || !flow_required) {
// 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 {
bool movement_ok = ((_imu_sample_delayed.time_us - _time_bad_motion_us) > (uint64_t)5E6) || _range_aid_mode_enabled;
if (movement_ok || flow_required) {
// 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;
if (preflight_motion_ok || flight_motion_ok || flow_required) {
_inhibit_flow_use = false;
}
}