From d761b24b92253cebeb3c76ba89ea9a0473791ebe Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 22 Jun 2018 15:25:27 +1000
Subject: [PATCH] AP_GPS: optionally configure a hardware PPS pin for uBlox

useful for testing time lag estimation
---
 libraries/AP_GPS/AP_GPS_UBLOX.cpp | 51 ++++++++++++++++++++++++++++++-
 libraries/AP_GPS/AP_GPS_UBLOX.h   | 17 +++++++++++
 2 files changed, 67 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp
index 91b1ee2782..bf356d6e7d 100644
--- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp
+++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp
@@ -34,6 +34,7 @@
 
 #define UBLOX_DEBUGGING 0
 #define UBLOX_FAKE_3DLOCK 0
+#define CONFIGURE_PPS_PIN 0
 
 extern const AP_HAL::HAL& hal;
 
@@ -57,6 +58,10 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART
 
     // start the process of updating the GPS rates
     _request_next_config();
+
+#if CONFIGURE_PPS_PIN
+    _unconfigured_messages |= CONFIG_TP5;
+#endif
 }
 
 void
@@ -73,7 +78,7 @@ AP_GPS_UBLOX::_request_next_config(void)
         return;
     }
 
-   Debug("Unconfigured messages: %d Current message: %d\n", _unconfigured_messages, _next_message);
+    Debug("Unconfigured messages: %u Current message: %u\n", (unsigned)_unconfigured_messages, (unsigned)_next_message);
 
    // check AP_GPS_UBLOX.h for the enum that controls the order.
    // This switch statement isn't maintained against the enum in order to reduce code churn
@@ -111,6 +116,13 @@ AP_GPS_UBLOX::_request_next_config(void)
             _next_message--;
         }
         break;
+    case STEP_POLL_TP5:
+#if CONFIGURE_PPS_PIN
+        if (!_send_message(CLASS_CFG, MSG_CFG_TP5, nullptr, 0)) {
+            _next_message--;
+        }
+#endif
+        break;
     case STEP_NAV_RATE:
         if (!_send_message(CLASS_CFG, MSG_CFG_RATE, nullptr, 0)) {
             _next_message--;
@@ -622,6 +634,9 @@ AP_GPS_UBLOX::_parse_gps(void)
                 case MSG_CFG_SBAS:
                     _unconfigured_messages &= ~CONFIG_SBAS;
                     break;
+                case MSG_CFG_TP5:
+                    _unconfigured_messages &= ~CONFIG_TP5;
+                    break;
                 }
                 break;
             case CLASS_MON:
@@ -779,6 +794,40 @@ AP_GPS_UBLOX::_parse_gps(void)
                 _unconfigured_messages &= ~CONFIG_RATE_NAV;
             }
             return false;
+            
+#if CONFIGURE_PPS_PIN
+        case MSG_CFG_TP5: {
+            // configure the PPS pin for 1Hz, zero delay
+            Debug("Got TP5 ver=%u 0x%04x %u\n", 
+                  (unsigned)_buffer.nav_tp5.version,
+                  (unsigned)_buffer.nav_tp5.flags,
+                  (unsigned)_buffer.nav_tp5.freqPeriod);
+            const uint16_t desired_flags = 0x003f;
+            const uint16_t desired_period_hz = 1;
+            if (_buffer.nav_tp5.flags != desired_flags ||
+                _buffer.nav_tp5.freqPeriod != desired_period_hz) {
+                _buffer.nav_tp5.tpIdx = 0;
+                _buffer.nav_tp5.reserved1[0] = 0;
+                _buffer.nav_tp5.reserved1[1] = 0;
+                _buffer.nav_tp5.antCableDelay = 0;
+                _buffer.nav_tp5.rfGroupDelay = 0;
+                _buffer.nav_tp5.freqPeriod = desired_period_hz;
+                _buffer.nav_tp5.freqPeriodLock = desired_period_hz;
+                _buffer.nav_tp5.pulseLenRatio = 1;
+                _buffer.nav_tp5.pulseLenRatioLock = 2;
+                _buffer.nav_tp5.userConfigDelay = 0;
+                _buffer.nav_tp5.flags = desired_flags;
+                _send_message(CLASS_CFG, MSG_CFG_TP5,
+                              &_buffer.nav_tp5,
+                              sizeof(_buffer.nav_tp5));
+                _unconfigured_messages |= CONFIG_TP5;
+                _cfg_needs_save = true;
+            } else {
+                _unconfigured_messages &= ~CONFIG_TP5;
+            }
+            return false;
+        }
+#endif // CONFIGURE_PPS_PIN
         }
            
     }
diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h
index e48674e5c2..0e35990806 100644
--- a/libraries/AP_GPS/AP_GPS_UBLOX.h
+++ b/libraries/AP_GPS/AP_GPS_UBLOX.h
@@ -72,6 +72,7 @@
 #define CONFIG_GNSS          (1<<11)
 #define CONFIG_SBAS          (1<<12)
 #define CONFIG_RATE_PVT      (1<<13)
+#define CONFIG_TP5           (1<<14)
 
 #define CONFIG_REQUIRED_INITIAL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_VELNED)
 
@@ -182,6 +183,19 @@ private:
         uint32_t res3;
         uint32_t res4;
     };
+    struct PACKED ubx_cfg_tp5 {
+        uint8_t tpIdx;
+        uint8_t version;
+        uint8_t reserved1[2];
+        int16_t antCableDelay;
+        int16_t rfGroupDelay;
+        uint32_t freqPeriod;
+        uint32_t freqPeriodLock;
+        uint32_t pulseLenRatio;
+        uint32_t pulseLenRatioLock;
+        int32_t userConfigDelay;
+        uint32_t flags;
+    };
     struct PACKED ubx_cfg_prt {
         uint8_t portID;
     };
@@ -409,6 +423,7 @@ private:
         ubx_mon_hw_68 mon_hw_68;
         ubx_mon_hw2 mon_hw2;
         ubx_mon_ver mon_ver;
+        ubx_cfg_tp5 nav_tp5;
 #if UBLOX_GNSS_SETTINGS
         ubx_cfg_gnss gnss;
 #endif
@@ -444,6 +459,7 @@ private:
         MSG_CFG_PRT = 0x00,
         MSG_CFG_SBAS = 0x16,
         MSG_CFG_GNSS = 0x3E,
+        MSG_CFG_TP5 = 0x31,
         MSG_MON_HW = 0x09,
         MSG_MON_HW2 = 0x0B,
         MSG_MON_VER = 0x04,
@@ -494,6 +510,7 @@ private:
         STEP_POLL_SBAS, // poll SBAS
         STEP_POLL_NAV, // poll NAV settings
         STEP_POLL_GNSS, // poll GNSS
+        STEP_POLL_TP5, // poll TP5
         STEP_DOP,
         STEP_MON_HW,
         STEP_MON_HW2,