From 2363c03bfe82762930f793d21e9d1ae498007fbb Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Sun, 26 Nov 2023 01:27:48 +0000 Subject: [PATCH] [uxrce_dds_client] wait for Timesync to converge Signed-off-by: Beniamino Pozzan --- src/lib/timesync/Timesync.hpp | 14 +++++++------- src/modules/uxrce_dds_client/uxrce_dds_client.cpp | 5 +++-- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/lib/timesync/Timesync.hpp b/src/lib/timesync/Timesync.hpp index b17766b09a35..ff6dcc1668df 100644 --- a/src/lib/timesync/Timesync.hpp +++ b/src/lib/timesync/Timesync.hpp @@ -106,13 +106,6 @@ class Timesync int64_t offset() const { return (int64_t)_time_offset; } -private: - - /** - * Online exponential filter to smooth time offset - */ - void add_sample(int64_t offset_us); - /** * Return true if the timesync algorithm converged to a good estimate, * return false otherwise @@ -124,6 +117,13 @@ class Timesync */ void reset_filter(); + +private: + + /** + * Online exponential filter to smooth time offset + */ + void add_sample(int64_t offset_us); uORB::PublicationMulti _timesync_status_pub{ORB_ID(timesync_status)}; uint32_t _sequence{0}; diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp index be575fb6db63..43a391de1131 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp @@ -441,7 +441,7 @@ void UxrceddsClient::run() // Spin until sync with the Agent while (_synchronize_timestamps) { - if (uxr_sync_session(&session, 1000)) { + if (uxr_sync_session(&session, 1000) && _timesync.sync_converged()) { PX4_INFO("synchronized with time offset %-5" PRId64 "us", session.time_offset / 1000); if (_param_uxrce_dds_syncc.get() > 0) { @@ -505,7 +505,7 @@ void UxrceddsClient::run() // time sync session if (_synchronize_timestamps && hrt_elapsed_time(&last_sync_session) > 1_s) { - if (uxr_sync_session(&session, 100)) { + if (uxr_sync_session(&session, 100) && _timesync.sync_converged()) { //PX4_INFO("synchronized with time offset %-5" PRId64 "ns", session.time_offset); last_sync_session = hrt_absolute_time(); @@ -563,6 +563,7 @@ void UxrceddsClient::run() _last_payload_tx_rate = 0; _last_payload_tx_rate = 0; _subs->reset(); + _timesync.reset_filter(); } }