fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 02:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

microRTPS: extend verbose arg to the timesync

Browse Source
This commit is contained in:
TSC21
2020-08-28 09:22:36 +02:00
committed by Beat Küng
parent d09cb8d1f0
commit 98f53c2bc4
3 changed files with 14 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/templates/urtps/microRTPS_timesync.cpp.em
+7 -6
View File
@@ -66,13 +66,14 @@ except AttributeError:
#include "microRTPS_timesync.h"
TimeSync::TimeSync()
TimeSync::TimeSync(bool debug)
: _offset_ns(-1),
_skew_ns_per_sync(0.0),
_num_samples(0),
_request_reset_counter(0),
_last_msg_seq(0),
_last_remote_msg_seq(0)
_last_remote_msg_seq(0),
_debug(debug)
{ }
TimeSync::~TimeSync() { stop(); }
@@ -116,7 +117,7 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t
if (_request_reset_counter > REQUEST_RESET_COUNTER_THRESHOLD) {
reset();
std::cout << "\033[1;33m[ micrortps__timesync ]\tTimesync clock changed, resetting\033[0m" << std::endl;
if (_debug) std::cout << "\033[1;33m[ micrortps__timesync ]\tTimesync clock changed, resetting\033[0m" << std::endl;
}
if (_num_samples == 0) {
@@ -127,7 +128,7 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t
if (_num_samples >= WINDOW_SIZE) {
if (std::abs(measurement_offset - _offset_ns.load()) > TRIGGER_RESET_THRESHOLD_NS) {
_request_reset_counter++;
std::cout << "\033[1;33m[ micrortps__timesync ]\tTimesync offset outlier, discarding\033[0m" << std::endl;
if (_debug) std::cout << "\033[1;33m[ micrortps__timesync ]\tTimesync offset outlier, discarding\033[0m" << std::endl;
return false;
} else {
_request_reset_counter = 0;
@@ -136,7 +137,7 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t
// ignore if rtti > 50ms
if (rtti > 50ll * 1000ll * 1000ll) {
std::cout << "\033[1;33m[ micrortps__timesync ]\tRTTI too high for timesync: " << rtti / (1000ll * 1000ll) << "ms\033[0m" << std::endl;
if (_debug) std::cout << "\033[1;33m[ micrortps__timesync ]\tRTTI too high for timesync: " << rtti / (1000ll * 1000ll) << "ms\033[0m" << std::endl;
return false;
}
@@ -167,7 +168,7 @@ void TimeSync::processTimesyncMsg(timesync_msg_t * msg) {
if (getMsgTC1(msg) > 0) {
if (!addMeasurement(getMsgTS1(msg), getMsgTC1(msg), getMonoRawTimeNSec())) {
std::cerr << "\033[1;33m[ micrortps__timesync ]\tOffset not updated\033[0m" << std::endl;
if (_debug) std::cerr << "\033[1;33m[ micrortps__timesync ]\tOffset not updated\033[0m" << std::endl;
}
} else if (getMsgTC1(msg) == 0) {