Add support for calculating GPS Lag over DroneCAN

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

Tools/AP_Periph/can.cpp

@@ -1627,6 +1627,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);
         }
     }
 
@@ -1742,6 +1743,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();

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

libraries/AP_GPS/AP_GPS.cpp

@@ -598,6 +598,13 @@ uint64_t AP_GPS::last_message_epoch_usec(uint8_t instance) const
     return istate_time_to_epoch_ms(istate.time_week, drivers[instance]->get_last_itow_ms()) * 1000ULL;
 }
 
+uint64_t AP_GPS::last_pps_time_usec(uint8_t instance) const
+{
+    if (drivers[instance] == nullptr) {
+        return 0;
+    }
+    return  drivers[instance]->get_last_pps_time_us();
+}
 /*
   send some more initialisation string bytes if there is room in the
   UART transmit buffer
@@ -2288,11 +2295,16 @@ void AP_GPS::Write_GPS(uint8_t i)
 
     /* write auxiliary accuracy information as well */
     float hacc = 0, vacc = 0, sacc = 0;
-    float undulation = 0;
+    float undulation = 0, lag = 0;
+    int64_t clock_drift = 0;
     horizontal_accuracy(i, hacc);
     vertical_accuracy(i, vacc);
     speed_accuracy(i, sacc);
     get_undulation(i, undulation);
+    get_lag(i, lag);
+    if (drivers[i] != nullptr) {
+        clock_drift = drivers[i]->get_clock_drift();
+    }
     struct log_GPA pkt2{
         LOG_PACKET_HEADER_INIT(LOG_GPA_MSG),
         time_us       : time_us,
@@ -2307,7 +2319,9 @@ void AP_GPS::Write_GPS(uint8_t i)
         delta_ms      : last_message_delta_time_ms(i),
         undulation    : undulation,
         rtcm_fragments_used: rtcm_stats.fragments_used,
-        rtcm_fragments_discarded: rtcm_stats.fragments_discarded
+        rtcm_fragments_discarded: rtcm_stats.fragments_discarded,
+        lag           : lag,
+        clock_drift   : clock_drift
     };
     AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
 }

libraries/AP_GPS/AP_GPS.h

@@ -519,6 +519,11 @@ class AP_GPS
         return last_message_epoch_usec(primary_instance);
     }
 
+    uint64_t last_pps_time_usec(uint8_t instance) const;
+    uint64_t last_pps_time_usec() const {
+        return last_pps_time_usec(primary_instance);
+    }
+
     // convert GPS week and millis to unix epoch in ms
     static uint64_t istate_time_to_epoch_ms(uint16_t gps_week, uint32_t gps_ms);
 

libraries/AP_GPS/AP_GPS_DroneCAN.cpp

@@ -54,11 +54,6 @@ extern const AP_HAL::HAL& hal;
 
 #define LOG_TAG "GPS"
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-#define NATIVE_TIME_OFFSET (AP_HAL::micros64() - AP_HAL::micros64())
-#else
-#define NATIVE_TIME_OFFSET 0
-#endif
 AP_GPS_DroneCAN::DetectedModules AP_GPS_DroneCAN::_detected_modules[];
 HAL_Semaphore AP_GPS_DroneCAN::_sem_registry;
 
@@ -116,6 +111,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)
@@ -336,6 +335,18 @@ void AP_GPS_DroneCAN::handle_velocity(const float vx, const float vy, const floa
     }
 }
 
+bool AP_GPS_DroneCAN::get_fix_lag(float &lag) const
+{
+    if (_fix_lag_us) {
+        // we have gps message time synchronized using the timesync message
+        // which is driven by PPS. So we have accurate timestamp of 
+        // which timeslice the solution was calculated for
+        lag = _fix_lag_us/1000000.0f;
+        return true;
+    }
+    return AP_GPS_Backend::get_lag(lag);
+}
+
 void AP_GPS_DroneCAN::handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uint64_t timestamp_usec)
 {
     bool process = false;
@@ -428,25 +439,34 @@ 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 && msg.gnss_timestamp.usec) {
+        interim_state.last_corrected_gps_time_us = AP_HAL::micros64();
+        interim_state.last_gps_time_ms = interim_state.last_corrected_gps_time_us/1000U;
+        int32_t fix_lag_us = interim_state.last_corrected_gps_time_us - (msg.gnss_timestamp.usec - timesync_correction_us);
+        Debug("fix_lag[%d]: %ld\n",  _detected_modules[_detected_module].node_id, fix_lag_us);
+        interim_state.corrected_timestamp_updated = true;
+        if (fix_lag_us > 5000 && fix_lag_us < 250000 ) {
+            _fix_lag_us = fix_lag_us;
+        }
+        last_gnss_timestamp_usec = msg.gnss_timestamp.usec; // for use by update_relposned_lag
+    } else 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);
         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)/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 +475,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) {
@@ -661,6 +681,50 @@ 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) {
+        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 != ((driver->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.
+        int64_t last_timesync_correction_us = driver->timesync_correction_us;
+        driver->timesync_correction_us = msg.previous_transmission_timestamp_usec - driver->last_timesync_timestamp_us;
+        int64_t clock_drift = driver->timesync_correction_us - last_timesync_correction_us;
+        if (abs(clock_drift) > 10000 && last_timesync_correction_us) {
+            // if the clock drift is more than 10ms, we might have a correction dropped
+#if HAL_LOGGING_ENABLED
+            AP::logger().Write_MessageF("GPS %d: timesync drop, o:%lld n:%lld", (unsigned int)(driver->state.instance + 1), driver->timesync_correction_us, last_timesync_correction_us);
+#endif
+            driver->timesync_correction_us = last_timesync_correction_us;
+        } else if (last_timesync_correction_us) {
+            // add to the clock drift to keep track of drift over time
+            driver->clock_drift += clock_drift;
+        }
+        driver->timesync_correction = true;
+        Debug("[%u]: correction: %lld/%lld\n",
+                      transfer.source_node_id,
+                      driver->timesync_correction_us,
+                      driver->clock_drift);
+    }
+    driver->last_timesync_timestamp_us = transfer.timestamp_usec;
+    driver->last_transfer_id = transfer.transfer_id;
+}
+
 bool AP_GPS_DroneCAN::do_config()
 {
     AP_DroneCAN *ap_dronecan = _detected_modules[_detected_module].ap_dronecan;

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;
 
@@ -70,6 +72,10 @@ class AP_GPS_DroneCAN : public AP_GPS_Backend {
     static bool instance_exists(const AP_DroneCAN* ap_dronecan);
 #endif
 
+    int64_t get_clock_drift() override {
+        return clock_drift;
+    }
+
 private:
 
     bool param_configured = true;
@@ -81,10 +87,19 @@ 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);
 
+    bool get_lag(float &lag) const override {
+        return get_fix_lag(lag);
+    }
+
+    bool get_fix_lag(float &lag) const;
+
     void handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uint64_t timestamp_usec);
     void handle_aux_msg(const uavcan_equipment_gnss_Auxiliary& msg);
     void handle_heading_msg(const ardupilot_gnss_Heading& msg);
@@ -100,6 +115,9 @@ class AP_GPS_DroneCAN : public AP_GPS_Backend {
     static void give_registry();
     static AP_GPS_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id);
 
+    uint64_t last_gnss_timestamp_usec;
+
+    uint32_t _fix_lag_us;
     bool _new_data;
     AP_GPS::GPS_State interim_state;
 
@@ -149,6 +167,9 @@ class AP_GPS_DroneCAN : public AP_GPS_Backend {
         uint32_t last_send_ms;
         ByteBuffer *buf;
     } _rtcm_stream;
+
+    uint16_t last_transfer_id;
+    int64_t clock_drift;
 };
 
 #endif  // AP_GPS_DRONECAN_ENABLED