From 574946f0aab733fcc65ac14e758be855222780c1 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 4 Jan 2014 17:04:34 +1100
Subject: [PATCH] AP_AHRS: start EKF 5 seconds after getting GPS lock

---
 libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 15 ++++++++-------
 1 file changed, 8 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index d1b0e2fdcb..6460508e82 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -52,14 +52,15 @@ void AP_AHRS_NavEKF::update(void)
     _dcm_attitude(roll, pitch, yaw);
 
     if (!ekf_started) {
-        if (start_time_ms == 0) {
-            start_time_ms = hal.scheduler->millis();
-        }
         // if we have GPS lock and a compass set we can start the EKF
-        if (get_compass() && _gps && _gps->status() >= GPS::GPS_OK_FIX_3D &&
-            hal.scheduler->millis() - start_time_ms > startup_delay_ms) {
-            ekf_started = true;
-            EKF.InitialiseFilter();
+        if (get_compass() && _gps && _gps->status() >= GPS::GPS_OK_FIX_3D) {
+            if (start_time_ms == 0) {
+                start_time_ms = hal.scheduler->millis();
+            }
+            if (hal.scheduler->millis() - start_time_ms > startup_delay_ms) {
+                ekf_started = true;
+                EKF.InitialiseFilter();
+            }
         }
     }
     if (ekf_started) {