GCS_MAVLink: added function for correcting offboard timestamps

libraries/GCS_MAVLink/GCS.h

@@ -485,6 +485,12 @@ private:
 
     void lock_channel(mavlink_channel_t chan, bool lock);
 
+    /*
+      correct an offboard timestamp in microseconds to a local time
+      since boot in milliseconds
+     */
+    uint32_t correct_offboard_timestamp_usec_to_ms(uint64_t offboard_usec);
+    
     mavlink_signing_t signing;
     static mavlink_signing_streams_t signing_streams;
     static uint32_t last_signing_save_ms;
@@ -503,6 +509,14 @@ private:
         uint32_t last_alternate_ms;
         bool active;
     } alternative;
+
+    // state associated with offboard transport lag correction
+    struct {
+        bool initialised;
+        int64_t link_offset_usec;
+        uint32_t min_sample_counter;
+        int64_t min_sample_us;
+    } lag_correction;
     
 };
 

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -1941,6 +1941,9 @@ void GCS_MAVLINK::handle_common_vision_position_estimate_data(const uint64_t use
                                                               const float pitch,
                                                               const float yaw)
 {
+    // correct offboard timestamp to be in local ms since boot
+    uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(usec);
+    
     // sensor assumed to be at 0,0,0 body-frame; need parameters for this?
     // or a new message 
     const Vector3f sensor_offset = {};
@@ -1953,7 +1956,6 @@ void GCS_MAVLINK::handle_common_vision_position_estimate_data(const uint64_t use
     attitude.from_euler(roll, pitch, yaw); // from_vector312?
     const float posErr = 0; // parameter required?
     const float angErr = 0; // parameter required?
-    const uint32_t timestamp_ms = usec * 0.001;
     const uint32_t reset_timestamp_ms = 0; // no data available
 
     AP::ahrs().writeExtNavData(sensor_offset,
@@ -2779,6 +2781,76 @@ void GCS_MAVLINK::data_stream_send(void)
     }
 }
 
+/*
+  correct an offboard timestamp in microseconds into a local timestamp
+  since boot in milliseconds. This is a transport lag correction function, and works by assuming two key things:
+
+   1) the data did not come from the future in our time-domain
+   2) the data is not older than max_lag_ms in our time-domain
+
+  It works by estimating the transport lag by looking for the incoming
+  packet that had the least lag, and converging on the offset that is
+  associated with that lag
+
+  Return a value in milliseconds since boot (for use by the EKF)
+ */
+uint32_t GCS_MAVLINK::correct_offboard_timestamp_usec_to_ms(uint64_t offboard_usec)
+{
+    const uint32_t max_lag_us = 500*1000UL;
+    uint64_t local_us = AP_HAL::micros64();
+    int64_t diff_us = int64_t(local_us) - int64_t(offboard_usec);
+
+    if (!lag_correction.initialised ||
+        diff_us < lag_correction.link_offset_usec) {
+        // this message arrived from the remote system with a
+        // timestamp that would imply the message was from the
+        // future. We know that isn't possible, so we adjust down the
+        // correction value
+        lag_correction.link_offset_usec = diff_us;
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+        printf("link_offset_usec=%lld\n", diff_us);
+#endif
+        lag_correction.initialised = true;
+    }
+
+    int64_t estimate_us = offboard_usec + lag_correction.link_offset_usec;
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    if (estimate_us > local_us) {
+        // this should be impossible, just check it under SITL
+        printf("msg from future %lld\n", estimate_us - local_us);
+    }
+#endif
+
+    if (estimate_us + max_lag_us < int64_t(local_us)) {
+        // this implies the message came from too far in the past. Clamp the lag estimate
+        // to assume the message had maximum lag
+        estimate_us = local_us - max_lag_us;
+        lag_correction.link_offset_usec = estimate_us - offboard_usec;
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+        printf("offboard timestammp too old %lld\n", local_us - estimate_us);
+#endif
+    }
+
+    if (lag_correction.min_sample_counter == 0) {
+        lag_correction.min_sample_us = diff_us;
+    }
+    lag_correction.min_sample_counter++;
+    if (diff_us < lag_correction.min_sample_us) {
+        lag_correction.min_sample_us = diff_us;
+    }
+    if (lag_correction.min_sample_counter == 200) {
+        // we have 200 samples of the transport lag. To
+        // account for long term clock drift we set the diff we will
+        // use in future to this value
+        lag_correction.link_offset_usec = lag_correction.min_sample_us;
+        lag_correction.min_sample_counter = 0;
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+        printf("new link_offset_usec=%lld\n", lag_correction.min_sample_us);
+#endif
+    }
+    
+    return estimate_us / 1000U;
+}
 
 GCS &gcs()
 {