diff --git a/src/lib/timesync/Timesync.hpp b/src/lib/timesync/Timesync.hpp index b17766b09a..ff6dcc1668 100644 --- a/src/lib/timesync/Timesync.hpp +++ b/src/lib/timesync/Timesync.hpp @@ -106,13 +106,6 @@ public: 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 @@ private: */ 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 be575fb6db..43a391de11 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(); } }