fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: allow external modes more time for initial response

Browse Source 
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.
This commit is contained in:
Beat Küng 2024-07-30 11:52:18 +02:00
parent d4d60a5181
commit 4883f2128a
2 changed files with 10 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
View File

@ -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);

4
src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp
View File

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