From 47a17f9d82ecde3bf61d80fa7c2ebff0b67d9b29 Mon Sep 17 00:00:00 2001 From: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Date: Sun, 19 Jan 2025 13:58:13 -0700 Subject: [PATCH] AP_DDS: Add infinite initialization wait Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> --- libraries/AP_DDS/AP_DDS_Client.cpp | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index f180bf250a2eb1..87d54222e76ec4 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -154,8 +154,8 @@ const AP_Param::GroupInfo AP_DDS_Client::var_info[] { // @Param: _MAX_RETRY // @DisplayName: DDS ping max attempts - // @Description: The maximum number of times the DDS client will attempt to ping the XRCE agent before exiting. - // @Range: 1 100 + // @Description: The maximum number of times the DDS client will attempt to ping the XRCE agent before exiting. Set to 0 to allow unlimited retries. + // @Range: 0 100 // @RebootRequired: True // @Increment: 1 // @User: Standard @@ -1180,6 +1180,7 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u */ void AP_DDS_Client::main_loop(void) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s initializing...", msg_prefix); if (!init_transport()) { return; } @@ -1192,9 +1193,16 @@ void AP_DDS_Client::main_loop(void) } // check ping - if (!uxr_ping_agent_attempts(comm, ping_timeout_ms, ping_max_retry)) { - GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s No ping response, exiting", msg_prefix); - return; + if (ping_max_retry == 0) { + if (!uxr_ping_agent(comm, ping_timeout_ms)) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s No ping response, retrying", msg_prefix); + continue; + } + } else { + if (!uxr_ping_agent_attempts(comm, ping_timeout_ms, ping_max_retry)) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s No ping response, exiting", msg_prefix); + continue; + } } // create session