AP_GPS: handle case where TimeSync message gets missed at Rx

bugobliterator · Dec 25, 2023 · 62c0f28 · 62c0f28
1 parent 1dcf967
commit 62c0f28
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 5 deletions.
diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp
@@ -712,13 +712,26 @@ void AP_GPS_DroneCAN::handle_timesync_msg_trampoline(AP_DroneCAN *ap_dronecan, c
     WITH_SEMAPHORE(_sem_registry);
 
     AP_GPS_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id);
-    if (driver != nullptr) {
-        if ((driver->last_timesync_timestamp_us != 0) && (msg.previous_transmission_timestamp_usec != 0)) {
-            driver->timesync_correction_us = msg.previous_transmission_timestamp_usec - driver->last_timesync_timestamp_us;
-        }
-        driver->last_timesync_timestamp_us = transfer.timestamp_usec;
+    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 != ((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.
+        driver->timesync_correction_us = msg.previous_transmission_timestamp_usec - (driver->last_timesync_timestamp_us);
         driver->timesync_correction = true;
     }
+    driver->last_timesync_timestamp_us = transfer.timestamp_usec;
+    driver->last_transfer_id = transfer.transfer_id;
 }
 
 bool AP_GPS_DroneCAN::do_config()

diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.h b/libraries/AP_GPS/AP_GPS_DroneCAN.h
@@ -171,6 +171,8 @@ class AP_GPS_DroneCAN : public AP_GPS_Backend {
         uint32_t last_send_ms;
         ByteBuffer *buf;
     } _rtcm_stream;
+
+    uint16_t last_transfer_id;
 };
 
 #endif  // AP_GPS_DRONECAN_ENABLED