From a19015ed61e9b52cf9c1379889f16239f31e3c42 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Fri, 16 May 2014 21:13:30 +1000
Subject: [PATCH] AP_NavEKF: Scale divergence check with covariance

This provides consistent behaviour for a range of gyro bias process
noise settings and automatically adjusts sensitivity as filter learns bias
---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 0b49a51382..13f304e1e9 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -3398,7 +3398,8 @@ void NavEKF::checkDivergence()
     Vector3f tempVec = state.gyro_bias - lastGyroBias;
     float tempLength = tempVec.length();
     if (tempLength != 0.0f) {
-        scaledDeltaGyrBiasLgth = 5e4f*tempVec.length()/dtIMU;
+        float temp = constrain_float((P[10][10] + P[11][11] + P[12][12]),1e-12f,1e-8f);
+        scaledDeltaGyrBiasLgth = (2e-6f / temp) * tempVec.length() / dtIMU;
     }
     bool divergenceDetected = (scaledDeltaGyrBiasLgth > 1.0f);
     lastGyroBias = state.gyro_bias;