Copter: do not speed up EKF failsafe if optflow works

5 years ago · 5a0fe4e322
1 changed files with 1 additions and 1 deletions
--- a/ArduCopter/ekf_check.cpp
+++ b/ArduCopter/ekf_check.cpp
@ -110,7 +110,7 @@ bool Copter::ekf_over_threshold()
				@@ -110,7 +110,7 @@ bool Copter::ekf_over_threshold()
    if (mag_variance.length() >= g.fs_ekf_thresh) {
        over_thresh_count++;
    }
-    if (vel_variance >= (2.0f * g.fs_ekf_thresh)) {
+    if (!optflow.healthy() && (vel_variance >= (2.0f * g.fs_ekf_thresh))) {
        over_thresh_count += 2;
    } else if (vel_variance >= g.fs_ekf_thresh) {
        over_thresh_count++;