From 4883f2128a02286df603d27104ed8a5110ee6d4c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 30 Jul 2024 11:52:18 +0200 Subject: [PATCH] commander: allow external modes more time for initial response We've come accross a case where a ROS node would consistently take something over 800 ms until the first arming check request subscription callback was triggered. After the first sample, the callback always triggered within the expected timeframe. Therefore this patch allows for more time right after registration until timing out. --- .../HealthAndArmingChecks/checks/externalChecks.cpp | 7 ++++++- .../HealthAndArmingChecks/checks/externalChecks.hpp | 4 ++++ 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp index c60a9a4af0..8ef3c5cfa9 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp @@ -64,6 +64,7 @@ int ExternalChecks::addRegistration(int8_t nav_mode_id, int8_t replaces_nav_stat _active_registrations_mask |= 1 << free_registration_index; _registrations[free_registration_index].nav_mode_id = nav_mode_id; _registrations[free_registration_index].replaces_nav_state = replaces_nav_state; + _registrations[free_registration_index].waiting_for_first_response = true; _registrations[free_registration_index].num_no_response = 0; _registrations[free_registration_index].unresponsive = false; _registrations[free_registration_index].total_num_unresponsive = 0; @@ -230,6 +231,7 @@ void ExternalChecks::update() && _current_request_id == reply.request_id) { _reply_received_mask |= 1u << reply.registration_id; _registrations[reply.registration_id].num_no_response = 0; + _registrations[reply.registration_id].waiting_for_first_response = false; // Prevent toggling between unresponsive & responsive state if (_registrations[reply.registration_id].total_num_unresponsive <= 3) { @@ -253,7 +255,10 @@ void ExternalChecks::update() for (int i = 0; i < MAX_NUM_REGISTRATIONS; ++i) { if ((1u << i) & no_reply) { - if (!_registrations[i].unresponsive && ++_registrations[i].num_no_response >= NUM_NO_REPLY_UNTIL_UNRESPONSIVE) { + const int max_num_no_reply = + _registrations[i].waiting_for_first_response ? NUM_NO_REPLY_UNTIL_UNRESPONSIVE_INIT : NUM_NO_REPLY_UNTIL_UNRESPONSIVE; + + if (!_registrations[i].unresponsive && ++_registrations[i].num_no_response > max_num_no_reply) { // Clear immediately if not a mode if (_registrations[i].nav_mode_id == -1) { removeRegistration(i, -1); diff --git a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp index b4ee24cba6..7129e46203 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp @@ -72,6 +72,9 @@ private: static constexpr hrt_abstime UPDATE_INTERVAL = 300_ms; static_assert(REQUEST_TIMEOUT < UPDATE_INTERVAL, "keep timeout < update interval"); static constexpr int NUM_NO_REPLY_UNTIL_UNRESPONSIVE = 3; ///< Mode timeout = this value * UPDATE_INTERVAL + /// Timeout directly after registering (in some cases ROS can take a while until the subscription gets the first + /// sample, around 800ms was observed) + static constexpr int NUM_NO_REPLY_UNTIL_UNRESPONSIVE_INIT = 10; void checkNonRegisteredModes(const Context &context, Report &reporter) const; @@ -83,6 +86,7 @@ private: int8_t nav_mode_id{-1}; ///< associated mode, -1 if none int8_t replaces_nav_state{-1}; + bool waiting_for_first_response{true}; uint8_t num_no_response{0}; bool unresponsive{false}; uint8_t total_num_unresponsive{0};