AP_GPS: added jitter correction to timestamp calculation

this allows for much more accurate (and consistent) timestamping of GPS
messages where the protocol has a ITOW field.

libraries/AP_GPS/GPS_Backend.cpp

@@ -233,6 +233,49 @@ void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length)
 {
     if (itow != _last_itow) {
         _last_itow = itow;
-        set_uart_timestamp(msg_length);
+
+        /*
+          we need to calculate a pseudo-itow, which copes with the
+          iTow from the GPS changing in unexpected ways. We assume
+          that timestamps from the GPS are always in multiples of
+          50ms. That means we can't handle a GPS with an update rate
+          of more than 20Hz. We could do more, but we'd need the GPS
+          poll time to be higher
+         */
+        const uint32_t gps_min_period_ms = 50;
+        
+        uint32_t now = AP_HAL::millis();
+        uint32_t dt_ms = now - _last_ms;
+        _last_ms = now;
+
+        // round to nearest 50ms period
+        dt_ms = ((dt_ms + (gps_min_period_ms/2)) / gps_min_period_ms) * gps_min_period_ms;
+
+        // work out an actual message rate. If we get 5 messages in a
+        // row with a new rate we switch rate
+        if (_last_rate_ms == dt_ms) {
+            if (_rate_counter < 5) {
+                _rate_counter++;
+            } else if (_rate_ms != dt_ms) {
+                _rate_ms = dt_ms;
+            }
+        } else {
+            _rate_counter = 0;
+            _last_rate_ms = dt_ms;
+        }
+        if (_rate_ms == 0) {
+            _rate_ms = gps.get_rate_ms(state.instance);
+        }
+
+        // round to calculated message rate
+        dt_ms = ((dt_ms + (_rate_ms/2)) / _rate_ms) * _rate_ms;
+
+        // calculate pseudo-itow
+        _pseudo_itow += dt_ms * 1000U;
+
+        // get msg arrival time, and correct for jitter
+        uint64_t uart_us = port->receive_time_constraint_us(msg_length);
+        uint64_t local_us = jitter_correction.correct_offboard_timestamp_usec(_pseudo_itow, uart_us);
+        state.uart_timestamp_ms = local_us / 1000U;
     }
 }

libraries/AP_GPS/GPS_Backend.h

@@ -19,6 +19,7 @@
 #pragma once
 
 #include <GCS_MAVLink/GCS_MAVLink.h>
+#include <AP_RTC/JitterCorrection.h>
 #include "AP_GPS.h"
 
 class AP_GPS_Backend
@@ -99,4 +100,11 @@ protected:
 private:
     // itow from previous message
     uint32_t _last_itow;
+    uint64_t _pseudo_itow;
+    uint32_t _last_ms;
+    uint32_t _rate_ms;
+    uint32_t _last_rate_ms;
+    uint16_t _rate_counter;
+
+    JitterCorrection jitter_correction;
 };