AP_GPS: make more precise calculation of gps message

use PPS signal if available as well

libraries/AP_GPS/AP_GPS.cpp

@@ -513,12 +513,30 @@ uint64_t AP_GPS::time_epoch_convert(uint16_t gps_week, uint32_t gps_ms)
 uint64_t AP_GPS::time_epoch_usec(uint8_t instance) const
 {
     const GPS_State &istate = state[instance];
-    if (istate.last_gps_time_ms == 0 || istate.time_week == 0) {
+    if ((istate.last_gps_time_ms == 0 && istate.last_corrected_gps_time_us == 0) || istate.time_week == 0) {
         return 0;
     }
-    uint64_t fix_time_ms = time_epoch_convert(istate.time_week, istate.time_week_ms);
-    // add in the milliseconds since the last fix
-    return (fix_time_ms + (AP_HAL::millis() - istate.last_gps_time_ms)) * 1000ULL;
+    uint64_t fix_time_ms;
+    // add in the time since the last fix message
+    if (istate.last_corrected_gps_time_us != 0) {
+        fix_time_ms = time_epoch_convert(istate.time_week, drivers[instance]->get_last_itow());
+        return (fix_time_ms*1000ULL) + (AP_HAL::micros64() - istate.last_corrected_gps_time_us);
+    } else {
+        fix_time_ms = time_epoch_convert(istate.time_week, istate.time_week_ms) * 1000ULL;
+        return (fix_time_ms + (AP_HAL::millis() - istate.last_gps_time_ms)) * 1000ULL;
+    }
+}
+
+/**
+   calculate last message time since the unix epoch in microseconds
+ */
+uint64_t AP_GPS::last_message_epoch_usec(uint8_t instance) const
+{
+    const GPS_State &istate = state[instance];
+    if (istate.time_week == 0) {
+        return 0;
+    }
+    return time_epoch_convert(istate.time_week, drivers[instance]->get_last_itow()) * 1000ULL;
 }
 
 /*

libraries/AP_GPS/AP_GPS.h

@@ -195,6 +195,7 @@ public:
         bool have_gps_yaw_accuracy;       ///< does the GPS give a heading accuracy estimate? Set to true only once available
         uint32_t last_gps_time_ms;          ///< the system time we got the last GPS timestamp, milliseconds
         uint32_t uart_timestamp_ms;         ///< optional timestamp from set_uart_timestamp()
+        uint64_t last_corrected_gps_time_us;///< the system time we got the last corrected GPS timestamp, microseconds
         uint32_t lagged_sample_count;       ///< number of samples with 50ms more lag than expected
 
         // all the following fields must only all be filled by RTK capable backend drivers
@@ -478,6 +479,11 @@ public:
         return time_epoch_usec(primary_instance);
     }
 
+    uint64_t last_message_epoch_usec(uint8_t instance) const;
+    uint64_t last_message_epoch_usec() const {
+        return last_message_epoch_usec(primary_instance);
+    }
+
     // convert GPS week and millis to unix epoch in ms
     static uint64_t time_epoch_convert(uint16_t gps_week, uint32_t gps_ms);
 

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -1055,6 +1055,9 @@ AP_GPS_UBLOX::_parse_gps(void)
                   (unsigned)_buffer.nav_tp5.version,
                   (unsigned)_buffer.nav_tp5.flags,
                   (unsigned)_buffer.nav_tp5.freqPeriod);
+#ifdef HAL_GPIO_PPS
+            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;
             if (_buffer.nav_tp5.flags != desired_flags ||
@@ -1531,6 +1534,17 @@ AP_GPS_UBLOX::_parse_gps(void)
     return false;
 }
 
+/*
+ *  handle pps interrupt
+ */
+#ifdef HAL_GPIO_PPS
+void
+AP_GPS_UBLOX::pps_interrupt(uint8_t pin, bool high, uint32_t timestamp_us)
+{
+    _last_pps_time_us = timestamp_us;
+}
+#endif
+
 
 // UBlox auto configuration
 

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -759,6 +759,10 @@ private:
     // return true if GPS is capable of F9 config
     bool supports_F9_config(void) const;
 
+#ifdef HAL_GPIO_PPS
+    void pps_interrupt(uint8_t pin, bool high, uint32_t timestamp_us);
+#endif
+
 #if GPS_MOVING_BASELINE
     // config for moving baseline base
     static const config_list config_MB_Base_uart1[];

libraries/AP_GPS/GPS_Backend.cpp

@@ -249,8 +249,11 @@ void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length)
 
         // get the time the packet arrived on the UART
         uint64_t uart_us;
-        if (port) {
+        if (port && _last_pps_time_us == 0) {
             uart_us = port->receive_time_constraint_us(msg_length);
+        } else if (_last_pps_time_us != 0) {
+            uart_us = _last_pps_time_us;
+            _last_pps_time_us = 0;
         } else {
             uart_us = AP_HAL::micros64();
         }
@@ -287,6 +290,7 @@ void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length)
 
         // use msg arrival time, and correct for jitter
         uint64_t local_us = jitter_correction.correct_offboard_timestamp_usec(_pseudo_itow, uart_us);
+        state.last_corrected_gps_time_us = local_us;
         state.uart_timestamp_ms = local_us / 1000U;
 
         // look for lagged data from the GPS. This is meant to detect
@@ -302,6 +306,10 @@ void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length)
                 state.lagged_sample_count = 0;
             }
         }
+        if (state.status >= AP_GPS::GPS_OK_FIX_2D) {
+            // we must have a decent fix to calculate difference between itow and pseudo-itow
+            _pseudo_itow_delta_ms = itow - (_pseudo_itow/1000ULL);
+        }
     }
 }
 

libraries/AP_GPS/GPS_Backend.h

@@ -91,7 +91,7 @@ public:
 
     // return iTOW of last message, or zero if not supported
     uint32_t get_last_itow(void) const {
-        return _last_itow;
+        return (_pseudo_itow_delta_ms == 0)?(_last_itow*1000ULL):((_pseudo_itow/1000ULL) + _pseudo_itow_delta_ms);
     }
 
     enum DriverOptions : int16_t {
@@ -155,10 +155,14 @@ protected:
     void log_data(const uint8_t *data, uint16_t length);
 #endif
 
+protected:
+    uint64_t _last_pps_time_us;
+
 private:
     // itow from previous message
     uint32_t _last_itow;
     uint64_t _pseudo_itow;
+    int32_t _pseudo_itow_delta_ms;
     uint32_t _last_ms;
     uint32_t _rate_ms;
     uint32_t _last_rate_ms;

libraries/AP_RTC/JitterCorrection.h

@@ -16,7 +16,9 @@ public:
     // correct an offboard timestamp to a jitter-free local
     // timestamp. See JitterCorrection.cpp for details
     uint32_t correct_offboard_timestamp_msec(uint32_t offboard_ms, uint32_t local_ms);
-    
+
+    int64_t get_link_offset_usec(void) const { return link_offset_usec; }
+
 private:
     const uint16_t max_lag_ms;
     const uint16_t convergence_loops;