From 44e1695d5a01d0d8194f8498ea2f572d129b5695 Mon Sep 17 00:00:00 2001 From: priseborough Date: Wed, 29 Oct 2014 11:55:53 +1100 Subject: [PATCH] AP_AHRS : Enable EKF start without GPS --- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index afbf464edf..c0fcc50573 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -77,15 +77,13 @@ void AP_AHRS_NavEKF::update(void) _dcm_attitude(roll, pitch, yaw); if (!ekf_started) { - // if we have a GPS lock and more than 6 satellites, we can start the EKF - if (get_gps().status() >= AP_GPS::GPS_OK_FIX_3D && get_gps().num_sats() >= _gps_minsats) { - 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.InitialiseFilterDynamic(); - } + // wait 10 seconds + 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.InitialiseFilterDynamic(); } } if (ekf_started) {