fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 23:33:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

microRTPS: timesync: add getters and setter to generalize interface

Browse Source
This commit is contained in:
TSC21
2020-03-11 15:13:07 +00:00
committed by Nuno Marques
parent 7612879ffd
commit 99f96437c3
4 changed files with 85 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/templates/urtps/microRTPS_timesync.cpp.em
+14 -14
View File
@@ -163,19 +163,19 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t
}
void TimeSync::processTimesyncMsg(timesync_msg_t * msg) {
if (msg->sys_id() == 1 && msg->seq() != _last_remote_msg_seq) {
_last_remote_msg_seq = msg->seq();
if (getMsgSysID(msg) == 1 && getMsgSeq(msg) != _last_remote_msg_seq) {
_last_remote_msg_seq = getMsgSeq(msg);
if (msg->tc1() > 0) {
if (!addMeasurement(msg->ts1(), msg->tc1(), getMonoRawTimeNSec())) {
if (getMsgTC1(msg) > 0) {
if (!addMeasurement(getMsgTS1(msg), getMsgTC1(msg), getMonoRawTimeNSec())) {
std::cerr << "Offset not updated" << std::endl;
}
} else if (msg->tc1() == 0) {
msg->timestamp() = getMonoTimeUSec();
msg->sys_id() = 0;
msg->seq()++;
msg->tc1() = getMonoRawTimeNSec();
} else if (getMsgTC1(msg) == 0) {
setMsgTimestamp(msg, getMonoTimeUSec());
setMsgSysID(msg, 0);
setMsgSeq(msg, getMsgSeq(msg) + 1);
setMsgTC1(msg, getMonoRawTimeNSec());
_timesync_pub.publish(msg);
}
@@ -185,11 +185,11 @@ void TimeSync::processTimesyncMsg(timesync_msg_t * msg) {
timesync_msg_t TimeSync::newTimesyncMsg() {
timesync_msg_t msg{};
msg.timestamp() = getMonoTimeUSec();
msg.sys_id() = 0;
msg.seq() = _last_msg_seq;
msg.tc1() = 0;
msg.ts1() = getMonoRawTimeNSec();
setMsgTimestamp(&msg, getMonoTimeUSec());
setMsgSysID(&msg, 0);
setMsgSeq(&msg, _last_msg_seq);
setMsgTC1(&msg, 0);
setMsgTS1(&msg, getMonoRawTimeNSec());
_last_msg_seq++;