modes: make available modes user selectable with a registration option

Some modes should only be run within the context of a mode executor and the user should not be able
to select them in the GCS. With this change, the external component registration request can be
used to set if a mode is selectable or not.
This commit is contained in:
Michael Schaeuble
2025-11-24 10:13:53 +01:00
committed by Beat Küng
parent 276cab8d3c
commit a2299b02c8
9 changed files with 155 additions and 3 deletions
@@ -0,0 +1,15 @@
uint32 MESSAGE_VERSION = 0
uint64 timestamp # time since system start (microseconds)
uint64 request_id # ID from the request
char[25] name # name from the request
uint16 px4_ros2_api_version
bool success
int8 arming_check_id # arming check registration ID (-1 if invalid)
int8 mode_id # assigned mode ID (-1 if invalid)
int8 mode_executor_id # assigned mode executor ID (-1 if invalid)
uint8 ORB_QUEUE_LENGTH = 2
@@ -0,0 +1,24 @@
# Request to register an external component
uint32 MESSAGE_VERSION = 0
uint64 timestamp # time since system start (microseconds)
uint64 request_id # ID, set this to a random value
char[25] name # either the requested mode name, or component name
uint16 LATEST_PX4_ROS2_API_VERSION = 1 # API version compatibility. Increase this on a breaking semantic change. Changes to any message field are detected separately and do not require an API version change.
uint16 px4_ros2_api_version # Set to LATEST_PX4_ROS2_API_VERSION
# Components to be registered
bool register_arming_check
bool register_mode # registering a mode also requires arming_check to be set
bool register_mode_executor # registering an executor also requires a mode to be registered (which is the owned mode by the executor)
bool enable_replace_internal_mode # set to true if an internal mode should be replaced
uint8 replace_internal_mode # vehicle_status::NAVIGATION_STATE_*
bool activate_mode_immediately # switch to the registered mode (can only be set in combination with an executor)
uint8 ORB_QUEUE_LENGTH = 2
@@ -12,6 +12,8 @@
#include "translation_battery_status_v1.h"
#include "translation_event_v1.h"
#include "translation_home_position_v1.h"
#include "translation_register_ext_component_reply_v1.h"
#include "translation_register_ext_component_request_v1.h"
#include "translation_vehicle_attitude_setpoint_v1.h"
#include "translation_vehicle_status_v1.h"
#include "translation_vehicle_local_position_v1.h"
@@ -0,0 +1,45 @@
/****************************************************************************
* Copyright (c) 2025 PX4 Development Team.
* SPDX-License-Identifier: BSD-3-Clause
****************************************************************************/
#pragma once
// Translate RegisterExtComponentReply v0 <--> v1
#include <px4_msgs_old/msg/register_ext_component_reply_v0.hpp>
#include <px4_msgs/msg/register_ext_component_reply.hpp>
class RegisterExtComponentReplyV1Translation {
public:
using MessageOlder = px4_msgs_old::msg::RegisterExtComponentReplyV0;
static_assert(MessageOlder::MESSAGE_VERSION == 0);
using MessageNewer = px4_msgs::msg::RegisterExtComponentReply;
static_assert(MessageNewer::MESSAGE_VERSION == 1);
static constexpr const char* kTopic = "fmu/out/register_ext_component_reply";
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.name = msg_older.name;
msg_newer.px4_ros2_api_version = msg_older.px4_ros2_api_version;
msg_newer.success = msg_older.success;
msg_newer.arming_check_id = msg_older.arming_check_id;
msg_newer.mode_id = msg_older.mode_id;
msg_newer.mode_executor_id = msg_older.mode_executor_id;
msg_newer.not_user_selectable = false;
}
static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) {
msg_older.timestamp = msg_newer.timestamp;
msg_older.request_id = msg_newer.request_id;
msg_older.name = msg_newer.name;
msg_older.px4_ros2_api_version = msg_newer.px4_ros2_api_version;
msg_older.success = msg_newer.success;
msg_older.arming_check_id = msg_newer.arming_check_id;
msg_older.mode_id = msg_newer.mode_id;
msg_older.mode_executor_id = msg_newer.mode_executor_id;
}
};
REGISTER_TOPIC_TRANSLATION_DIRECT(RegisterExtComponentReplyV1Translation);
@@ -0,0 +1,49 @@
/****************************************************************************
* Copyright (c) 2025 PX4 Development Team.
* SPDX-License-Identifier: BSD-3-Clause
****************************************************************************/
#pragma once
// Translate RegisterExtComponentRequest v0 <--> v1
#include <px4_msgs_old/msg/register_ext_component_request_v0.hpp>
#include <px4_msgs/msg/register_ext_component_request.hpp>
class RegisterExtComponentRequestV1Translation {
public:
using MessageOlder = px4_msgs_old::msg::RegisterExtComponentRequestV0;
static_assert(MessageOlder::MESSAGE_VERSION == 0);
using MessageNewer = px4_msgs::msg::RegisterExtComponentRequest;
static_assert(MessageNewer::MESSAGE_VERSION == 1);
static constexpr const char* kTopic = "fmu/in/register_ext_component_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.name = msg_older.name;
msg_newer.px4_ros2_api_version = msg_older.px4_ros2_api_version;
msg_newer.register_arming_check = msg_older.register_arming_check;
msg_newer.register_mode = msg_older.register_mode;
msg_newer.register_mode_executor = msg_older.register_mode_executor;
msg_newer.enable_replace_internal_mode = msg_older.enable_replace_internal_mode;
msg_newer.replace_internal_mode = msg_older.replace_internal_mode;
msg_newer.activate_mode_immediately = msg_older.activate_mode_immediately;
msg_newer.not_user_selectable = false;
}
static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) {
msg_older.timestamp = msg_newer.timestamp;
msg_older.request_id = msg_newer.request_id;
msg_older.name = msg_newer.name;
msg_older.px4_ros2_api_version = msg_newer.px4_ros2_api_version;
msg_older.register_arming_check = msg_newer.register_arming_check;
msg_older.register_mode = msg_newer.register_mode;
msg_older.register_mode_executor = msg_newer.register_mode_executor;
msg_older.enable_replace_internal_mode = msg_newer.enable_replace_internal_mode;
msg_older.replace_internal_mode = msg_newer.replace_internal_mode;
msg_older.activate_mode_immediately = msg_newer.activate_mode_immediately;
}
};
REGISTER_TOPIC_TRANSLATION_DIRECT(RegisterExtComponentRequestV1Translation);
+3 -1
View File
@@ -1,4 +1,4 @@
uint32 MESSAGE_VERSION = 0
uint32 MESSAGE_VERSION = 1
uint64 timestamp # time since system start (microseconds)
@@ -12,4 +12,6 @@ int8 arming_check_id # arming check registration ID (-1 if invalid)
int8 mode_id # assigned mode ID (-1 if invalid)
int8 mode_executor_id # assigned mode executor ID (-1 if invalid)
bool not_user_selectable # mode cannot be selected by the user
uint8 ORB_QUEUE_LENGTH = 2
@@ -1,6 +1,6 @@
# Request to register an external component
uint32 MESSAGE_VERSION = 0
uint32 MESSAGE_VERSION = 1
uint64 timestamp # time since system start (microseconds)
@@ -19,6 +19,6 @@ bool register_mode_executor # registering an executor also requires a mod
bool enable_replace_internal_mode # set to true if an internal mode should be replaced
uint8 replace_internal_mode # vehicle_status::NAVIGATION_STATE_*
bool activate_mode_immediately # switch to the registered mode (can only be set in combination with an executor)
bool not_user_selectable # mode cannot be selected by the user
uint8 ORB_QUEUE_LENGTH = 2
+1
View File
@@ -232,6 +232,7 @@ void ModeManagement::checkNewRegistrations(UpdateRequest &update_request)
static_assert(sizeof(request.name) == sizeof(reply.name), "size mismatch");
memcpy(reply.name, request.name, sizeof(request.name));
reply.request_id = request.request_id;
reply.not_user_selectable = request.not_user_selectable;
reply.px4_ros2_api_version = register_ext_component_request_s::LATEST_PX4_ROS2_API_VERSION;
// validate
@@ -38,6 +38,7 @@
#include <uORB/topics/register_ext_component_reply.h>
#include <lib/modes/standard_modes.hpp>
#include <lib/modes/ui.hpp>
#include <limits.h>
class MavlinkStreamAvailableModes : public MavlinkStream
{
@@ -71,6 +72,8 @@ private:
char name[sizeof(register_ext_component_reply_s::name)] {};
};
ExternalModeName *_external_mode_names{nullptr};
uint8_t _not_user_selectable_mask{0};
static_assert(MAX_NUM_EXTERNAL_MODES <= (sizeof(_not_user_selectable_mask) * CHAR_BIT), "Mask too small");
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _register_ext_component_reply_sub{ORB_ID(register_ext_component_reply)};
@@ -116,6 +119,10 @@ private:
} else if (_external_mode_names) {
strncpy(available_modes.mode_name, _external_mode_names[external_mode_index].name, sizeof(available_modes.mode_name));
available_modes.mode_name[sizeof(available_modes.mode_name) - 1] = '\0';
if ((_not_user_selectable_mask & (1 << external_mode_index)) > 0) {
available_modes.properties |= MAV_MODE_PROPERTY_NOT_USER_SELECTABLE;
}
}
} else { // Internal
@@ -205,6 +212,13 @@ private:
if (_external_mode_names && mode_index < MAX_NUM_EXTERNAL_MODES) {
memcpy(_external_mode_names[mode_index].name, reply.name, sizeof(ExternalModeName::name));
if (reply.not_user_selectable) {
_not_user_selectable_mask |= (1 << mode_index);
} else {
_not_user_selectable_mask &= ~(1 << mode_index);
}
}
dynamic_update = true;