From 46294c0fba2bfb2582bd5c640e1934262575b1b1 Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Tue, 18 Jan 2022 17:07:15 +0530
Subject: [PATCH] AP_GPS: sync pps rate with message rate

---
 libraries/AP_GPS/AP_GPS_UBLOX.cpp | 10 +++++++++-
 libraries/AP_GPS/AP_GPS_UBLOX.h   |  2 ++
 libraries/AP_GPS/GPS_Backend.cpp  |  3 +++
 libraries/AP_GPS/GPS_Backend.h    |  2 ++
 4 files changed, 16 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp
index dbb4012a79..7d7ccfe444 100644
--- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp
+++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp
@@ -1059,7 +1059,8 @@ AP_GPS_UBLOX::_parse_gps(void)
             hal.gpio->attach_interrupt(HAL_GPIO_PPS, FUNCTOR_BIND_MEMBER(&AP_GPS_UBLOX::pps_interrupt, void, uint8_t, bool, uint32_t), AP_HAL::GPIO::INTERRUPT_FALLING);
 #endif
             const uint16_t desired_flags = 0x003f;
-            const uint16_t desired_period_hz = 1;
+            const uint16_t desired_period_hz = _pps_freq;
+
             if (_buffer.nav_tp5.flags != desired_flags ||
                 _buffer.nav_tp5.freqPeriod != desired_period_hz) {
                 _buffer.nav_tp5.tpIdx = 0;
@@ -1543,6 +1544,13 @@ AP_GPS_UBLOX::pps_interrupt(uint8_t pin, bool high, uint32_t timestamp_us)
 {
     _last_pps_time_us = timestamp_us;
 }
+
+void
+AP_GPS_UBLOX::set_pps_desired_freq(uint8_t freq)
+{
+    _pps_freq = freq;
+    _unconfigured_messages |= CONFIG_TP5;
+}
 #endif
 
 
diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h
index c23765bfb7..a0b3a6bbb7 100644
--- a/libraries/AP_GPS/AP_GPS_UBLOX.h
+++ b/libraries/AP_GPS/AP_GPS_UBLOX.h
@@ -759,8 +759,10 @@ private:
     // return true if GPS is capable of F9 config
     bool supports_F9_config(void) const;
 
+    uint8_t _pps_freq = 1;
 #ifdef HAL_GPIO_PPS
     void pps_interrupt(uint8_t pin, bool high, uint32_t timestamp_us);
+    void set_pps_desired_freq(uint8_t freq) override;
 #endif
 
 #if GPS_MOVING_BASELINE
diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp
index a39ff7bdc8..ea09008b94 100644
--- a/libraries/AP_GPS/GPS_Backend.cpp
+++ b/libraries/AP_GPS/GPS_Backend.cpp
@@ -277,6 +277,9 @@ void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length)
         } else {
             _rate_counter = 0;
             _last_rate_ms = dt_ms;
+            if (_rate_ms != 0) {
+                set_pps_desired_freq(1000/_rate_ms);
+            }
         }
         if (_rate_ms == 0) {
             // only allow 5Hz to 20Hz in user config
diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h
index 6e6d16d84a..909ed41b4b 100644
--- a/libraries/AP_GPS/GPS_Backend.h
+++ b/libraries/AP_GPS/GPS_Backend.h
@@ -150,6 +150,8 @@ protected:
         return gps.get_type(state.instance);
     }
 
+    virtual void set_pps_desired_freq(uint8_t freq) {}
+
 #if AP_GPS_DEBUG_LOGGING_ENABLED
     // log some data for debugging
     void log_data(const uint8_t *data, uint16_t length);