AP_GPS: sync pps rate with message rate

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
 
 

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

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

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);