From d97a8d7d3b9be0925fdf975c08733b6314c6af80 Mon Sep 17 00:00:00 2001 From: Michael Schaeuble Date: Mon, 24 Nov 2025 10:29:47 +0100 Subject: [PATCH] mode: control auto set home from an external mode The mode executor can run land mode which updates the home position to the landing location. This can be not the desirable behavior and the home position should stay at the original location. A flag is added to the configuration overrides to control if the home position is updated or not. --- msg/px4_msgs_old/msg/ConfigOverridesV0.msg | 21 ++++++++++ .../translations/all_translations.h | 1 + .../translation_config_overrides_v1.h | 41 +++++++++++++++++++ msg/versioned/ConfigOverrides.msg | 4 +- src/modules/commander/Commander.cpp | 7 ++-- src/modules/commander/ModeManagement.cpp | 4 ++ 6 files changed, 73 insertions(+), 5 deletions(-) create mode 100644 msg/px4_msgs_old/msg/ConfigOverridesV0.msg create mode 100644 msg/translation_node/translations/translation_config_overrides_v1.h diff --git a/msg/px4_msgs_old/msg/ConfigOverridesV0.msg b/msg/px4_msgs_old/msg/ConfigOverridesV0.msg new file mode 100644 index 0000000000..b274adfbec --- /dev/null +++ b/msg/px4_msgs_old/msg/ConfigOverridesV0.msg @@ -0,0 +1,21 @@ +# Configurable overrides by (external) modes or mode executors + +uint32 MESSAGE_VERSION = 0 + +uint64 timestamp # time since system start (microseconds) + +bool disable_auto_disarm # Prevent the drone from automatically disarming after landing (if configured) + +bool defer_failsafes # Defer all failsafes that can be deferred (until the flag is cleared) +int16 defer_failsafes_timeout_s # Maximum time a failsafe can be deferred. 0 = system default, -1 = no timeout + + +int8 SOURCE_TYPE_MODE = 0 +int8 SOURCE_TYPE_MODE_EXECUTOR = 1 +int8 source_type + +uint8 source_id # ID depending on source_type + +uint8 ORB_QUEUE_LENGTH = 4 + +# TOPICS config_overrides config_overrides_request diff --git a/msg/translation_node/translations/all_translations.h b/msg/translation_node/translations/all_translations.h index e463911482..12a8763ec6 100644 --- a/msg/translation_node/translations/all_translations.h +++ b/msg/translation_node/translations/all_translations.h @@ -10,6 +10,7 @@ #include "translation_arming_check_reply_v1.h" #include "translation_arming_check_request_v1.h" #include "translation_battery_status_v1.h" +#include "translation_config_overrides_v1.h" #include "translation_event_v1.h" #include "translation_home_position_v1.h" #include "translation_register_ext_component_reply_v1.h" diff --git a/msg/translation_node/translations/translation_config_overrides_v1.h b/msg/translation_node/translations/translation_config_overrides_v1.h new file mode 100644 index 0000000000..1f7e813951 --- /dev/null +++ b/msg/translation_node/translations/translation_config_overrides_v1.h @@ -0,0 +1,41 @@ +/**************************************************************************** + * Copyright (c) 2025 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +// Translate ConfigOverrides v0 <--> v1 +#include +#include + +class ConfigOverridesV1Translation { +public: + using MessageOlder = px4_msgs_old::msg::ConfigOverridesV0; + static_assert(MessageOlder::MESSAGE_VERSION == 0); + + using MessageNewer = px4_msgs::msg::ConfigOverrides; + static_assert(MessageNewer::MESSAGE_VERSION == 1); + + static constexpr const char* kTopic = "fmu/in/config_overrides_request"; + + static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { + msg_newer.timestamp = msg_older.timestamp; + msg_newer.disable_auto_disarm = msg_older.disable_auto_disarm; + msg_newer.defer_failsafes = msg_older.defer_failsafes; + msg_newer.defer_failsafes_timeout_s = msg_older.defer_failsafes_timeout_s; + msg_newer.disable_auto_set_home = false; + msg_newer.source_type = msg_older.source_type; + msg_newer.source_id = msg_older.source_id; + } + + static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) { + msg_older.timestamp = msg_newer.timestamp; + msg_older.disable_auto_disarm = msg_newer.disable_auto_disarm; + msg_older.defer_failsafes = msg_newer.defer_failsafes; + msg_older.defer_failsafes_timeout_s = msg_newer.defer_failsafes_timeout_s; + msg_older.source_type = msg_newer.source_type; + msg_older.source_id = msg_newer.source_id; + } +}; + +REGISTER_TOPIC_TRANSLATION_DIRECT(ConfigOverridesV1Translation); diff --git a/msg/versioned/ConfigOverrides.msg b/msg/versioned/ConfigOverrides.msg index b274adfbec..51fd86b037 100644 --- a/msg/versioned/ConfigOverrides.msg +++ b/msg/versioned/ConfigOverrides.msg @@ -1,6 +1,6 @@ # Configurable overrides by (external) modes or mode executors -uint32 MESSAGE_VERSION = 0 +uint32 MESSAGE_VERSION = 1 uint64 timestamp # time since system start (microseconds) @@ -8,7 +8,7 @@ bool disable_auto_disarm # Prevent the drone from automatically disarmin bool defer_failsafes # Defer all failsafes that can be deferred (until the flag is cleared) int16 defer_failsafes_timeout_s # Maximum time a failsafe can be deferred. 0 = system default, -1 = no timeout - +bool disable_auto_set_home # Prevent the drone from automatically setting the home position on arm or takeoff int8 SOURCE_TYPE_MODE = 0 int8 SOURCE_TYPE_MODE_EXECUTOR = 1 diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 4f6bfa0faf..3923eb379f 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -638,7 +638,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ events::send(events::ID("commander_armed_by"), events::Log::Info, "Armed by {1}", calling_reason); - if (_param_com_home_en.get() && !_mission_in_progress) { + if (_param_com_home_en.get() && !_mission_in_progress && !_config_overrides.disable_auto_set_home) { _home_position.setHomePosition(); } @@ -1850,7 +1850,8 @@ void Commander::run() _mission_in_progress = (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) && !_mission_result_sub.get().finished; - _home_position.update(_param_com_home_en.get(), !isArmed() && _vehicle_land_detected.landed && !_mission_in_progress); + _home_position.update(_param_com_home_en.get(), !isArmed() && _vehicle_land_detected.landed && !_mission_in_progress + && !_config_overrides.disable_auto_set_home); handleAutoDisarm(); @@ -2140,7 +2141,7 @@ void Commander::landDetectorUpdate() } // automatically set or update home position - if (_param_com_home_en.get() && !_mission_in_progress) { + if (_param_com_home_en.get() && !_mission_in_progress && !_config_overrides.disable_auto_set_home) { // set the home position when taking off if (!_vehicle_land_detected.landed) { if (was_landed) { diff --git a/src/modules/commander/ModeManagement.cpp b/src/modules/commander/ModeManagement.cpp index d5d800f8de..ae2dee41ff 100644 --- a/src/modules/commander/ModeManagement.cpp +++ b/src/modules/commander/ModeManagement.cpp @@ -563,6 +563,10 @@ void ModeManagement::updateActiveConfigOverrides(uint8_t nav_state, config_overr current_overrides.disable_auto_disarm = true; } + if (executor_overrides.disable_auto_set_home) { + current_overrides.disable_auto_set_home = true; + } + if (executor_overrides.defer_failsafes) { current_overrides.defer_failsafes = true; current_overrides.defer_failsafes_timeout_s = executor_overrides.defer_failsafes_timeout_s;