GCS_MAVLink: add TIMESYNC message handler

libraries/GCS_MAVLink/GCS.h

@@ -246,6 +246,8 @@ protected:
 
     void handle_gps_inject(const mavlink_message_t *msg, AP_GPS &gps);
 
+    void handle_timesync(mavlink_message_t *msg);
+
     void handle_common_message(mavlink_message_t *msg);
     void handle_log_message(mavlink_message_t *msg, DataFlash_Class &dataflash);
     void handle_setup_signing(const mavlink_message_t *msg);

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -761,6 +761,31 @@ void GCS_MAVLINK::handle_radio_status(mavlink_message_t *msg, DataFlash_Class &d
     }
 }
 
+/*
+  return a timesync request
+  Sends back ts1 as received, and tc1 is the local timestamp in usec
+ */
+void GCS_MAVLINK::handle_timesync(mavlink_message_t *msg)
+{
+    // Timestamp to return is usec since boot
+    uint64_t now = AP_HAL::micros64();
+    
+    // Decode incoming timesync message
+    mavlink_timesync_t tsync;
+    mavlink_msg_timesync_decode(msg, &tsync);
+
+    // Create new timestruct to return
+    mavlink_timesync_t rsync;
+    rsync.ts1 = tsync.ts1;
+    rsync.tc1 = now;
+
+    // Return a timesync message with updated tc1 field
+    mavlink_msg_timesync_send(
+        chan,
+        rsync.tc1,
+        rsync.ts1
+        );
+}
 
 /*
   handle an incoming mission item