From 6bee4216c06806224884ffe85b08a40083b109a8 Mon Sep 17 00:00:00 2001
From: chobits <chobits@itri.org.tw>
Date: Wed, 13 Mar 2019 16:21:14 +0800
Subject: [PATCH] Copter: speed up EKF failsafe by checking if velocity
 innovations > 2x FS_EKF_THRESH

---
 ArduCopter/ekf_check.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/ArduCopter/ekf_check.cpp b/ArduCopter/ekf_check.cpp
index 55155a19f4..e0f60d225b 100644
--- a/ArduCopter/ekf_check.cpp
+++ b/ArduCopter/ekf_check.cpp
@@ -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) {