GCS_MAVLink: only respond to timesync messages with empty tc1

This avoids the possibility of a timesync message storm
Also process as a common message because no vehicle information is required
Moved location of new method to bottom of cpp file

libraries/GCS_MAVLink/GCS.h

@@ -246,8 +246,6 @@ 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);
@@ -256,6 +254,8 @@ protected:
 
     void handle_device_op_read(mavlink_message_t *msg);
     void handle_device_op_write(mavlink_message_t *msg);
+
+    void handle_timesync(mavlink_message_t *msg);
     
 private:
 

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -761,32 +761,6 @@ 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
   return true if this is the last mission item, otherwise false
@@ -1854,6 +1828,35 @@ uint8_t GCS_MAVLINK::handle_rc_bind(const mavlink_command_long_t &packet)
     return MAV_RESULT_ACCEPTED;
 }
 
+/*
+  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)
+{
+    // decode incoming timesync message
+    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) {
+        return;
+    }
+
+    // create new timesync struct with tc1 field filled in
+    mavlink_timesync_t rsync;
+    rsync.tc1 = AP_HAL::micros64();
+    rsync.ts1 = tsync.ts1;
+
+    // respond with a timesync message
+    mavlink_msg_timesync_send(
+        chan,
+        rsync.tc1,
+        rsync.ts1
+        );
+}
+
+
 /*
   handle messages which don't require vehicle specific data
  */
@@ -1872,6 +1875,9 @@ void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg)
     case MAVLINK_MSG_ID_DEVICE_OP_WRITE:
         handle_device_op_write(msg);
         break;
+    case MAVLINK_MSG_ID_TIMESYNC:
+        handle_timesync(msg);
+        break;
     }
 }