Copter: speed up EKF failsafe by checking if velocity innovations > 2x FS_EKF_THRESH

6 years ago · 6bee4216c0
1 changed files with 3 additions and 1 deletions
--- a/ArduCopter/ekf_check.cpp
+++ b/ArduCopter/ekf_check.cpp
@ -105,7 +105,9 @@ bool Copter::ekf_over_threshold()
				@@ -105,7 +105,9 @@ bool Copter::ekf_over_threshold()
    if (mag_variance.length() >= g.fs_ekf_thresh) {
        over_thresh_count++;
    }
-    if (vel_variance >= g.fs_ekf_thresh) {
+    if (vel_variance >= (2.0f * g.fs_ekf_thresh)) {
+        over_thresh_count+=2;
+    } else if (vel_variance >= g.fs_ekf_thresh) {
        over_thresh_count++;
    }
    if (position_variance >= g.fs_ekf_thresh) {