From 4a5bf0a266b1e2de85defe0accf6d5c440b6692c Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Wed, 22 Apr 2015 11:13:49 +1000
Subject: [PATCH] AP_NavEKF: Reduce EKF start time

Makes EKF start conditional on DCM solution tilt error
---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 11 ++++++++---
 1 file changed, 8 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 78f599d02f..2b5b3b3891 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -450,8 +450,8 @@ bool NavEKF::healthy(void) const
         // extremely unhealthy.
         return false;
     }
-    // Give the filter 10 seconds to settle before use
-    if ((imuSampleTime_ms - ekfStartTime_ms) < 10000) {
+    // Give the filter a second to settle before use
+    if ((imuSampleTime_ms - ekfStartTime_ms) < 1000 ) {
         return false;
     }
     // barometer innovations must be within limits when on-ground
@@ -537,6 +537,11 @@ bool NavEKF::InitialiseFilterDynamic(void)
         return false;
     }
 
+    // If the DCM solution has not converged, then don't initialise
+    if (_ahrs->get_error_rp() > 0.02f) {
+        return false;
+    }
+
     // Set re-used variables to zero
     InitialiseVariables();
 
@@ -4742,7 +4747,7 @@ void NavEKF::performArmingChecks()
 {
     // determine vehicle arm status and don't allow filter to arm until it has been running for long enough to stabilise
     prevVehicleArmed = vehicleArmed;
-    vehicleArmed = (getVehicleArmStatus() && (imuSampleTime_ms - ekfStartTime_ms) > 10000);
+    vehicleArmed = (getVehicleArmStatus() && (imuSampleTime_ms - ekfStartTime_ms) > 1000);
 
     // check to see if arm status has changed and reset states if it has
     if (vehicleArmed != prevVehicleArmed) {