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

RTPS timesync don't use MONOTONIC_RAW

Browse Source
This commit is contained in:
Julian Kent
2021-03-17 17:13:24 +01:00
committed by Beat Küng
parent 1011382098
commit c1cb964c2a
2 changed files with 18 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
msg/templates/urtps/microRTPS_timesync.cpp.em
View File

@@ -107,6 +107,16 @@ void TimeSync::reset() {
_request_reset_counter = 0;
}
int64_t TimeSync::getTimeNSec() {
auto time = std::chrono::steady_clock::now();
return std::chrono::time_point_cast<std::chrono::nanoseconds>(time).time_since_epoch().count();
}
int64_t TimeSync::getTimeUSec() {
auto time = std::chrono::steady_clock::now();
return std::chrono::time_point_cast<std::chrono::microseconds>(time).time_since_epoch().count();
}
bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t local_t3_ns) {
int64_t rtti = local_t3_ns - local_t1_ns;
@@ -167,15 +177,15 @@ void TimeSync::processTimesyncMsg(timesync_msg_t * msg) {
_last_remote_msg_seq = getMsgSeq(msg);
if (getMsgTC1(msg) > 0) {
if (!addMeasurement(getMsgTS1(msg), getMsgTC1(msg), getMonoRawTimeNSec())) {
if (!addMeasurement(getMsgTS1(msg), getMsgTC1(msg), getTimeNSec())) {
if (_debug) std::cerr << "\033[1;33m[ micrortps__timesync ]\tOffset not updated\033[0m" << std::endl;
}
} else if (getMsgTC1(msg) == 0) {
setMsgTimestamp(msg, getMonoTimeUSec());
setMsgTimestamp(msg, getTimeUSec());
setMsgSysID(msg, 0);
setMsgSeq(msg, getMsgSeq(msg) + 1);
setMsgTC1(msg, getMonoRawTimeNSec());
setMsgTC1(msg, getTimeNSec());
_timesync_pub.publish(msg);
}
@@ -185,11 +195,11 @@ void TimeSync::processTimesyncMsg(timesync_msg_t * msg) {
timesync_msg_t TimeSync::newTimesyncMsg() {
timesync_msg_t msg{};
setMsgTimestamp(&msg, getMonoTimeUSec());
setMsgTimestamp(&msg, getTimeUSec());
setMsgSysID(&msg, 0);
setMsgSeq(&msg, _last_msg_seq);
setMsgTC1(&msg, 0);
setMsgTS1(&msg, getMonoRawTimeNSec());
setMsgTS1(&msg, getTimeNSec());
_last_msg_seq++;

14
msg/templates/urtps/microRTPS_timesync.h.em
View File

@@ -130,23 +130,15 @@ public:
/**
* @@brief Get clock monotonic time (raw) in nanoseconds
* @@return System CLOCK_MONOTONIC_RAW time in nanoseconds
* @@return System CLOCK_MONOTONIC time in nanoseconds
*/
inline int64_t getMonoRawTimeNSec() {
timespec t;
clock_gettime(CLOCK_MONOTONIC_RAW, &t);
return static_cast<int64_t>(t.tv_sec * 1000000000LL + t.tv_nsec);
}
static int64_t getTimeNSec();
/**
* @@brief Get system monotonic time in microseconds
* @@return System CLOCK_MONOTONIC time in microseconds
*/
inline int64_t getMonoTimeUSec() {
timespec t;
clock_gettime(CLOCK_MONOTONIC, &t);
return static_cast<int64_t>(t.tv_sec * 1000000000LL + t.tv_nsec) / 1000LL;
}
static int64_t getTimeUSec();
/**
* @@brief Adds a time offset measurement to be filtered