From e383b6a272665b01465a855abb0fa2fa4d24e6d0 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 29 May 2018 17:45:59 +1000 Subject: [PATCH] EKF: rework optical flow selection logic Make use of range aiding movement check clearer. Fix logic errors that caused rapid toggling of flow use when on ground. --- EKF/control.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 7cdcb7c30b..4405fc2401 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -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; } }