From 82de3141f00a22361c449543a004a7c091a046b7 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 18 Apr 2018 08:26:03 +1000 Subject: [PATCH] EKF: Don't use optical flow if GPS is good and the vehicle is not using range finder for height --- EKF/control.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 32bb61e0a2..08ec2351ad 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -352,8 +352,13 @@ void Ekf::controlOpticalFlowFusion() } } else { - _time_bad_motion_us = 0; - _time_good_motion_us = _imu_sample_delayed.time_us; + // inhibit flow use if GPS quality is good and we are not using range aiding + // this enables use of optical flow to improve station keeping when low and slow + if (_control_status.flags.gps && (_time_last_imu - _last_gps_fail_us > (uint64_t)5e6) && !_in_range_aid_mode) { + _time_bad_motion_us = _imu_sample_delayed.time_us; + } else { + _time_good_motion_us = _imu_sample_delayed.time_us; + } } // Inhibit flow use if on ground and motion is excessive