From f668d0dc5728d80a29b6550fccf5df0839ab523a Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Wed, 16 May 2018 17:17:25 +1000
Subject: [PATCH] GCS_MAVLink: enhance TIMESYNC handling

GCS_MAVLink: use de-jittered timestamp for our timesync responses

GCS_MAVLink: periodically send timesync request packets

Currently no use is made of the result of these requests, however,
having the result present in telemetry logs will be useful data.
---
 libraries/GCS_MAVLink/GCS.h          | 11 +++++
 libraries/GCS_MAVLink/GCS_Common.cpp | 74 ++++++++++++++++++++++++++--
 2 files changed, 81 insertions(+), 4 deletions(-)

diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index 77d305f432..d4ed96f95c 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -320,7 +320,18 @@ protected:
     void handle_device_op_read(mavlink_message_t *msg);
     void handle_device_op_write(mavlink_message_t *msg);
 
+    void send_timesync();
+    // returns the time a timesync message was most likely received:
+    uint64_t timesync_receive_timestamp_ns() const;
+    // returns a timestamp suitable for packing into the ts1 field of TIMESYNC:
+    uint64_t timesync_timestamp_ns() const;
     void handle_timesync(mavlink_message_t *msg);
+    struct {
+        int64_t sent_ts1;
+        uint32_t last_sent_ms;
+        const uint16_t interval_ms = 10000;
+    }  _timesync_request;
+
     void handle_statustext(mavlink_message_t *msg);
 
     bool telemetry_delayed() const;
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index c681c2a78f..79a4f3705c 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -939,12 +939,22 @@ GCS_MAVLINK::update(uint32_t max_time_us)
         }
     }
 
+    const uint32_t tnow = AP_HAL::millis();
+
+    // send a timesync message every 10 seconds; this is for data
+    // collection purposes
+    if (tnow - _timesync_request.last_sent_ms > _timesync_request.interval_ms) {
+        if (HAVE_PAYLOAD_SPACE(chan, TIMESYNC)) {
+            send_timesync();
+            _timesync_request.last_sent_ms = tnow;
+        }
+    }
+
     if (!waypoint_receiving) {
         hal.util->perf_end(_perf_update);    
         return;
     }
 
-    uint32_t tnow = AP_HAL::millis();
     uint32_t wp_recv_time = 1000U + (stream_slowdown*20);
 
     // stop waypoint receiving if timeout
@@ -1791,6 +1801,22 @@ MAV_RESULT GCS_MAVLINK::handle_rc_bind(const mavlink_command_long_t &packet)
     return MAV_RESULT_ACCEPTED;
 }
 
+uint64_t GCS_MAVLINK::timesync_receive_timestamp_ns() const
+{
+    uint64_t ret = _port->receive_time_constraint_us(PAYLOAD_SIZE(chan, TIMESYNC));
+    if (ret == 0) {
+        ret = AP_HAL::micros64();
+    }
+    return ret*1000LL;
+}
+
+uint64_t GCS_MAVLINK::timesync_timestamp_ns() const
+{
+    // we add in our own system id try to ensure we only consider
+    // responses to our own timesync request messages
+    return AP_HAL::micros64()*1000LL + mavlink_system.sysid;
+}
+
 /*
   return a timesync request
   Sends back ts1 as received, and tc1 is the local timestamp in usec
@@ -1801,14 +1827,41 @@ void GCS_MAVLINK::handle_timesync(mavlink_message_t *msg)
     mavlink_timesync_t tsync;
     mavlink_msg_timesync_decode(msg, &tsync);
 
-    // ignore messages in which tc1 field (timestamp 1) has already been filled in
     if (tsync.tc1 != 0) {
+        // this is a response to a timesync request
+        if (tsync.ts1 != _timesync_request.sent_ts1) {
+            // we didn't actually send the request.... or it's a
+            // response to an ancient request...
+            return;
+        }
+        const uint64_t round_trip_time_us = (timesync_receive_timestamp_ns() - _timesync_request.sent_ts1)*0.001f;
+#if 0
+        gcs().send_text(MAV_SEVERITY_INFO,
+                        "timesync response sysid=%u (latency=%fms)",
+                        msg->sysid,
+                        round_trip_time_us*0.001f);
+#endif
+        DataFlash_Class *df = DataFlash_Class::instance();
+        if (df != nullptr) {
+            DataFlash_Class::instance()->Log_Write(
+                "TSYN",
+                "TimeUS,SysID,RTT",
+                "s-s",
+                "F-F",
+                "QBQ",
+                AP_HAL::micros64(),
+                msg->sysid,
+                round_trip_time_us
+                );
+        }
         return;
     }
 
-    // create new timesync struct with tc1 field as system time in nanoseconds
+    // create new timesync struct with tc1 field as system time in
+    // nanoseconds.  The client timestamp is as close as possible to
+    // the time we received the TIMESYNC message.
     mavlink_timesync_t rsync;
-    rsync.tc1 = AP_HAL::micros64() * 1000;
+    rsync.tc1 = timesync_receive_timestamp_ns();
     rsync.ts1 = tsync.ts1;
 
     // respond with a timesync message
@@ -1819,6 +1872,19 @@ void GCS_MAVLINK::handle_timesync(mavlink_message_t *msg)
         );
 }
 
+/*
+ * broadcast a timesync message.  We may get multiple responses to this request.
+ */
+void GCS_MAVLINK::send_timesync()
+{
+    _timesync_request.sent_ts1 = timesync_timestamp_ns();
+    mavlink_msg_timesync_send(
+        chan,
+        0,
+        _timesync_request.sent_ts1
+        );
+}
+
 void GCS_MAVLINK::handle_statustext(mavlink_message_t *msg)
 {
     DataFlash_Class *df = DataFlash_Class::instance();