From 6cc38776c88f61e108e32459aae69618b3d4f93d Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Wed, 22 Nov 2023 18:06:11 -0900 Subject: [PATCH] src/modules/uxrce_dds_client: set system clock from agent CLOCK_REALTIME (#22290) * Added parameter UXRCE_DDS_SYNCC to enable system clock synchronization. Refactored and cleaned up. Only set system time if it's off by more than 5 seconds (same as mavlink and gps). --- src/modules/uxrce_dds_client/CMakeLists.txt | 1 + src/modules/uxrce_dds_client/module.yaml | 10 ++ .../uxrce_dds_client/uxrce_dds_client.cpp | 111 ++++++++++-------- .../uxrce_dds_client/uxrce_dds_client.h | 23 +++- 4 files changed, 90 insertions(+), 55 deletions(-) diff --git a/src/modules/uxrce_dds_client/CMakeLists.txt b/src/modules/uxrce_dds_client/CMakeLists.txt index 10beaf69be..60c390892b 100644 --- a/src/modules/uxrce_dds_client/CMakeLists.txt +++ b/src/modules/uxrce_dds_client/CMakeLists.txt @@ -137,6 +137,7 @@ else() ${CMAKE_CURRENT_SOURCE_DIR} COMPILE_FLAGS #-O0 + # -DDEBUG_BUILD ${MAX_CUSTOM_OPT_LEVEL} SRCS ${CMAKE_CURRENT_BINARY_DIR}/dds_topics.h diff --git a/src/modules/uxrce_dds_client/module.yaml b/src/modules/uxrce_dds_client/module.yaml index 26d79d92a2..207c27459c 100644 --- a/src/modules/uxrce_dds_client/module.yaml +++ b/src/modules/uxrce_dds_client/module.yaml @@ -95,3 +95,13 @@ parameters: category: System reboot_required: true default: 1 + + UXRCE_DDS_SYNCC: + description: + short: Enable uXRCE-DDS system clock synchronization + long: When enabled along with UXRCE_DDS_SYNCT, uxrce_dds_client will + set the system clock using the agents UTC timestamp. + 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 9f25d439eb..10892a2a42 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp @@ -108,11 +108,9 @@ void on_request( } 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, - bool synchronize_timestamps) : + const char *port, const char *client_namespace) : ModuleParams(nullptr), - _localhost_only(localhost_only), _custom_participant(custom_participant), - _client_namespace(client_namespace), _synchronize_timestamps(synchronize_timestamps) + _client_namespace(client_namespace) { if (transport == Transport::Serial) { @@ -124,7 +122,7 @@ UxrceddsClient::UxrceddsClient(Transport transport, const char *device, int baud if (fd < 0) { PX4_ERR("open %s failed (%i)", device, errno); // sleep before trying again - usleep(1'000'000); + px4_usleep(1_s); } else { break; @@ -164,10 +162,14 @@ UxrceddsClient::UxrceddsClient(Transport transport, const char *device, int baud } } + #else PX4_ERR("UDP not supported"); #endif } + + _participant_config = static_cast(_param_uxrce_dds_ptcfg.get()); + _synchronize_timestamps = _param_uxrce_dds_synct.get() > 0; } UxrceddsClient::~UxrceddsClient() @@ -249,6 +251,39 @@ void UxrceddsClient::handleMessageFormatRequest() } } +void UxrceddsClient::syncSystemClock(uxrSession *session) +{ + struct timespec ts = {}; + px4_clock_gettime(CLOCK_REALTIME, &ts); + + // UTC timestamps in microseconds + int64_t system_utc = int64_t(ts.tv_sec) * 1000000LL + int64_t(ts.tv_nsec / 1000L); + int64_t agent_utc = int64_t(hrt_absolute_time()) + (session->time_offset / 1000LL); // ns to us + + uint64_t delta = abs(system_utc - agent_utc); + + if (delta < 5_s) { + // Only set the time if it's more than 5 seconds off (matches Mavlink and GPS logic) + PX4_DEBUG("agents UTC time is %s by %lld us, not setting clock", agent_utc > system_utc ? "ahead" : "behind", + abs(system_utc - agent_utc)); + return; + } + + ts.tv_sec = agent_utc / 1_s; + ts.tv_nsec = (agent_utc % 1_s) * 1000; + + if (px4_clock_settime(CLOCK_REALTIME, &ts)) { + PX4_ERR("failed setting system clock"); + + } else { + char buf[40]; + struct tm date_time; + localtime_r(&ts.tv_sec, &date_time); + strftime(buf, sizeof(buf), "%a %Y-%m-%d %H:%M:%S %Z", &date_time); + PX4_INFO("successfully set system clock: %s", buf); + } +} + void UxrceddsClient::run() { if (!_comm) { @@ -278,7 +313,7 @@ void UxrceddsClient::run() // Session // The key identifier of the Client. All Clients connected to an Agent must have a different key. - const uint32_t key = (uint32_t)_param_xrce_key.get(); + const uint32_t key = (uint32_t)_param_uxrce_key.get(); if (key == 0) { PX4_ERR("session key must be different from zero"); @@ -316,11 +351,11 @@ void UxrceddsClient::run() // Create entities uxrObjectId participant_id = uxr_object_id(0x01, UXR_PARTICIPANT_ID); - uint16_t domain_id = _param_xrce_dds_dom_id.get(); + uint16_t domain_id = _param_uxrce_dds_dom_id.get(); uint16_t participant_req{}; - if (_custom_participant) { + if (_participant_config == ParticipantConfig::Custom) { // Create participant by reference (XML not required) participant_req = uxr_buffer_create_participant_ref(&session, reliable_out, participant_id, domain_id, "px4_participant", UXR_REPLACE); @@ -329,7 +364,7 @@ void UxrceddsClient::run() // Construct participant XML and create participant by XML char participant_xml[PARTICIPANT_XML_SIZE]; int ret = snprintf(participant_xml, PARTICIPANT_XML_SIZE, "%s%s/px4_micro_xrce_dds%s", - _localhost_only ? + (_participant_config == ParticipantConfig::LocalHostOnly) ? "" "" "" @@ -350,7 +385,7 @@ void UxrceddsClient::run() _client_namespace : "", - _localhost_only ? + (_participant_config == ParticipantConfig::LocalHostOnly) ? "false" "udp_localhost" "" @@ -404,19 +439,19 @@ void UxrceddsClient::run() uxr_set_request_callback(&session, on_request, this); - // Synchronize with the Agent - bool synchronized = false; - - while (_synchronize_timestamps && !synchronized) { - synchronized = uxr_sync_session(&session, 1000); - - if (synchronized) { + // Spin until sync with the Agent + while (_synchronize_timestamps) { + if (uxr_sync_session(&session, 1000)) { PX4_INFO("synchronized with time offset %-5" PRId64 "us", session.time_offset / 1000); - //sleep(1); - } else { - usleep(10000); + if (_param_uxrce_dds_syncc.get() > 0) { + syncSystemClock(&session); + } + + break; } + + px4_usleep(10_ms); } hrt_abstime last_sync_session = 0; @@ -473,6 +508,10 @@ void UxrceddsClient::run() if (uxr_sync_session(&session, 100)) { //PX4_INFO("synchronized with time offset %-5" PRId64 "ns", session.time_offset); last_sync_session = hrt_absolute_time(); + + if (_param_uxrce_dds_syncc.get() > 0) { + syncSystemClock(&session); + } } } @@ -735,8 +774,8 @@ int UxrceddsClient::print_status() PX4_INFO("Using transport: udp"); PX4_INFO("Agent IP: %s", _agent_ip); PX4_INFO("Agent port: %s", _port); - PX4_INFO("Custom participant: %s", _custom_participant ? "yes" : "no"); - PX4_INFO("Localhost only: %s", _localhost_only ? "yes" : "no"); + PX4_INFO("Custom participant: %s", _participant_config == ParticipantConfig::Custom ? "yes" : "no"); + PX4_INFO("Localhost only: %s", _participant_config == ParticipantConfig::LocalHostOnly ? "yes" : "no"); } #endif @@ -771,9 +810,6 @@ UxrceddsClient *UxrceddsClient::instantiate(int argc, char *argv[]) const char *device = nullptr; int baudrate = 921600; - bool localhost_only = false; - bool custom_participant = false; - const char *client_namespace = nullptr;//"px4"; while ((ch = px4_getopt(argc, argv, "t:d:b:h:p:n:", &myoptind, &myoptarg)) != EOF) { @@ -855,19 +891,6 @@ UxrceddsClient *UxrceddsClient::instantiate(int argc, char *argv[]) static_cast(ip_i & 0xff)); } - int32_t participant_config = 0; - param_get(param_find("UXRCE_DDS_PTCFG"), &participant_config); - - switch (participant_config) { - case 1: - localhost_only = true; - break; - - case 2: - custom_participant = true; - break; - } - #endif // UXRCE_DDS_CLIENT_UDP if (error_flag) { @@ -881,17 +904,7 @@ 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, synchronize_timestamps); + return new UxrceddsClient(transport, device, baudrate, agent_ip, port, client_namespace); } 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 4ba16e0f50..b8f38ee76c 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.h +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.h @@ -57,7 +57,7 @@ public: }; 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 synchornize_timestamps); + const char *client_namespace); ~UxrceddsClient(); @@ -115,10 +115,18 @@ private: uORB::Publication _message_format_response_pub{ORB_ID(message_format_response)}; uORB::Subscription _message_format_request_sub{ORB_ID(message_format_request)}; - const bool _localhost_only; - const bool _custom_participant; + /** Synchronizes the system clock if the time is off by more than 5 seconds */ + void syncSystemClock(uxrSession *session); + const char *_client_namespace; - const bool _synchronize_timestamps; + + enum class ParticipantConfig { + Default, + LocalHostOnly, + Custom, + } _participant_config{ParticipantConfig::Default}; + + bool _synchronize_timestamps; // max port characters (5+'\0') static const uint8_t PORT_MAX_LENGTH = 6; @@ -148,7 +156,10 @@ private: Timesync _timesync{timesync_status_s::SOURCE_PROTOCOL_DDS}; DEFINE_PARAMETERS( - (ParamInt) _param_xrce_dds_dom_id, - (ParamInt) _param_xrce_key + (ParamInt) _param_uxrce_dds_dom_id, + (ParamInt) _param_uxrce_key, + (ParamInt) _param_uxrce_dds_ptcfg, + (ParamInt) _param_uxrce_dds_syncc, + (ParamInt) _param_uxrce_dds_synct ) };