yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

microRTPS: timesync: apply offset atomically

Browse Source
This commit is contained in:
TSC21
2020-03-05 11:36:16 +00:00
committed by Nuno Marques
parent 170835f3f8
commit 441e6290eb
2 changed files with 8 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
msg/templates/urtps/RtpsTopics.cpp.em
View File

@@ -122,6 +122,8 @@ void RtpsTopics::publish(uint8_t topic_ID, char data_buffer[], size_t len)
@[ if topic == 'Timesync']@
_timesync->processTimesyncMsg(&st);
@[ end if]@
// apply timestamp offset
_timesync->applyOffset(st.timestamp());
_@(topic)_pub.publish(&st);
}
break;
@@ -156,6 +158,9 @@ bool RtpsTopics::getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr)
@(topic) msg = _@(topic)_sub.getMsg();
@[ end if]@
@[ end if]@
// apply timestamp offset
_timesync->applyOffset(msg.timestamp());
msg.serialize(scdr);
ret = true;
_@(topic)_sub.unlockMsg();

4
msg/templates/urtps/microRTPS_timesync.h
View File

@@ -90,8 +90,10 @@ public:
px4_msgs::msg::Timesync newTimesyncMsg();
inline void applyOffset(uint64_t &timestamp) { timestamp += _offset_ns.load(); }
private:
int64_t _offset_ns;
std::atomic<int64_t> _offset_ns;
int64_t _skew_ns_per_sync;
int64_t _num_samples;