AP_Periph: add support for Global Time Sync

bugobliterator · Feb 5, 2024 · 2c9ee6e · 2c9ee6e
1 parent d8692d7
commit 2c9ee6e
Show file tree

Hide file tree

Showing 3 changed files with 49 additions and 0 deletions.
diff --git 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
@@ -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
@@ -65,14 +65,49 @@ 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()) {
         return;
     }
     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