From be0096e812a7f9233b106bd85c6208d28b72f364 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Thu, 29 Dec 2016 09:20:06 +1100
Subject: [PATCH] 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 | 29 +++++++++++++++++++++++++----
 libraries/AP_GPS/AP_GPS.h   |  1 +
 2 files changed, 26 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
index e68a843602..d35a6d423c 100644
--- a/libraries/AP_GPS/AP_GPS.cpp
+++ b/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();
 }
diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h
index 15b1379aa8..078bcfe93a 100644
--- a/libraries/AP_GPS/AP_GPS.h
+++ b/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);