From b9667e955d150b249d34251b8f89cfe5e9154b64 Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Wed, 26 Jul 2023 15:16:29 +0200 Subject: [PATCH] [uxrce-dds-client]: add parameter to disable time synchronization between Agent and Client Signed-off-by: Beniamino Pozzan --- src/modules/uxrce_dds_client/module.yaml | 11 +++++++ .../uxrce_dds_client/uxrce_dds_client.cpp | 33 +++++++++++++++---- .../uxrce_dds_client/uxrce_dds_client.h | 3 +- 3 files changed, 40 insertions(+), 7 deletions(-) diff --git a/src/modules/uxrce_dds_client/module.yaml b/src/modules/uxrce_dds_client/module.yaml index 739ae43af161..77078e560a09 100644 --- a/src/modules/uxrce_dds_client/module.yaml +++ b/src/modules/uxrce_dds_client/module.yaml @@ -64,3 +64,14 @@ parameters: reboot_required: true default: 2130706433 requires_ethernet: true + + UXRCE_DDS_SYNCT: + description: + short: uXRCE-DDS timestamp synchronization enable + long: When enabled, uxrce_dds_client will synchronize the timestamps + of the incoming and outgoing messages measuring the offset + between the Agent OS time and the PX4 time. + type: boolean + category: System + reboot_required: true + default: 1 diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp index b9f1d066e41f..91d7792d3640 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp @@ -79,11 +79,18 @@ void on_time(uxrSession *session, int64_t current_time, int64_t received_timesta } } +void on_time_no_sync(uxrSession *session, int64_t current_time, int64_t received_timestamp, int64_t transmit_timestamp, + int64_t originate_timestamp, void *args) +{ + session->time_offset = 0; +} + UxrceddsClient::UxrceddsClient(Transport transport, const char *device, int baudrate, const char *agent_ip, - const char *port, bool localhost_only, bool custom_participant, const char *client_namespace) : + const char *port, bool localhost_only, bool custom_participant, const char *client_namespace, + bool synchronize_timestamps) : ModuleParams(nullptr), _localhost_only(localhost_only), _custom_participant(custom_participant), - _client_namespace(client_namespace) + _client_namespace(client_namespace), _synchronize_timestamps(synchronize_timestamps) { if (transport == Transport::Serial) { @@ -299,12 +306,17 @@ void UxrceddsClient::run() _connected = true; // Set time-callback. - uxr_set_time_callback(&session, on_time, &_timesync); + if (_synchronize_timestamps) { + uxr_set_time_callback(&session, on_time, &_timesync); + + } else { + uxr_set_time_callback(&session, on_time_no_sync, nullptr); + } // Synchronize with the Agent bool synchronized = false; - while (!synchronized) { + while (_synchronize_timestamps && !synchronized) { synchronized = uxr_sync_session(&session, 1000); if (synchronized) { @@ -331,7 +343,7 @@ void UxrceddsClient::run() uxr_run_session_timeout(&session, 0); // time sync session - if (hrt_elapsed_time(&last_sync_session) > 1_s) { + if (_synchronize_timestamps && hrt_elapsed_time(&last_sync_session) > 1_s) { if (uxr_sync_session(&session, 100)) { //PX4_INFO("synchronized with time offset %-5" PRId64 "ns", session.time_offset); last_sync_session = hrt_absolute_time(); @@ -694,8 +706,17 @@ UxrceddsClient *UxrceddsClient::instantiate(int argc, char *argv[]) } } + // determines if timestamps should be synchronized + int32_t synchronize_timestamps = 0; + param_get(param_find("UXRCE_DDS_SYNCT"), &synchronize_timestamps); + + if ((synchronize_timestamps != 1) && (synchronize_timestamps != 0)) { + PX4_ERR("UXRCE_DDS_SYNCT must be either 0 or 1"); + } + + return new UxrceddsClient(transport, device, baudrate, agent_ip, port, localhost_only, custom_participant, - client_namespace); + client_namespace, synchronize_timestamps); } int UxrceddsClient::print_usage(const char *reason) diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.h b/src/modules/uxrce_dds_client/uxrce_dds_client.h index aab381b73dc6..f757040214b1 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.h +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.h @@ -49,7 +49,7 @@ class UxrceddsClient : public ModuleBase, public ModuleParams }; UxrceddsClient(Transport transport, const char *device, int baudrate, const char *host, const char *port, - bool localhost_only, bool custom_participant, const char *client_namespace); + bool localhost_only, bool custom_participant, const char *client_namespace, bool synchornize_timestamps); ~UxrceddsClient(); @@ -77,6 +77,7 @@ class UxrceddsClient : public ModuleBase, public ModuleParams const bool _localhost_only; const bool _custom_participant; const char *_client_namespace; + const bool _synchronize_timestamps; // max port characters (5+'\0')