From 4540faf6afd325a50a36fac2f6e5c1122ef1fabc Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Thu, 29 Dec 2016 09:17:47 +1100
Subject: [PATCH] AP_NavEKF3: Improve waiting for GPS configuration

Do not time out and provide an escalating series of messages. We may need to adjust the time thresholds used for escalation.
Do not wait if the EKF is not using the GPS.
---
 libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 38 ++++++++++++++----------
 1 file changed, 22 insertions(+), 16 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
index 6f9bb46664..ee9f8c0cf4 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
@@ -61,27 +61,33 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c
         return false;
     }
 
-    // Wait up to 30 seconds for all GPS units to finish their configuration.
-    // Until this has occurred we will not know what type of GPS is being used
-    // and what its time delay is
-    if (!_ahrs->get_gps().all_configured() && (AP_HAL::millis() < 30E3)) {
-        if (AP_HAL::millis() - lastInitFailReport_ms > 5000) {
-            lastInitFailReport_ms = AP_HAL::millis();
-            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 waiting for GPS config data");
-        }
-        return false;
-    } else if (!_ahrs->get_gps().all_configured()) {
-        GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF3 GPS config unavailable - using default time delay");
-    }
-
     // find the maximum time delay for all potential sensors
-    uint16_t maxTimeDelay_ms = MAX((uint16_t)(_ahrs->get_gps().get_lag() * 1000.0f) ,
-        MAX(_frontend->_hgtDelay_ms ,
+    uint16_t maxTimeDelay_ms = MAX(_frontend->_hgtDelay_ms ,
             MAX(_frontend->_flowDelay_ms ,
                 MAX(_frontend->_rngBcnDelay_ms ,
                     MAX(_frontend->magDelay_ms ,
                         (uint16_t)(dtEkfAvg*1000.0f)
-                                  )))));
+                                  ))));
+
+    // GPS sensing can have large delays and should not be included if disabled
+    if (_frontend->_fusionModeGPS != 3) {
+        // Wait for the configuration of all GPS units to be confirmed. Until this has occurred the GPS driver cannot provide a correct time delay
+        if (!_ahrs->get_gps().all_configured()) {
+            if (AP_HAL::millis() - lastInitFailReport_ms > 10000) {
+                lastInitFailReport_ms = AP_HAL::millis();
+                // provide an escalating series of messages
+                if (AP_HAL::millis() > 30000) {
+                    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_ERROR, "EKF3 waiting for GPS config data");
+                } else if (AP_HAL::millis() > 15000) {
+                    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF3 waiting for GPS config data");
+                } else  {
+                    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 waiting for GPS config data");
+                }
+            }
+            return false;
+        }
+        maxTimeDelay_ms = MAX(maxTimeDelay_ms , (uint16_t)(_ahrs->get_gps().get_lag() * 1000.0f));
+    }
 
     // airspeed sensing can have large delays and should not be included if disabled
     if (_ahrs->airspeed_sensor_enabled()) {