From 6ec8dec63a14e382dbcf2e46aa8979bb805a0f9d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 19 Aug 2025 15:55:04 +0200 Subject: [PATCH] commander: add valid_registrations_mask to ArmingCheckRequest.msg This allows external modes to individually check if they are flagged as invalid/unresponsive. Previously this was done only based on whether or not ArmingCheckRequest was received, which does not work when multiple modes are running. --- msg/px4_msgs_old/msg/ArmingCheckRequestV0.msg | 14 ++++++++ .../translations/all_translations.h | 1 + .../translation_arming_check_request_v1.h | 36 +++++++++++++++++++ msg/versioned/ArmingCheckRequest.msg | 4 ++- .../checks/externalChecks.cpp | 9 +++++ 5 files changed, 63 insertions(+), 1 deletion(-) create mode 100644 msg/px4_msgs_old/msg/ArmingCheckRequestV0.msg create mode 100644 msg/translation_node/translations/translation_arming_check_request_v1.h diff --git a/msg/px4_msgs_old/msg/ArmingCheckRequestV0.msg b/msg/px4_msgs_old/msg/ArmingCheckRequestV0.msg new file mode 100644 index 0000000000..0f7b1ec729 --- /dev/null +++ b/msg/px4_msgs_old/msg/ArmingCheckRequestV0.msg @@ -0,0 +1,14 @@ +# Arming check request. +# +# Broadcast message to request arming checks be reported by all registered components, such as external ROS 2 navigation modes. +# All registered components should respond with an ArmingCheckReply message that indicates their current mode requirements, and any arming failure information. +# The request is sent regularly, even while armed, so that the FMU always knows the current arming state for external modes, and can forward it to ground stations. +# +# The reply will include the published request_id, allowing correlation of all arming check information for a particular request. +# The reply will also include the registration_id for each external component, provided to it during the registration process (RegisterExtComponentReply). + +uint32 MESSAGE_VERSION = 0 + +uint64 timestamp # [us] Time since system start. + +uint8 request_id # Id of this request. Allows correlation with associated ArmingCheckReply messages. diff --git a/msg/translation_node/translations/all_translations.h b/msg/translation_node/translations/all_translations.h index 971062a576..1322253715 100644 --- a/msg/translation_node/translations/all_translations.h +++ b/msg/translation_node/translations/all_translations.h @@ -8,6 +8,7 @@ #include "translation_airspeed_validated_v1.h" #include "translation_arming_check_reply_v1.h" +#include "translation_arming_check_request_v1.h" #include "translation_battery_status_v1.h" #include "translation_event_v1.h" #include "translation_home_position_v1.h" diff --git a/msg/translation_node/translations/translation_arming_check_request_v1.h b/msg/translation_node/translations/translation_arming_check_request_v1.h new file mode 100644 index 0000000000..f6b2f412a1 --- /dev/null +++ b/msg/translation_node/translations/translation_arming_check_request_v1.h @@ -0,0 +1,36 @@ +/**************************************************************************** + * Copyright (c) 2025 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +// Translate ArmingCheckRequest v0 <--> v1 +#include +#include + +class ArmingCheckRequestV1Translation { +public: + using MessageOlder = px4_msgs_old::msg::ArmingCheckRequestV0; + static_assert(MessageOlder::MESSAGE_VERSION == 0); + + using MessageNewer = px4_msgs::msg::ArmingCheckRequest; + static_assert(MessageNewer::MESSAGE_VERSION == 1); + + static constexpr const char* kTopic = "/fmu/out/arming_check_request"; + + static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { + msg_newer.timestamp = msg_older.timestamp; + + msg_newer.request_id = msg_older.request_id; + + msg_newer.valid_registrations_mask = 0xffffffff; + } + + static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) { + msg_older.timestamp = msg_newer.timestamp; + + msg_older.request_id = msg_newer.request_id; + } +}; + +REGISTER_TOPIC_TRANSLATION_DIRECT(ArmingCheckRequestV1Translation); diff --git a/msg/versioned/ArmingCheckRequest.msg b/msg/versioned/ArmingCheckRequest.msg index 0f7b1ec729..42d56bdf51 100644 --- a/msg/versioned/ArmingCheckRequest.msg +++ b/msg/versioned/ArmingCheckRequest.msg @@ -7,8 +7,10 @@ # The reply will include the published request_id, allowing correlation of all arming check information for a particular request. # The reply will also include the registration_id for each external component, provided to it during the registration process (RegisterExtComponentReply). -uint32 MESSAGE_VERSION = 0 +uint32 MESSAGE_VERSION = 1 uint64 timestamp # [us] Time since system start. uint8 request_id # Id of this request. Allows correlation with associated ArmingCheckReply messages. + +uint32 valid_registrations_mask # Bitmask of valid registration ID's (the bit is also cleared if flagged as unresponsive) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp index 80a92b067c..c5de25278d 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp @@ -293,6 +293,15 @@ void ExternalChecks::update() arming_check_request_s request{}; request.request_id = ++_current_request_id; request.timestamp = hrt_absolute_time(); + request.valid_registrations_mask = _active_registrations_mask; + + // Clear unresponsive ones + for (int i = 0; i < MAX_NUM_REGISTRATIONS; ++i) { + if (_registrations[i].unresponsive) { + request.valid_registrations_mask &= ~(1u << i); + } + } + _arming_check_request_pub.publish(request); } }