From 6bdbe032f9edad7aa9f452d52dafd1336dbeeaaf Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 28 May 2018 18:20:15 +1000 Subject: [PATCH] EKF: Fallback to optical flow for all in-flight loss of navigation scenarios --- EKF/control.cpp | 16 +++++++++------- EKF/ekf.h | 2 +- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 207ae9a1d5..7339733251 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -337,7 +337,7 @@ void Ekf::controlExternalVisionFusion() void Ekf::controlOpticalFlowFusion() { - // Check if motion is un-suitable for use of optical flow + // Check if on ground motion is un-suitable for use of optical flow if (!_control_status.flags.in_air) { // When on ground check if the vehicle is being shaken or moved in a way that could cause a loss of navigation float accel_norm = _accel_vec_filt.norm(); @@ -365,19 +365,21 @@ void Ekf::controlOpticalFlowFusion() } else { gps_err_norm_lim = 1.0f; } - bool flow_required = !_control_status.flags.gps || (_gps_error_norm > gps_err_norm_lim); + // 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) - && !_range_aid_mode_enabled; + 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_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) { + bool movement_ok = ((_imu_sample_delayed.time_us - _time_bad_motion_us) > (uint64_t)5E6) || _range_aid_mode_enabled; + if (movement_ok || flow_required) { _inhibit_flow_use = false; } } diff --git a/EKF/ekf.h b/EKF/ekf.h index 8fcdb99806..0b46088cfa 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -436,7 +436,7 @@ private: bool _bad_vert_accel_detected{false}; ///< true when bad vertical accelerometer data has been detected // variables used to control range aid functionality - bool _range_aid_mode_enabled{false}; ///< true when range finder can be used as the height reference instead of the primary height sensor + bool _range_aid_mode_enabled{false}; ///< true when range finder can be used in flight as the height reference instead of the primary height sensor bool _range_aid_mode_selected{false}; ///< true when range finder is being used as the height reference instead of the primary height sensor // variables used to check for "stuck" rng data