Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 19:32:36 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

microRTPS: timesync: properly apply offsets

Browse Source
This commit is contained in:
TSC21
2020-03-09 13:16:24 +00:00
committed by Nuno Marques
parent 67491b473c
commit 250b6b24ab
3 changed files with 57 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/templates/urtps/RtpsTopics.cpp.em
+13 -6
View File
@@ -121,13 +121,15 @@ void RtpsTopics::publish(uint8_t topic_ID, char data_buffer[], size_t len)
st.deserialize(cdr_des);
@[ if topic == 'Timesync' or topic == 'timesync']@
_timesync->processTimesyncMsg(&st);
if (st.sys_id() == 1) {
@[ end if]@
// apply timestamp offset
_timesync->applyOffset(st.timestamp());
@[ if topic == 'Timesync' or topic == 'timesync']@
if (st.sys_id() == 1)
@[ end if]@
_timesync->subtractOffset(st.timestamp());
_@(topic)_pub.publish(&st);
@[ if topic == 'Timesync' or topic == 'timesync']@
}
@[ end if]@
}
break;
@[end for]@
@@ -160,11 +162,16 @@ bool RtpsTopics::getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr)
@[ else]@
@(topic) msg = _@(topic)_sub.getMsg();
@[ end if]@
@[ end if]@
@[ if topic == 'Timesync' or topic == 'timesync']@
if (msg.sys_id() == 0) {
@[ end if]@
// apply timestamp offset
_timesync->applyOffset(msg.timestamp());
_timesync->addOffset(msg.timestamp());
msg.serialize(scdr);
@[ if topic == 'Timesync' or topic == 'timesync']@
}
@[ end if]@
ret = true;
_@(topic)_sub.unlockMsg();
}