AP_GPS: add support for synchronizing time using Global Time Sync

bugobliterator · Dec 9, 2023 · 830bb0b · 830bb0b
1 parent e4a2794
commit 830bb0b
Show file tree

Hide file tree

Showing 2 changed files with 37 additions and 9 deletions.
diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp
@@ -116,6 +116,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)
@@ -428,25 +432,29 @@ 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) {
+       // we have gps time synchronized even more accurately with the timesync message
+       gps_message_timestamp_usec = timestamp_usec + timesync_correction_us;
+    }
+    // fallback to using the timestamp from the message for correction
+    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 + NATIVE_TIME_OFFSET));
         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 + NATIVE_TIME_OFFSET)/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 +463,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) {
@@ -659,6 +667,21 @@ 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) {
+        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 + NATIVE_TIME_OFFSET);
+        }
+        driver->last_timesync_timestamp_us = transfer.timestamp_usec;
+        driver->timesync_correction = true;
+    }
+}
+
 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
@@ -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;
 
@@ -81,6 +83,9 @@ 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);