From 49b767b24ead8cec768f7dc414db6b5e5a177f45 Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Tue, 18 Jan 2022 16:13:42 +0530
Subject: [PATCH] AP_GPS: add support for correcting GPS times while using
 DroneCAN GPS

---
 libraries/AP_GPS/AP_GPS_UAVCAN.cpp | 42 +++++++++++++++++++++++++++++-
 libraries/AP_GPS/GPS_Backend.h     |  4 +--
 2 files changed, 42 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp
index eec0b5e00e..4f0310d171 100644
--- a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp
+++ b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp
@@ -39,6 +39,8 @@
 
 #include <AP_BoardConfig/AP_BoardConfig.h>
 
+#define GPS_PPS_EMULATION 0
+
 extern const AP_HAL::HAL& hal;
 
 #define GPS_UAVCAN_DEBUGGING 0
@@ -68,6 +70,11 @@ UC_REGISTRY_BINDER(MovingBaselineDataCb, ardupilot::gnss::MovingBaselineData);
 UC_REGISTRY_BINDER(RelPosHeadingCb, ardupilot::gnss::RelPosHeading);
 #endif
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#define NATIVE_TIME_OFFSET (AP_HAL::micros64() - AP_HAL::native_micros64())
+#else
+#define NATIVE_TIME_OFFSET 0
+#endif
 AP_GPS_UAVCAN::DetectedModules AP_GPS_UAVCAN::_detected_modules[] = {0};
 HAL_Semaphore AP_GPS_UAVCAN::_sem_registry;
 
@@ -578,8 +585,41 @@ void AP_GPS_UAVCAN::handle_fix2_msg(const Fix2Cb &cb)
         // hdop from pdop. Some GPS modules don't provide the Aux message
         interim_state.hdop = interim_state.vdop = cb.msg->pdop * 100.0;
     }
+
+    if ((cb.msg->timestamp.usec > cb.msg->gnss_timestamp.usec) && (cb.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(cb.msg->timestamp.usec, (cb.msg->getUtcTimestamp().toUSec() + NATIVE_TIME_OFFSET));
+        interim_state.last_gps_time_ms = interim_state.last_corrected_gps_time_us/1000U;
+        interim_state.last_corrected_gps_time_us -= cb.msg->timestamp.usec - cb.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(cb.msg->timestamp.usec, cb.msg->getUtcTimestamp().toUSec() + 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)
+    {
+        (void)arg;
+        //we are called from ISR context
+        chSysLockFromISR();
+        hal.gpio->toggle(51);
+        chSysUnlockFromISR();
+    };
+
+    static uint64_t next_toggle, last_toggle;
     
-    interim_state.last_gps_time_ms = AP_HAL::millis();
+    next_toggle = (cb.msg->timestamp.usec) + (1000000ULL - ((cb.msg->timestamp.usec) % 1000000ULL));
+
+    next_toggle += jitter_correction.get_link_offset_usec();
+    if (next_toggle != last_toggle) {
+        chVTSet(&timeout_vt, chTimeUS2I(next_toggle - AP_HAL::micros64()), handle_timeout, nullptr);
+        last_toggle = next_toggle;
+    }
+#endif
 
     _new_data = true;
     if (!seen_message) {
diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h
index e485e6590d..6e6d16d84a 100644
--- a/libraries/AP_GPS/GPS_Backend.h
+++ b/libraries/AP_GPS/GPS_Backend.h
@@ -157,6 +157,7 @@ protected:
 
 protected:
     uint64_t _last_pps_time_us;
+    JitterCorrection jitter_correction;
 
 private:
     // itow from previous message
@@ -167,9 +168,6 @@ private:
     uint32_t _rate_ms;
     uint32_t _last_rate_ms;
     uint16_t _rate_counter;
-
-    JitterCorrection jitter_correction;
-
 #if AP_GPS_DEBUG_LOGGING_ENABLED
     struct {
         int fd = -1;