microRTPS: add timesync handshake

msg/templates/uorb_microcdr/microRTPS_client.cpp.em

@@ -78,6 +78,9 @@ receive_base_types = [s.short_name for idx, s in enumerate(spec) if scope[idx] =
 
 void* send(void *data);
 
+uint8_t last_remote_msg_seq = 0;
+uint8_t last_msg_seq = 0;
+
 @[if send_topics]@
 void* send(void* /*unused*/)
 {
@@ -106,11 +109,16 @@ void* send(void* /*unused*/)
         @(send_base_types[idx])_s @(topic)_data;
         if (@(topic)_sub.update(&@(topic)_data)) {
 @[if topic == 'Timesync' or topic == 'timesync']@
-                if (@(topic)_data.tc1 == 0) {
-                        @(topic)_data.seq++;
-                        @(topic)_data.tc1 = hrt_absolute_time() * 1000ULL;
-                        @(topic)_data.ts1 = @(topic)_data.ts1;
-                }
+            if(@(topic)_data.sys_id == 0 && @(topic)_data.seq != last_remote_msg_seq && @(topic)_data.tc1 == 0) {
+                last_remote_msg_seq = @(topic)_data.seq;
+
+                @(topic)_data.timestamp = hrt_absolute_time();
+                @(topic)_data.sys_id = 1;
+                @(topic)_data.seq = last_msg_seq;
+                @(topic)_data.tc1 = hrt_absolute_time() * 1000ULL;
+                @(topic)_data.ts1 = @(topic)_data.ts1;
+
+                last_msg_seq++;
 @[end if]@
                 // copy raw data into local buffer. Payload is shifted by header length to make room for header
                 serialize_@(send_base_types[idx])(&writer, &@(topic)_data, &data_buffer[header_length], &length);
@@ -119,6 +127,9 @@ void* send(void* /*unused*/)
                     total_sent += read;
                     ++sent;
                 }
+@[if topic == 'Timesync' or topic == 'timesync']@
+            }
+@[end if]@
         }
 @[end for]@
         px4_usleep(_options.sleep_ms * 1000);

msg/templates/urtps/RtpsTopics.cpp.em

@@ -124,6 +124,9 @@ void RtpsTopics::publish(uint8_t topic_ID, char data_buffer[], size_t len)
 @[    end if]@
             // apply timestamp offset
             _timesync->applyOffset(st.timestamp());
+@[    if topic == 'Timesync' or topic == 'timesync']@
+            if (st.sys_id() == 1)
+@[    end if]@
             _@(topic)_pub.publish(&st);
         }
         break;

msg/templates/urtps/microRTPS_timesync.cpp.em

@@ -71,7 +71,8 @@ TimeSync::TimeSync()
       _skew_ns_per_sync(0.0),
       _num_samples(0),
       _request_reset_counter(0),
-      _last_msg_seq(0)
+      _last_msg_seq(0),
+      _last_remote_msg_seq(0)
 {}
 
 TimeSync::~TimeSync() { stop(); }
@@ -198,34 +199,14 @@ void TimeSync::processTimesyncMsg(const @(package)::msg::Timesync* msg) {
 void TimeSync::processTimesyncMsg(const timesync* msg) {
 @[    end if]@
 @[end if]@
-	if (msg->seq() == _last_msg_seq) return;
-	_last_msg_seq = msg->seq();
+	if (msg->sys_id() == 1 && msg->seq() != _last_remote_msg_seq && msg->tc1() > 0) {
+                _last_remote_msg_seq = msg->seq();
 
-	int64_t now_time = getSystemMonoNanos();
+                int64_t now_time = getSystemMonoNanos();
 
-	if (msg->tc1() == 0) {
-@[if 1.5 <= fastrtpsgen_version <= 1.7]@
-@[    if ros2_distro]@
-		@(package)::msg::dds_::Timesync_ reply = (*msg);
-@[    else]@
-		timesync_ reply = (*msg);
-@[    end if]@
-@[else]@
-@[    if ros2_distro]@
-		@(package)::msg::Timesync reply = (*msg);
-@[    else]@
-		timesync reply = (*msg);
-@[    end if]@
-@[end if]@
-		reply.timestamp() = getSystemTime();
-		reply.seq() = _last_msg_seq;
-		reply.tc1() = now_time;
-
-		_timesync_pub.publish(&reply);
-
-	} else if (msg->tc1() > 0) {
-		bool added = addMeasurement(msg->ts1(), msg->tc1(), now_time);
+	        bool added = addMeasurement(msg->ts1(), msg->tc1(), now_time);
 		if (added && _notifyNewOffset) _notifyNewOffset(_offset_ns);
+                // std::cout << "Offset: " << _offset_ns << std::endl;
 	}
 }
 
@@ -246,7 +227,9 @@ timesync TimeSync::newTimesyncMsg() {
 	timesync msg{};
 @[    end if]@
 @[end if]@
-	msg.timestamp() = getSystemTime();
+	msg.timestamp() = getMonoTime();
+	applyOffset(msg.timestamp());
+	msg.sys_id() = 0;
 	msg.seq() = _last_msg_seq;
 	msg.tc1() = 0;
 	msg.ts1() = getSystemMonoNanos();

msg/templates/urtps/microRTPS_timesync.h.em

@@ -102,7 +102,7 @@ public:
 	void setNewOffsetCB(std::function<void(int64_t)> callback);
 
 	/**
-	 * Returns clock monotonic time in nanoseconds
+	 * Get clock monotonic time (raw) in nanoseconds
 	 */
 	inline int64_t getSystemMonoNanos() {
 		timespec t;
@@ -111,12 +111,12 @@ public:
 	}
 
 	/**
-	 * Returns system realtime in seconds
+	 * Get system monotonic time in microseconds
 	 */
-	inline uint64_t getSystemTime() {
+	inline uint64_t getMonoTime() {
 		timespec t;
-		clock_gettime(CLOCK_REALTIME, &t);
-		return (uint64_t)t.tv_sec + t.tv_nsec;
+		clock_gettime(CLOCK_MONOTONIC, &t);
+		return (t.tv_sec * 1000000000ll + t.tv_nsec) / 1000ULL;
 	}
 
 	bool addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t local_t3_ns);
@@ -143,7 +143,7 @@ public:
 @[    end if]@
 @[end if]@
 
-	inline void applyOffset(uint64_t &timestamp) { timestamp += _offset_ns.load(); }
+	inline void applyOffset(uint64_t timestamp) { timestamp += _offset_ns.load(); }
 
 private:
 	std::atomic<int64_t> _offset_ns;
@@ -152,6 +152,7 @@ private:
 
 	int32_t _request_reset_counter;
 	uint8_t _last_msg_seq;
+	uint8_t _last_remote_msg_seq;
 
 @[if ros2_distro]@
 	Timesync_Publisher _timesync_pub;

msg/timesync.msg

@@ -1,4 +1,5 @@
 uint64 timestamp		# time since system start (microseconds)
+uint8 sys_id                    # id of the origin system
 uint8 seq                       # timesync msg sequence
 int64 tc1                       # time sync timestamp 1
 int64 ts1                       # time sync timestamp 2