diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h
index 424e341c1c8c1..071d04350ba81 100644
--- a/Tools/AP_Periph/AP_Periph.h
+++ b/Tools/AP_Periph/AP_Periph.h
@@ -425,6 +425,9 @@ class AP_Periph_FW {
 #ifdef HAL_PERIPH_ENABLE_GPS
     uint32_t last_gps_update_ms;
     uint32_t last_gps_yaw_ms;
+#ifdef HAL_PERIPH_ENABLE_GLOBALTIMESYNC
+    uint64_t get_tracked_tx_timestamp(uint8_t i);
+#endif
 #endif
     uint32_t last_relposheading_ms;
 #ifdef HAL_PERIPH_ENABLE_BARO
diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp
index c985403fd282d..4f8b418bc1bcf 100644
--- a/Tools/AP_Periph/can.cpp
+++ b/Tools/AP_Periph/can.cpp
@@ -1627,6 +1627,7 @@ void AP_Periph_FW::can_start()
 #else
             can_iface_periph[i]->init(g.can_baudrate[i], AP_HAL::CANIface::NormalMode);
 #endif
+            can_iface_periph[i]->set_track_tx_timestamp((0xFFFFLU << 8), ((uint32_t)UAVCAN_PROTOCOL_GLOBALTIMESYNC_ID)<<8);
         }
     }
 
@@ -1742,6 +1743,16 @@ void AP_Periph_FW::apd_esc_telem_update()
 #endif // HAL_PERIPH_ENABLE_ESC_APD
 #endif // HAL_PERIPH_ENABLE_RC_OUT
 
+#if defined(HAL_PERIPH_ENABLE_GLOBALTIMESYNC) && defined(HAL_PERIPH_ENABLE_GPS)
+uint64_t AP_Periph_FW::get_tracked_tx_timestamp(uint8_t i)
+{
+    if (can_iface_periph[i]) {
+        return can_iface_periph[i]->get_tracked_tx_timestamp();
+    }
+    return 0;
+}
+#endif
+
 void AP_Periph_FW::can_update()
 {
     const uint32_t now = AP_HAL::millis();
diff --git a/Tools/AP_Periph/gps.cpp b/Tools/AP_Periph/gps.cpp
index 029676f26d1b8..d0dc935feed30 100644
--- a/Tools/AP_Periph/gps.cpp
+++ b/Tools/AP_Periph/gps.cpp
@@ -65,7 +65,13 @@ void AP_Periph_FW::can_gps_update(void)
     if (gps.get_type(0) == AP_GPS::GPS_Type::GPS_TYPE_NONE) {
         return;
     }
+
+#ifdef HAL_PERIPH_ENABLE_GLOBALTIMESYNC
+    // we need to record this time as its reset when we call gps.update()
+    uint64_t last_message_local_time_us = periph.gps.last_pps_time_usec();
+#endif
     gps.update();
+
     send_moving_baseline_msg();
     send_relposheading_msg();
     if (last_gps_update_ms == gps.last_message_time_ms()) {
@@ -73,6 +79,35 @@ void AP_Periph_FW::can_gps_update(void)
     }
     last_gps_update_ms = gps.last_message_time_ms();
 
+#ifdef HAL_PERIPH_ENABLE_GLOBALTIMESYNC
+    // send time sync message every second
+    uavcan_protocol_GlobalTimeSync ts {};
+    for (uint8_t i=0; i<HAL_NUM_CAN_IFACES; i++) {
+        uint8_t idx; // unused
+        if (gps.status() < AP_GPS::GPS_OK_FIX_3D &&
+            !gps.is_healthy() &&
+            !gps.first_unconfigured_gps(idx)) {
+            // no good fix and no healthy GPS, so we don't send timesync
+            break;
+        }
+        uint64_t last_message_epoch_usec = gps.last_message_epoch_usec();
+        if (last_message_local_time_us != 0 &&
+            last_message_epoch_usec != 0) {
+            // (last_message_epoch_usec - last_message_local_time_us) represents the offset between the time in gps epoch and the local time of the node
+            // periph.get_tracked_tx_timestamp(i) represent the offset timestamp in the local time of the node
+            ts.previous_transmission_timestamp_usec = (last_message_epoch_usec - last_message_local_time_us) + get_tracked_tx_timestamp(i);
+            uint8_t buffer[UAVCAN_PROTOCOL_GLOBALTIMESYNC_MAX_SIZE] {};
+            uint16_t total_size = uavcan_protocol_GlobalTimeSync_encode(&ts, buffer, !canfdout());
+            canard_broadcast(UAVCAN_PROTOCOL_GLOBALTIMESYNC_SIGNATURE,
+                                UAVCAN_PROTOCOL_GLOBALTIMESYNC_ID,
+                                CANARD_TRANSFER_PRIORITY_HIGH,
+                                buffer,
+                                total_size,
+                                1U<<i);
+        }
+    }
+#endif
+
     {
         /*
           send Fix2 packet
diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
index 25a790787a8d4..78081985fb179 100644
--- a/libraries/AP_GPS/AP_GPS.cpp
+++ b/libraries/AP_GPS/AP_GPS.cpp
@@ -598,6 +598,13 @@ uint64_t AP_GPS::last_message_epoch_usec(uint8_t instance) const
     return istate_time_to_epoch_ms(istate.time_week, drivers[instance]->get_last_itow_ms()) * 1000ULL;
 }
 
+uint64_t AP_GPS::last_pps_time_usec(uint8_t instance) const
+{
+    if (drivers[instance] == nullptr) {
+        return 0;
+    }
+    return  drivers[instance]->get_last_pps_time_us();
+}
 /*
   send some more initialisation string bytes if there is room in the
   UART transmit buffer
@@ -2288,11 +2295,16 @@ void AP_GPS::Write_GPS(uint8_t i)
 
     /* write auxiliary accuracy information as well */
     float hacc = 0, vacc = 0, sacc = 0;
-    float undulation = 0;
+    float undulation = 0, lag = 0;
+    int64_t clock_drift = 0;
     horizontal_accuracy(i, hacc);
     vertical_accuracy(i, vacc);
     speed_accuracy(i, sacc);
     get_undulation(i, undulation);
+    get_lag(i, lag);
+    if (drivers[i] != nullptr) {
+        clock_drift = drivers[i]->get_clock_drift();
+    }
     struct log_GPA pkt2{
         LOG_PACKET_HEADER_INIT(LOG_GPA_MSG),
         time_us       : time_us,
@@ -2307,7 +2319,9 @@ void AP_GPS::Write_GPS(uint8_t i)
         delta_ms      : last_message_delta_time_ms(i),
         undulation    : undulation,
         rtcm_fragments_used: rtcm_stats.fragments_used,
-        rtcm_fragments_discarded: rtcm_stats.fragments_discarded
+        rtcm_fragments_discarded: rtcm_stats.fragments_discarded,
+        lag           : lag,
+        clock_drift   : clock_drift
     };
     AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
 }
diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h
index a5ca9cdceaddd..0c475b12cb78b 100644
--- a/libraries/AP_GPS/AP_GPS.h
+++ b/libraries/AP_GPS/AP_GPS.h
@@ -519,6 +519,11 @@ class AP_GPS
         return last_message_epoch_usec(primary_instance);
     }
 
+    uint64_t last_pps_time_usec(uint8_t instance) const;
+    uint64_t last_pps_time_usec() const {
+        return last_pps_time_usec(primary_instance);
+    }
+
     // convert GPS week and millis to unix epoch in ms
     static uint64_t istate_time_to_epoch_ms(uint16_t gps_week, uint32_t gps_ms);
 
diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp
index 5da56758a517c..d7056b523d1d9 100644
--- a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp
+++ b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp
@@ -54,11 +54,6 @@ extern const AP_HAL::HAL& hal;
 
 #define LOG_TAG "GPS"
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-#define NATIVE_TIME_OFFSET (AP_HAL::micros64() - AP_HAL::micros64())
-#else
-#define NATIVE_TIME_OFFSET 0
-#endif
 AP_GPS_DroneCAN::DetectedModules AP_GPS_DroneCAN::_detected_modules[];
 HAL_Semaphore AP_GPS_DroneCAN::_sem_registry;
 
@@ -116,6 +111,10 @@ void AP_GPS_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
         AP_BoardConfig::allocation_error("relposheading_sub");
     }
 #endif
+
+    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_timesync_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
+        AP_BoardConfig::allocation_error("timesync_sub");
+    }
 }
 
 AP_GPS_Backend* AP_GPS_DroneCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)
@@ -336,6 +335,18 @@ void AP_GPS_DroneCAN::handle_velocity(const float vx, const float vy, const floa
     }
 }
 
+bool AP_GPS_DroneCAN::get_fix_lag(float &lag) const
+{
+    if (_fix_lag_us) {
+        // we have gps message time synchronized using the timesync message
+        // which is driven by PPS. So we have accurate timestamp of 
+        // which timeslice the solution was calculated for
+        lag = _fix_lag_us/1000000.0f;
+        return true;
+    }
+    return AP_GPS_Backend::get_lag(lag);
+}
+
 void AP_GPS_DroneCAN::handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uint64_t timestamp_usec)
 {
     bool process = false;
@@ -428,25 +439,34 @@ void AP_GPS_DroneCAN::handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uin
         // hdop from pdop. Some GPS modules don't provide the Aux message
         interim_state.hdop = interim_state.vdop = msg.pdop * 100.0;
     }
-
-    if ((msg.timestamp.usec > msg.gnss_timestamp.usec) && (msg.gnss_timestamp.usec > 0)) {
+    uint64_t gps_message_timestamp_usec = msg.timestamp.usec;
+    if (timesync_correction && msg.gnss_timestamp.usec) {
+        interim_state.last_corrected_gps_time_us = AP_HAL::micros64();
+        interim_state.last_gps_time_ms = interim_state.last_corrected_gps_time_us/1000U;
+        int32_t fix_lag_us = interim_state.last_corrected_gps_time_us - (msg.gnss_timestamp.usec - timesync_correction_us);
+        Debug("fix_lag[%d]: %ld\n",  _detected_modules[_detected_module].node_id, fix_lag_us);
+        interim_state.corrected_timestamp_updated = true;
+        if (fix_lag_us > 5000 && fix_lag_us < 250000 ) {
+            _fix_lag_us = fix_lag_us;
+        }
+        last_gnss_timestamp_usec = msg.gnss_timestamp.usec; // for use by update_relposned_lag
+    } else if ((gps_message_timestamp_usec > msg.gnss_timestamp.usec) && (msg.gnss_timestamp.usec > 0)) {
         // we have a valid timestamp based on gnss_timestamp timescale, we can use that to correct our gps message time
-        interim_state.last_corrected_gps_time_us = jitter_correction.correct_offboard_timestamp_usec(msg.timestamp.usec, (timestamp_usec + NATIVE_TIME_OFFSET));
+        interim_state.last_corrected_gps_time_us = jitter_correction.correct_offboard_timestamp_usec(gps_message_timestamp_usec, timestamp_usec);
         interim_state.last_gps_time_ms = interim_state.last_corrected_gps_time_us/1000U;
-        interim_state.last_corrected_gps_time_us -= msg.timestamp.usec - msg.gnss_timestamp.usec;
+        interim_state.last_corrected_gps_time_us -= gps_message_timestamp_usec - msg.gnss_timestamp.usec;
         // this is also the time the message was received on the UART on other end.
         interim_state.corrected_timestamp_updated = true;
     } else {
-        interim_state.last_gps_time_ms = jitter_correction.correct_offboard_timestamp_usec(msg.timestamp.usec, timestamp_usec + NATIVE_TIME_OFFSET)/1000U;
+        interim_state.last_gps_time_ms = jitter_correction.correct_offboard_timestamp_usec(gps_message_timestamp_usec, timestamp_usec)/1000U;
     }
 
 #if GPS_PPS_EMULATION
     // Emulates a PPS signal, can be used to check how close are we to real GPS time
     static virtual_timer_t timeout_vt;
-    hal.gpio->pinMode(51, 1);
-    auto handle_timeout = [](void *arg)
+    hal.gpio->pinMode(51, HAL_GPIO_OUTPUT);
+    auto handle_timeout = [](virtual_timer_t*,void *)
     {
-        (void)arg;
         //we are called from ISR context
         chSysLockFromISR();
         hal.gpio->toggle(51);
@@ -455,7 +475,7 @@ void AP_GPS_DroneCAN::handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uin
 
     static uint64_t next_toggle, last_toggle;
     
-    next_toggle = (msg.timestamp.usec) + (1000000ULL - ((msg.timestamp.usec) % 1000000ULL));
+    next_toggle = (gps_message_timestamp_usec) + (1000000ULL - ((gps_message_timestamp_usec) % 1000000ULL));
 
     next_toggle += jitter_correction.get_link_offset_usec();
     if (next_toggle != last_toggle) {
@@ -661,6 +681,50 @@ void AP_GPS_DroneCAN::handle_relposheading_msg_trampoline(AP_DroneCAN *ap_dronec
 }
 #endif
 
+// precise time synchronisation
+void AP_GPS_DroneCAN::handle_timesync_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_protocol_GlobalTimeSync& msg)
+{
+    WITH_SEMAPHORE(_sem_registry);
+
+    AP_GPS_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id);
+    if (driver == nullptr) {
+        return;
+    }
+    bool packet_dropped = false;
+    // check that we didn't drop a packet,
+    // if we dropped the packet we can't calculate the timesync correction
+    // using this packet
+    if (transfer.transfer_id != ((driver->last_transfer_id+1)%32)) {
+        packet_dropped = true;
+    }
+    if (!packet_dropped && (driver->last_timesync_timestamp_us != 0) && (msg.previous_transmission_timestamp_usec != 0)) {
+        // It is expected that previous_transmission_timestamp_usec is set using
+        // gnss timestamp timescale on GPS module. For AP_Periph, that would be calculated
+        // using: Ublox Message Timestamp[GPS Time Domain] + DroneCAN transmit timestamp[Periph Time Domain] - Ublox Message Timestamp[Periph Time Domain]
+        // Also it is expected that Ublox Message Timestamp[Periph Time Domain] is kept using PPS signal synced with GPS NAV messages.
+        int64_t last_timesync_correction_us = driver->timesync_correction_us;
+        driver->timesync_correction_us = msg.previous_transmission_timestamp_usec - driver->last_timesync_timestamp_us;
+        int64_t clock_drift = driver->timesync_correction_us - last_timesync_correction_us;
+        if (abs(clock_drift) > 10000 && last_timesync_correction_us) {
+            // if the clock drift is more than 10ms, we might have a correction dropped
+#if HAL_LOGGING_ENABLED
+            AP::logger().Write_MessageF("GPS %d: timesync drop, o:%lld n:%lld", (unsigned int)(driver->state.instance + 1), driver->timesync_correction_us, last_timesync_correction_us);
+#endif
+            driver->timesync_correction_us = last_timesync_correction_us;
+        } else if (last_timesync_correction_us) {
+            // add to the clock drift to keep track of drift over time
+            driver->clock_drift += clock_drift;
+        }
+        driver->timesync_correction = true;
+        Debug("[%u]: correction: %lld/%lld\n",
+                      transfer.source_node_id,
+                      driver->timesync_correction_us,
+                      driver->clock_drift);
+    }
+    driver->last_timesync_timestamp_us = transfer.timestamp_usec;
+    driver->last_transfer_id = transfer.transfer_id;
+}
+
 bool AP_GPS_DroneCAN::do_config()
 {
     AP_DroneCAN *ap_dronecan = _detected_modules[_detected_module].ap_dronecan;
diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.h b/libraries/AP_GPS/AP_GPS_DroneCAN.h
index 10f6d71954a3c..50a3a86aefaab 100644
--- a/libraries/AP_GPS/AP_GPS_DroneCAN.h
+++ b/libraries/AP_GPS/AP_GPS_DroneCAN.h
@@ -56,6 +56,8 @@ class AP_GPS_DroneCAN : public AP_GPS_Backend {
     static void handle_moving_baseline_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_MovingBaselineData& msg);
     static void handle_relposheading_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_RelPosHeading& msg);
 #endif
+    static void handle_timesync_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_protocol_GlobalTimeSync& msg);
+
     static bool backends_healthy(char failure_msg[], uint16_t failure_msg_len);
     void inject_data(const uint8_t *data, uint16_t len) override;
 
@@ -70,6 +72,10 @@ class AP_GPS_DroneCAN : public AP_GPS_Backend {
     static bool instance_exists(const AP_DroneCAN* ap_dronecan);
 #endif
 
+    int64_t get_clock_drift() override {
+        return clock_drift;
+    }
+
 private:
 
     bool param_configured = true;
@@ -81,10 +87,19 @@ class AP_GPS_DroneCAN : public AP_GPS_Backend {
     };
     uint8_t cfg_step;
     bool requires_save_and_reboot;
+    bool timesync_correction;
+    uint64_t last_timesync_timestamp_us;
+    int64_t timesync_correction_us;
 
     // returns true once configuration has finished
     bool do_config(void);
 
+    bool get_lag(float &lag) const override {
+        return get_fix_lag(lag);
+    }
+
+    bool get_fix_lag(float &lag) const;
+
     void handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uint64_t timestamp_usec);
     void handle_aux_msg(const uavcan_equipment_gnss_Auxiliary& msg);
     void handle_heading_msg(const ardupilot_gnss_Heading& msg);
@@ -100,6 +115,9 @@ class AP_GPS_DroneCAN : public AP_GPS_Backend {
     static void give_registry();
     static AP_GPS_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id);
 
+    uint64_t last_gnss_timestamp_usec;
+
+    uint32_t _fix_lag_us;
     bool _new_data;
     AP_GPS::GPS_State interim_state;
 
@@ -149,6 +167,9 @@ class AP_GPS_DroneCAN : public AP_GPS_Backend {
         uint32_t last_send_ms;
         ByteBuffer *buf;
     } _rtcm_stream;
+
+    uint16_t last_transfer_id;
+    int64_t clock_drift;
 };
 
 #endif  // AP_GPS_DRONECAN_ENABLED
diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp
index f073201373b55..3d72a5536b17c 100644
--- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp
+++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp
@@ -53,6 +53,9 @@
 // debug VALGET/VALSET configuration
 #define UBLOX_CFG_DEBUGGING 0
 
+// debug Lag calculation
+#define UBLOX_LAG_DEBUGGING 0
+
 extern const AP_HAL::HAL& hal;
 
 #if UBLOX_DEBUGGING
@@ -94,6 +97,19 @@ extern const AP_HAL::HAL& hal;
  # define CFG_Debug(fmt, args ...)
 #endif
 
+#if UBLOX_LAG_DEBUGGING
+#if defined(HAL_BUILD_AP_PERIPH)
+ extern "C" {
+   void can_printf(const char *fmt, ...);
+ }
+ # define Lag_Debug(fmt, args ...)  do {can_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args);} while(0)
+#else
+    # define Lag_Debug(fmt, args ...)  do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
+#endif
+#else
+ # define Lag_Debug(fmt, args ...)
+#endif
+
 AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port, AP_GPS::GPS_Role _role) :
     AP_GPS_Backend(_gps, _state, _port),
     _next_message(STEP_PVT),
@@ -1578,6 +1594,9 @@ AP_GPS_UBLOX::_parse_gps(void)
         Debug("MSG_PVT");
 
         havePvtMsg = true;
+#ifdef HAL_GPIO_PPS
+        update_lag();
+#endif
         // position
         _check_new_itow(_buffer.pvt.itow);
         _last_pvt_itow = _buffer.pvt.itow;
@@ -2074,6 +2093,12 @@ AP_GPS_UBLOX::broadcast_configuration_failure_reason(void) const {
  */
 bool AP_GPS_UBLOX::get_lag(float &lag_sec) const
 {
+#ifdef HAL_GPIO_PPS
+    if (_lag_us) {
+        lag_sec = _lag_us * 1.0e-6f;
+        return true;
+    }
+#endif
     switch (_hardware_generation) {
     case UBLOX_UNKNOWN_HARDWARE_GENERATION:
         lag_sec = 0.22f;
@@ -2128,9 +2153,38 @@ void AP_GPS_UBLOX::Write_AP_Logger_Log_Startup_messages() const
 // uBlox specific check_new_itow(), handling message length
 void AP_GPS_UBLOX::_check_new_itow(uint32_t itow)
 {
+#ifdef HAL_GPIO_PPS
+    if (_lag_us) {
+        // we have calculated exact lag so no need to
+        // jitter correct gps message arrival
+        return;
+    }
+#endif
     check_new_itow(itow, _payload_length + sizeof(ubx_header) + 2);
 }
 
+#ifdef HAL_GPIO_PPS
+void AP_GPS_UBLOX::update_lag()
+{
+    // calculate lag
+    if (_last_pps_time_us != 0 &&
+        (state.status > AP_GPS::GPS_OK_FIX_3D) &&
+        !(_unconfigured_messages & CONFIG_TP5)) {
+        uint64_t last_message_rx_time = port->receive_time_constraint_us(_payload_length + sizeof(ubx_header) + 2);
+        _lag_us = uint32_t(last_message_rx_time & 0xFFFFFFFF) - _last_pps_time_us;
+        Lag_Debug("trx lag: %lu", AP_HAL::micros64() - last_message_rx_time);
+        Lag_Debug("lag:%lu", _lag_us);
+        _lag_us = constrain_uint32(_lag_us, 5000, 250000); // [5ms, 250ms]
+        _last_pps_time_us = 0;
+        // since we have an accurate lag measurement,
+        // we can set last_corrected_gps_time_us without
+        // jitter correction
+        state.last_corrected_gps_time_us = last_message_rx_time;
+        state.corrected_timestamp_updated = true;
+    }
+}
+#endif
+
 // support for retrieving RTCMv3 data from a moving baseline base
 bool AP_GPS_UBLOX::get_RTCMV3(const uint8_t *&bytes, uint16_t &len)
 {
diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h
index cbf21f925dddb..d4b6e44ed535d 100644
--- a/libraries/AP_GPS/AP_GPS_UBLOX.h
+++ b/libraries/AP_GPS/AP_GPS_UBLOX.h
@@ -851,6 +851,8 @@ class AP_GPS_UBLOX : public AP_GPS_Backend
 #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;
+    void update_lag();
+    uint32_t _lag_us;
 #endif
 
     // status of active configuration for a role
diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp
index f6d08fc761705..6d84f4dcf085b 100644
--- a/libraries/AP_GPS/GPS_Backend.cpp
+++ b/libraries/AP_GPS/GPS_Backend.cpp
@@ -245,11 +245,7 @@ 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 (_last_pps_time_us != 0 && (state.status >= AP_GPS::GPS_OK_FIX_2D)) {
-            // pps is only reliable when we have some sort of GPS fix
-            uart_us = _last_pps_time_us;
-            _last_pps_time_us = 0;
-        } else if (port) {
+        if (port) {
             uart_us = port->receive_time_constraint_us(msg_length);
         } else {
             uart_us = AP_HAL::micros64();
@@ -326,6 +322,7 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(float reported_heading_deg, const
 bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state, const float reported_heading_deg, const float reported_distance, const float reported_D) {
     constexpr float minimum_antenna_seperation = 0.05; // meters
     constexpr float permitted_error_length_pct = 0.2;  // percentage
+
 #if HAL_LOGGING_ENABLED || AP_AHRS_ENABLED
     float min_D = 0.0f;
     float max_D = 0.0f;
@@ -402,6 +399,8 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state,
                 goto bad_yaw;
             }
         }
+#else
+    (void)lag;
 #endif // AP_AHRS_ENABLED
 
         {
diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h
index 2fdae674d9ba4..c54610531aa7f 100644
--- a/libraries/AP_GPS/GPS_Backend.h
+++ b/libraries/AP_GPS/GPS_Backend.h
@@ -105,6 +105,8 @@ class AP_GPS_Backend
 
     virtual bool get_error_codes(uint32_t &error_codes) const { return false; }
 
+    virtual int64_t get_clock_drift() { return 0; }
+
     // return iTOW of last message, or zero if not supported
     uint32_t get_last_itow_ms(void) const;
 
@@ -113,6 +115,8 @@ class AP_GPS_Backend
         return gps.option_set(option);
     }
 
+    uint64_t get_last_pps_time_us() const { return _last_pps_time_us; }
+
 protected:
     AP_HAL::UARTDriver *port;           ///< UART we are attached to
     AP_GPS &gps;                        ///< access to frontend (for parameters)
diff --git a/libraries/AP_GPS/LogStructure.h b/libraries/AP_GPS/LogStructure.h
index 5ed1d2794693d..112783671ed1f 100644
--- a/libraries/AP_GPS/LogStructure.h
+++ b/libraries/AP_GPS/LogStructure.h
@@ -64,8 +64,10 @@ struct PACKED log_GPS {
 // @Field: SMS: time since system startup this sample was taken
 // @Field: Delta: system time delta between the last two reported positions
 // @Field: Und: Undulation
-// @Field: RTCMFU: RTCM fragments used
-// @Field: RTCMFD: RTCM fragments discarded
+// @Field: RFU: RTCM fragments used
+// @Field: RFD: RTCM fragments discarded
+// @Field: lag: GPS lag
+// @Field: C: clock drift
 struct PACKED log_GPA {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -81,6 +83,8 @@ struct PACKED log_GPA {
     float undulation;
     uint16_t rtcm_fragments_used;
     uint16_t rtcm_fragments_discarded;
+    float lag;
+    int64_t clock_drift;
 };
 
 /*
@@ -207,7 +211,7 @@ struct PACKED log_GPS_RAWS {
     { LOG_GPS_MSG, sizeof(log_GPS), \
       "GPS",  "QBBIHBcLLeffffB", "TimeUS,I,Status,GMS,GWk,NSats,HDop,Lat,Lng,Alt,Spd,GCrs,VZ,Yaw,U", "s#-s-S-DUmnhnh-", "F--C-0BGGB000--" , true }, \
     { LOG_GPA_MSG,  sizeof(log_GPA), \
-      "GPA",  "QBCCCCfBIHfHH", "TimeUS,I,VDop,HAcc,VAcc,SAcc,YAcc,VV,SMS,Delta,Und,RTCMFU,RTCMFD", "s#-mmnd-ssm--", "F-BBBB0-CC0--" , true }, \
+      "GPA",  "QBCCCCfBIHfHHfq", "TimeUS,I,VDop,HAcc,VAcc,SAcc,YAcc,VV,SMS,Delta,Und,RFU,RFD,lag,C", "s#mmmnd-ssm--ss", "F-BBBB0-CC0--0F" , true }, \
     { LOG_GPS_UBX1_MSG, sizeof(log_Ubx1), \
       "UBX1", "QBHBBHI",  "TimeUS,Instance,noisePerMS,jamInd,aPower,agcCnt,config", "s#-----", "F------"  , true }, \
     { LOG_GPS_UBX2_MSG, sizeof(log_Ubx2), \
diff --git a/libraries/AP_HAL/CANIface.h b/libraries/AP_HAL/CANIface.h
index 1b2e88fb33f23..eed8dc7dd0b87 100644
--- a/libraries/AP_HAL/CANIface.h
+++ b/libraries/AP_HAL/CANIface.h
@@ -264,10 +264,17 @@ class AP_HAL::CANIface
     // register a frame callback function
     virtual bool register_frame_callback(FrameCb cb);
 
+    // get timestamp of last packet sent with matching mask and value
+    virtual uint64_t get_tracked_tx_timestamp() {
+        return 0;
+    }
+
+    // set mask and value on packet id to track tx timestamp for
+    virtual void set_track_tx_timestamp(uint32_t mask, uint32_t value) {}
+
 protected:
     virtual int8_t get_iface_num() const = 0;
     virtual bool add_to_rx_queue(const CanRxItem &rx_item) = 0;
-
     FrameCb frame_callback;
     uint32_t bitrate_;
     OperatingMode mode_;
diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp
index cc4001940a4ec..f45676d4cfb79 100644
--- a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp
+++ b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp
@@ -838,6 +838,9 @@ void CANIface::handleTxCompleteInterrupt(const uint64_t timestamp_us)
                     stats.fdf_tx_success++;
                 }
                 pending_tx_[i].pushed = true;
+                if ((pending_tx_[i].frame.id & tracked_tx_ts_mask) == tracked_tx_ts_value) {
+                    tracked_tx_timestamp_us = timestamp_us;
+                }
             } else {
                 continue;
             }
diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.h b/libraries/AP_HAL_ChibiOS/CANFDIface.h
index fb6a9719c7fcb..8453be94ae6e1 100644
--- a/libraries/AP_HAL_ChibiOS/CANFDIface.h
+++ b/libraries/AP_HAL_ChibiOS/CANFDIface.h
@@ -123,6 +123,11 @@ class ChibiOS::CANIface : public AP_HAL::CANIface
 
     const uint8_t self_index_;
 
+    // track tx timestamps
+    uint32_t tracked_tx_ts_mask;
+    uint32_t tracked_tx_ts_value;
+    uint64_t tracked_tx_timestamp_us;
+
     bool computeTimings(uint32_t target_bitrate, Timings& out_timings) const;
     bool computeFDTimings(uint32_t target_bitrate, Timings& out_timings) const;
 
@@ -253,6 +258,17 @@ class ChibiOS::CANIface : public AP_HAL::CANIface
     // register. Indexed by locical interface number
     static constexpr CanType* const Can[HAL_NUM_CAN_IFACES] = { HAL_CAN_BASE_LIST };
 
+    // set mask and value on packet id to track tx timestamp for
+    void set_track_tx_timestamp(uint32_t mask, uint32_t value) override {
+        tracked_tx_ts_mask = mask;
+        tracked_tx_ts_value = value;
+    }
+
+    // get timestamp of last packet sent with matching mask and value
+    uint64_t get_tracked_tx_timestamp() override {
+        return tracked_tx_timestamp_us;
+    }
+
 protected:
     bool add_to_rx_queue(const CanRxItem &rx_item) override {
         return rx_queue_.push(rx_item);
diff --git a/libraries/AP_HAL_ChibiOS/CANIface.h b/libraries/AP_HAL_ChibiOS/CANIface.h
index 363c002d491ec..fca0f830a9014 100644
--- a/libraries/AP_HAL_ChibiOS/CANIface.h
+++ b/libraries/AP_HAL_ChibiOS/CANIface.h
@@ -112,6 +112,11 @@ class ChibiOS::CANIface : public AP_HAL::CANIface
 
     const uint8_t self_index_;
 
+    // track tx timestamps
+    uint32_t tracked_tx_ts_mask;
+    uint32_t tracked_tx_ts_value;
+    uint64_t tracked_tx_timestamp_us;
+
     bool computeTimings(uint32_t target_bitrate, Timings& out_timings);
 
     void setupMessageRam(void);
@@ -238,6 +243,17 @@ class ChibiOS::CANIface : public AP_HAL::CANIface
     // CAN Peripheral register structure
     static constexpr bxcan::CanType* const Can[HAL_NUM_CAN_IFACES] = { HAL_CAN_BASE_LIST };
 
+    // get timestamp of last packet sent with matching mask and value
+    uint64_t get_tracked_tx_timestamp() override {
+        return tracked_tx_timestamp_us;
+    }
+
+    // set mask and value on packet id to track tx timestamp for
+    void set_track_tx_timestamp(uint32_t mask, uint32_t value) override {
+        tracked_tx_ts_mask = mask;
+        tracked_tx_ts_value = value;
+    }
+
 protected:
     bool add_to_rx_queue(const CanRxItem &rx_item) override {
         return rx_queue_.push(rx_item);
diff --git a/libraries/AP_HAL_ChibiOS/CanIface.cpp b/libraries/AP_HAL_ChibiOS/CanIface.cpp
index f6704b6438d09..75a28efcdea51 100644
--- a/libraries/AP_HAL_ChibiOS/CanIface.cpp
+++ b/libraries/AP_HAL_ChibiOS/CanIface.cpp
@@ -500,6 +500,9 @@ void CANIface::handleTxMailboxInterrupt(uint8_t mailbox_index, bool txok, const
 #if !defined(HAL_BOOTLOADER_BUILD)
         stats.last_transmit_us = timestamp_us;
 #endif
+        if ((txi.frame.id & tracked_tx_ts_mask) == tracked_tx_ts_value) {
+            tracked_tx_timestamp_us = timestamp_us;
+        }
     }
 }
 
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Here4AP/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Here4AP/hwdef.dat
index d91a47cd1ee4a..e3dfd4041cd40 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/Here4AP/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/Here4AP/hwdef.dat
@@ -122,9 +122,9 @@ define GPS_MOVING_BASELINE 1
 define HAL_PERIPH_GPS_PORT_DEFAULT 3
 
 #PD11 GPS_TP1 OUTPUT HIGH GPIO(5)
-#PD7  GPS_PPS INPUT FLOATING GPIO(6)
-#define CONFIGURE_PPS_PIN TRUE
-#define HAL_GPIO_PPS 6
+PD7  GPS_PPS INPUT FLOATING GPIO(6)
+define CONFIGURE_PPS_PIN TRUE
+define HAL_GPIO_PPS 6
 
 # GPS enable pin
 # PD13 GPS_ENABLE OUTPUT HIGH GPIO(20)
@@ -132,3 +132,5 @@ define HAL_PERIPH_GPS_PORT_DEFAULT 3
 # for ProfiLed we need RC out and notify
 define HAL_PERIPH_ENABLE_RC_OUT
 define HAL_PERIPH_ENABLE_NOTIFY
+
+define HAL_PERIPH_ENABLE_GLOBALTIMESYNC 1
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c
index f1e510ccebd07..e2881f68727e1 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c
@@ -73,6 +73,12 @@ static uint32_t system_time_u32_us(void)
 static uint32_t get_systime_us32(void)
 {
     uint32_t now = system_time_u32_us();
+#ifdef AP_INDUCE_PPM_OFFSET
+    // Please note that this will induce incorrect
+    // clock drift after 71 minutes, use hrt_micros64()
+    // as "now" here if you want to avoid that
+    now += (((uint64_t)now) * AP_INDUCE_PPM_OFFSET) / 1000000U;
+#endif
 #ifdef AP_BOARD_START_TIME
     now += AP_BOARD_START_TIME;
 #endif
@@ -90,6 +96,10 @@ static uint64_t hrt_micros64I(void)
 #if CH_CFG_ST_FREQUENCY != 1000000U
     ret *= 1000000U/CH_CFG_ST_FREQUENCY;
 #endif
+#ifdef AP_INDUCE_PPM_OFFSET
+    // induce clock drift
+    ret += (ret * AP_INDUCE_PPM_OFFSET) / 1000000U;
+#endif
 #ifdef AP_BOARD_START_TIME
     ret += AP_BOARD_START_TIME;
 #endif
diff --git a/libraries/AP_HAL_ChibiOS/system.cpp b/libraries/AP_HAL_ChibiOS/system.cpp
index 09cb666821500..b63d833eba5b8 100644
--- a/libraries/AP_HAL_ChibiOS/system.cpp
+++ b/libraries/AP_HAL_ChibiOS/system.cpp
@@ -353,7 +353,18 @@ __FASTRAMFUNC__ uint32_t micros()
 {
 #if CH_CFG_ST_RESOLUTION == 32 && CH_CFG_ST_FREQUENCY==1000000U
     // special case optimisation for 32 bit timers
-#ifdef AP_BOARD_START_TIME
+#if defined(AP_INDUCE_PPM_OFFSET)
+    uint64_t now = st_lld_get_counter();
+#if defined(AP_BOARD_START_TIME)
+    now += AP_BOARD_START_TIME;
+#else
+    // Please note that this will induce incorrect
+    // clock drift after 71 minutes, use hrt_micros64()
+    // as "now" here if you want to avoid that
+    now += (now * AP_INDUCE_PPM_OFFSET) / 1000000U;
+    return now & 0xFFFFFFFF;
+#endif
+#elif defined(AP_BOARD_START_TIME)
     return st_lld_get_counter() + AP_BOARD_START_TIME;
 #else
     return st_lld_get_counter();
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
index 1f60ae65f932c..ce7e42d83ab59 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
@@ -174,7 +174,8 @@ void NavEKF3_core::Log_Write_XKF4(uint64_t time_us) const
         timeouts : timeoutStatus,
         solution : solutionStatus.value,
         gps : gpsCheckStatus.value,
-        primary : frontend->getPrimaryCoreIndex()
+        primary : frontend->getPrimaryCoreIndex(),
+        gps_time_delta_ms : gps_time_delta_ms
     };
     AP::logger().WriteBlock(&pkt4, sizeof(pkt4));
 }
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
index 9ebe777515a84..996d012d2dd72 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
@@ -576,6 +576,13 @@ void NavEKF3_core::readGpsData()
     gps.get_lag(selected_gps, gps_delay_sec);
     gpsDataNew.time_ms = lastTimeGpsReceived_ms - (uint32_t)(gps_delay_sec * 1000.0f);
 
+    if (last_gpsdata_time_ms && gpsDataNew.time_ms &&
+        (last_gpsdata_time_ms != gpsDataNew.time_ms)) {
+        gps_time_delta_ms = gpsDataNew.time_ms - last_gpsdata_time_ms;
+    } else {
+        gps_time_delta_ms = 0;
+    }
+    last_gpsdata_time_ms = gpsDataNew.time_ms;
     // Correct for the average intersampling delay due to the filter updaterate
     gpsDataNew.time_ms -= localFilterTimeStep_ms/2;
 
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
index 8ba964b7e88fd..9be28bfac3677 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
@@ -1200,7 +1200,8 @@ class NavEKF3_core : public NavEKF_core_common
     uint32_t firstInitTime_ms;      // First time the initialise function was called (msec)
     uint32_t lastInitFailReport_ms; // Last time the buffer initialisation failure report was sent (msec)
     ftype tiltErrorVariance;        // variance of the angular uncertainty measured perpendicular to the vertical (rad^2)
-
+    int32_t gps_time_delta_ms; // time delta between GPS time measurements (msec)
+    uint32_t last_gpsdata_time_ms; // time of last GPS time measurement (msec)
     // variables used to calculate a vertical velocity that is kinematically consistent with the vertical position
     struct {
         ftype pos;
diff --git a/libraries/AP_NavEKF3/LogStructure.h b/libraries/AP_NavEKF3/LogStructure.h
index 8e124462fcf0d..b77b2f8ef7953 100644
--- a/libraries/AP_NavEKF3/LogStructure.h
+++ b/libraries/AP_NavEKF3/LogStructure.h
@@ -189,6 +189,7 @@ struct PACKED log_XKF3 {
 // @Field: SS: Filter solution status
 // @Field: GPS: Filter GPS status
 // @Field: PI: Primary core index
+// @Field: GDT: GPS sample time delta
 struct PACKED log_XKF4 {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -206,6 +207,7 @@ struct PACKED log_XKF4 {
     uint32_t solution;
     uint16_t gps;
     int8_t primary;
+    int32_t gps_time_delta_ms; 
 };
 
 
@@ -431,7 +433,7 @@ struct PACKED log_XKV {
     { LOG_XKF3_MSG, sizeof(log_XKF3), \
       "XKF3","QBcccccchhhccff","TimeUS,C,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT,RErr,ErSc", "s#nnnmmmGGGd?--", "F-BBBBBBCCCBB00" , true }, \
     { LOG_XKF4_MSG, sizeof(log_XKF4), \
-      "XKF4","QBcccccfffHBIHb","TimeUS,C,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI", "s#------mm-----", "F-------??-----" , true }, \
+      "XKF4","QBcccccfffHBIHbi","TimeUS,C,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI,GDT", "s#------mm-----s", "F-------??-----C" , true }, \
     { LOG_XKF5_MSG, sizeof(log_XKF5), \
       "XKF5","QBBhhhcccCCfff","TimeUS,C,NI,FIX,FIY,AFI,HAGL,offset,RI,rng,Herr,eAng,eVel,ePos", "s#----m???mrnm", "F-----BBBBB000" , true }, \
     { LOG_XKFD_MSG, sizeof(log_XKFD), \