AP_AHRS: fixed EKF startup bug

This fixes the EKF for when GPS lock takes more than 10 seconds

fixes issue #2010

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@@ -81,9 +81,9 @@ void AP_AHRS_NavEKF::update(void)
         if (start_time_ms == 0) {
             start_time_ms = hal.scheduler->millis();
         }
-        if (hal.scheduler->millis() - start_time_ms > startup_delay_ms) {
+        if (hal.scheduler->millis() - start_time_ms > startup_delay_ms &&
+            EKF.InitialiseFilterDynamic()) {
             ekf_started = true;
-            EKF.InitialiseFilterDynamic();
         }
     }
     if (ekf_started) {
@@ -152,7 +152,7 @@ void AP_AHRS_NavEKF::reset(bool recover_eulers)
 {
     AP_AHRS_DCM::reset(recover_eulers);
     if (ekf_started) {
-        EKF.InitialiseFilterBootstrap();        
+        ekf_started = EKF.InitialiseFilterBootstrap();        
     }
 }
 
@@ -161,7 +161,7 @@ void AP_AHRS_NavEKF::reset_attitude(const float &_roll, const float &_pitch, con
 {
     AP_AHRS_DCM::reset_attitude(_roll, _pitch, _yaw);
     if (ekf_started) {
-        EKF.InitialiseFilterBootstrap();        
+        ekf_started = EKF.InitialiseFilterBootstrap();        
     }
 }
 

libraries/AP_AHRS/AP_AHRS_NavEKF.h

@@ -38,7 +38,8 @@ public:
     AP_AHRS_DCM(ins, baro, gps),
         EKF(this, baro),
         ekf_started(false),
-        startup_delay_ms(10000)
+        startup_delay_ms(10000),
+        start_time_ms(0)
         {
         }