Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 11:06:04 +08:00
Code Issues Actions 18 Packages Projects Releases Wiki Activity

microRTPS: extend verbose arg to the timesync

Browse Source
This commit is contained in:
TSC21
2020-08-27 16:07:18 +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_agent.cpp.em
+4 -4
View File
@@ -90,9 +90,6 @@ Transport_node *transport_node = nullptr;
RtpsTopics topics; RtpsTopics topics;
uint32_t total_sent = 0, sent = 0; uint32_t total_sent = 0, sent = 0;
// Init timesync
std::shared_ptr<TimeSync> timeSync = std::make_shared<TimeSync>();
struct options { struct options {
enum class eTransports enum class eTransports
{ {
@@ -174,7 +171,6 @@ void signal_handler(int signum)
printf("\033[1;33m[ micrortps_agent ]\tInterrupt signal (%d) received.\033[0m\n", signum); printf("\033[1;33m[ micrortps_agent ]\tInterrupt signal (%d) received.\033[0m\n", signum);
running = 0; running = 0;
transport_node->close(); transport_node->close();
timeSync->stop();
} }
@[if recv_topics]@ @[if recv_topics]@
@@ -271,6 +267,9 @@ int main(int argc, char** argv)
std::chrono::time_point<std::chrono::steady_clock> start, end; std::chrono::time_point<std::chrono::steady_clock> start, end;
@[end if]@ @[end if]@
// Init timesync
std::shared_ptr<TimeSync> timeSync = std::make_shared<TimeSync>(_options.verbose_debug);
topics.set_timesync(timeSync); topics.set_timesync(timeSync);
@[if recv_topics]@ @[if recv_topics]@
@@ -319,6 +318,7 @@ int main(int argc, char** argv)
delete transport_node; delete transport_node;
transport_node = nullptr; transport_node = nullptr;
timeSync->stop();
timeSync->reset(); timeSync->reset();
return 0; return 0;
msg/templates/urtps/microRTPS_timesync.cpp.em
+7 -6
View File
@@ -66,13 +66,14 @@ except AttributeError:
#include "microRTPS_timesync.h" #include "microRTPS_timesync.h"
TimeSync::TimeSync() TimeSync::TimeSync(bool debug)
: _offset_ns(-1), : _offset_ns(-1),
_skew_ns_per_sync(0.0), _skew_ns_per_sync(0.0),
_num_samples(0), _num_samples(0),
_request_reset_counter(0), _request_reset_counter(0),
_last_msg_seq(0), _last_msg_seq(0),
_last_remote_msg_seq(0) _last_remote_msg_seq(0),
_debug(debug)
{ } { }
TimeSync::~TimeSync() { stop(); } 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) { if (_request_reset_counter > REQUEST_RESET_COUNTER_THRESHOLD) {
reset(); 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) { 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 (_num_samples >= WINDOW_SIZE) {
if (std::abs(measurement_offset - _offset_ns.load()) > TRIGGER_RESET_THRESHOLD_NS) { if (std::abs(measurement_offset - _offset_ns.load()) > TRIGGER_RESET_THRESHOLD_NS) {
_request_reset_counter++; _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; return false;
} else { } else {
_request_reset_counter = 0; _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 // ignore if rtti > 50ms
if (rtti > 50ll * 1000ll * 1000ll) { 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; return false;
} }
@@ -167,7 +168,7 @@ void TimeSync::processTimesyncMsg(timesync_msg_t * msg) {
if (getMsgTC1(msg) > 0) { if (getMsgTC1(msg) > 0) {
if (!addMeasurement(getMsgTS1(msg), getMsgTC1(msg), getMonoRawTimeNSec())) { 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) { } else if (getMsgTC1(msg) == 0) {
msg/templates/urtps/microRTPS_timesync.h.em
+3 -1
View File
@@ -109,7 +109,7 @@ using TimesyncPublisher = timesync_Publisher;
class TimeSync { class TimeSync {
public: public:
TimeSync(); TimeSync(bool debug);
virtual ~TimeSync(); virtual ~TimeSync();
/** /**
@@ -196,6 +196,8 @@ private:
uint8_t _last_msg_seq; uint8_t _last_msg_seq;
uint8_t _last_remote_msg_seq; uint8_t _last_remote_msg_seq;
bool _debug;
@[if ros2_distro]@ @[if ros2_distro]@
Timesync_Publisher _timesync_pub; Timesync_Publisher _timesync_pub;
Timesync_Subscriber _timesync_sub; Timesync_Subscriber _timesync_sub;