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};