diff --git a/msg/templates/uorb_microcdr/microRTPS_client.cpp.em b/msg/templates/uorb_microcdr/microRTPS_client.cpp.em index 7723a59156..4c221ad892 100644 --- a/msg/templates/uorb_microcdr/microRTPS_client.cpp.em +++ b/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); diff --git a/msg/templates/urtps/RtpsTopics.cpp.em b/msg/templates/urtps/RtpsTopics.cpp.em index 554c40be08..1763d8979e 100644 --- a/msg/templates/urtps/RtpsTopics.cpp.em +++ b/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; diff --git a/msg/templates/urtps/microRTPS_timesync.cpp.em b/msg/templates/urtps/microRTPS_timesync.cpp.em index fc521120ce..0e10bd37ea 100644 --- a/msg/templates/urtps/microRTPS_timesync.cpp.em +++ b/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(); diff --git a/msg/templates/urtps/microRTPS_timesync.h.em b/msg/templates/urtps/microRTPS_timesync.h.em index e8cef660a3..a6896b5c1c 100644 --- a/msg/templates/urtps/microRTPS_timesync.h.em +++ b/msg/templates/urtps/microRTPS_timesync.h.em @@ -102,7 +102,7 @@ public: void setNewOffsetCB(std::function 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 ×tamp) { timestamp += _offset_ns.load(); } + inline void applyOffset(uint64_t timestamp) { timestamp += _offset_ns.load(); } private: std::atomic _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; diff --git a/msg/timesync.msg b/msg/timesync.msg index 754e6eb675..1104bd287c 100644 --- a/msg/timesync.msg +++ b/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