AP_GPS: Allow the user to specify the GPS time delay

If the user sets a non-zero value of the delay it will be used in preference over the default value for that GPS type.
If the GPS type is unknown and the parameter is set to zero, then a default delay of 1 sample period will be used (eg 200ms for 5Hz).

libraries/AP_GPS/AP_GPS.cpp

@@ -198,6 +198,21 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("POS2", 17, AP_GPS, _antenna_offset[1], 0.0f),
 
+    // @Param: DELAY_MS
+    // @DisplayName: GPS delay in milliseconds
+    // @Description: Controls the amount of GPS  measurement delay that the autopilot compensates for. Set to zero to use the default delay for the detected GPS type.
+    // @Units: msec
+    // @Range: 0 250
+    // @User: Advanced
+    AP_GROUPINFO("DELAY_MS", 18, AP_GPS, _delay_ms[0], 0),
+
+    // @Param: DELAY_MS2
+    // @DisplayName: GPS 2 delay in milliseconds
+    // @Description: Controls the amount of GPS  measurement delay that the autopilot compensates for. Set to zero to use the default delay for the detected GPS type.
+    // @Units: msec
+    // @Range: 0 250
+    // @User: Advanced
+    AP_GROUPINFO("DELAY_MS2", 19, AP_GPS, _delay_ms[1], 0),
     AP_GROUPEND
 };
 
@@ -867,9 +882,15 @@ void AP_GPS::inject_data_all(const uint8_t *data, uint16_t len)
  */
 float AP_GPS::get_lag(uint8_t instance) const
 {
-    if (drivers[instance] == nullptr || state[instance].status == NO_GPS) {
-        // return default;
-        return 0.2f;
+    if (_delay_ms[instance] > 0) {
+        // if the user has specified a non zero time delay, always return that value
+        return 0.001f * (float)_delay_ms[instance];
+    } else if (drivers[instance] == nullptr || state[instance].status == NO_GPS) {
+        // the user has not specified a value and we cannot determine it from the GPS type
+        // so return a default delay of 1 measurement interval
+        return 0.001f * (float)_rate_ms[instance];
+    } else {
+        // the user has not specified a delay so we determine it from the GPS type
+        return drivers[instance]->get_lag();
     }
-    return drivers[instance]->get_lag();
 }

libraries/AP_GPS/AP_GPS.h

@@ -354,6 +354,7 @@ public:
     AP_Int8 _save_config;
     AP_Int8 _auto_config;
     AP_Vector3f _antenna_offset[2];
+    AP_Int16 _delay_ms[2];
 
     // handle sending of initialisation strings to the GPS
     void send_blob_start(uint8_t instance, const char *_blob, uint16_t size);