AP_NavEKF3: Enable use of GPS delay value from returned by the driver

Use the time delay returned by the GPS driver.
Wait long enough for the GPS configuration to be determined, but time out after 30 seconds and warn the user that a default value for time delay will be used.

libraries/AP_NavEKF3/AP_NavEKF3.cpp

@@ -182,14 +182,9 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
     // @Units: m
     AP_GROUPINFO("GLITCH_RAD", 7, NavEKF3, _gpsGlitchRadiusMax, GLITCH_RADIUS_DEFAULT),
 
-    // @Param: GPS_DELAY
+    // 8 previously used for EKF3_GPS_DELAY parameter that has been deprecated.
-    // @DisplayName: GPS measurement delay (msec)
+    // The EKF now takes its GPs delay form the GPS library with the default delays
-    // @Description: This is the number of msec that the GPS measurements lag behind the inertial measurements. The autpilot should be restarted after adjusting this parameter.
+    // specified by the GPS_DELAY and GPS_DELAY2 parameters.
-    // @Range: 0 250
-    // @Increment: 10
-    // @User: Advanced
-    // @Units: msec
-    AP_GROUPINFO("GPS_DELAY", 8, NavEKF3, _gpsDelay_ms, 150),
 
     // Height measurement parameters
 
@@ -605,33 +600,43 @@ bool NavEKF3::InitialiseFilter(void)
 
     imuSampleTime_us = AP_HAL::micros64();
 
+    if (core == nullptr) {
+
         // see if we will be doing logging
         DataFlash_Class *dataflash = DataFlash_Class::instance();
         if (dataflash != nullptr) {
             logging.enabled = dataflash->log_replay();
         }
 
-    if (core == nullptr) {
-
         // don't run multiple filters for 1 IMU
         const AP_InertialSensor &ins = _ahrs->get_ins();
         uint8_t mask = (1U<<ins.get_accel_count())-1;
         _imuMask.set(_imuMask.get() & mask);
         
-        // count IMUs from mask
+        // initialise the setup variables
+        for (uint8_t i=0; i<7; i++) {
+            coreSetupRequired[i] = false;
+            coreImuIndex[i] = 0;
+        }
         num_cores = 0;
+
+        // count IMUs from mask
         for (uint8_t i=0; i<7; i++) {
             if (_imuMask & (1U<<i)) {
+                coreSetupRequired[num_cores] = true;
+                coreImuIndex[num_cores] = i;
                 num_cores++;
             }
         }
 
+        // check if there is enough memory to create the EKF cores
         if (hal.util->available_memory() < sizeof(NavEKF3_core)*num_cores + 4096) {
             GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "NavEKF3: not enough memory");
             _enable.set(0);
             return false;
         }
         
+        // create the cores
         core = new NavEKF3_core[num_cores];
         if (core == nullptr) {
             _enable.set(0);
@@ -639,22 +644,24 @@ bool NavEKF3::InitialiseFilter(void)
             return false;
         }
 
-        // set the IMU index for the cores
+    }
-        num_cores = 0;
+
-        for (uint8_t i=0; i<7; i++) {
+    // Set up any cores that have been created
-            if (_imuMask & (1U<<i)) {
+    // This specifies the IMU to be used and creates the data buffers
-                if(!core[num_cores].setup_core(this, i, num_cores)) {
+    // If we are unble to set up a core, return false and try again next time the function is called
+    for (uint8_t core_index=0; core_index<num_cores; core_index++) {
+        if (coreSetupRequired[core_index]) {
+            coreSetupRequired[core_index] = !core[core_index].setup_core(this, coreImuIndex[core_index], core_index);
+            if(coreSetupRequired[core_index]) {
                 return false;
             }
-                num_cores++;
         }
     }
 
     // Set the primary initially to be the lowest index
     primary = 0;
-    }
 
-    // initialse the cores. We return success only if all cores
+    // initialise the cores. We return success only if all cores
     // initialise successfully
     bool ret = true;
     for (uint8_t i=0; i<num_cores; i++) {

libraries/AP_NavEKF3/AP_NavEKF3.h

@@ -327,7 +327,6 @@ private:
     AP_Float _accNoise;             // accelerometer process noise : m/s^2
     AP_Float _gyroBiasProcessNoise; // gyro bias state process noise : rad/s
     AP_Float _accelBiasProcessNoise;// accel bias state process noise : m/s^2
-    AP_Int16 _gpsDelay_ms;          // effective average delay of GPS measurements relative to inertial measurement (msec)
     AP_Int16 _hgtDelay_ms;          // effective average delay of Height measurements relative to inertial measurements (msec)
     AP_Int8  _fusionModeGPS;        // 0 = use 3D velocity, 1 = use 2D velocity, 2 = use no velocity
     AP_Int16  _gpsVelInnovGate;     // Percentage number of standard deviations applied to GPS velocity innovation consistency check
@@ -423,6 +422,9 @@ private:
 
     bool runCoreSelection; // true when the primary core has stabilised and the core selection logic can be started
 
+    bool coreSetupRequired[7]; // true when this core index needs to be setup
+    uint8_t coreImuIndex[7];   // IMU index used by this core
+
     // update the yaw reset data to capture changes due to a lane switch
     // new_primary - index of the ekf instance that we are about to switch to as the primary
     // old_primary - index of the ekf instance that we are currently using as the primary

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -411,7 +411,8 @@ void NavEKF3_core::readGpsData()
 
             // estimate when the GPS fix was valid, allowing for GPS processing and other delays
             // ideally we should be using a timing signal from the GPS receiver to set this time
-            gpsDataNew.time_ms = lastTimeGpsReceived_ms - frontend->_gpsDelay_ms;
+            // Use the driver specified delay
+            gpsDataNew.time_ms = lastTimeGpsReceived_ms - (uint32_t)(_ahrs->get_gps().get_lag() * 1000.0f);
 
             // Correct for the average intersampling delay due to the filter updaterate
             gpsDataNew.time_ms -= localFilterTimeStep_ms/2;

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -36,6 +36,7 @@ NavEKF3_core::NavEKF3_core(void) :
     _perf_test[8] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test8");
     _perf_test[9] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK2_Test9");
     firstInitTime_ms = 0;
+    lastInitFailReport_ms = 0;
 }
 
 // setup this core backend
@@ -60,8 +61,21 @@ 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(_frontend->_gpsDelay_ms  ,
+    uint16_t maxTimeDelay_ms = MAX((uint16_t)(_ahrs->get_gps().get_lag() * 1000.0f) ,
         MAX(_frontend->_hgtDelay_ms ,
             MAX(_frontend->_flowDelay_ms ,
                 MAX(_frontend->_rngBcnDelay_ms ,

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -897,6 +897,7 @@ private:
     Vector3f velOffsetNED;          // This adds to the earth frame velocity estimate at the IMU to give the velocity at the body origin (m/s)
     Vector3f posOffsetNED;          // This adds to the earth frame position estimate at the IMU to give the position at the body origin (m)
     uint32_t firstInitTime_ms;      // First time the initialise function was called (msec)
+    uint32_t lastInitFailReport_ms; // Last time the buffer initialisation failure report wass sent (msec)
 
     // Specify preferred source of data to be used for a state reset
     enum resetDataSource {