From 2c9ee6ebbdade04d221216f4564e38095955e754 Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Wed, 31 Jan 2024 12:23:59 +1100
Subject: [PATCH] AP_Periph: add support for Global Time Sync

---
 Tools/AP_Periph/AP_Periph.h |  3 +++
 Tools/AP_Periph/can.cpp     | 11 +++++++++++
 Tools/AP_Periph/gps.cpp     | 35 +++++++++++++++++++++++++++++++++++
 3 files changed, 49 insertions(+)

diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h
index 424e341c1c8c14..071d04350ba811 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 1c658463a6e84a..0c59ea273af9b0 100644
--- a/Tools/AP_Periph/can.cpp
+++ b/Tools/AP_Periph/can.cpp
@@ -1624,6 +1624,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);
         }
     }
 
@@ -1739,6 +1740,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 029676f26d1b8a..d0dc935feed30c 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