From fb7e973dfdf2ef72594851a294a9a62105df989a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 28 Jul 2023 07:33:47 +0200 Subject: [PATCH] commander: allow external modes to be assigned to RC Stores a hash of the mode name so that the same mode is always assigned to the same index independent from registration order. --- src/modules/commander/CMakeLists.txt | 3 + src/modules/commander/ModeManagement.cpp | 84 ++++++++++- src/modules/commander/ModeManagementTest.cpp | 101 +++++++++++++ src/modules/commander/commander_params.c | 144 ------------------- src/modules/commander/module.yaml | 50 +++++++ src/modules/manual_control/ManualControl.cpp | 9 ++ 6 files changed, 243 insertions(+), 148 deletions(-) create mode 100644 src/modules/commander/ModeManagementTest.cpp create mode 100644 src/modules/commander/module.yaml diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index 4be4afc81b..651fbcb680 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -61,6 +61,8 @@ px4_add_module( Safety.cpp UserModeIntention.cpp worker_thread.cpp + MODULE_CONFIG + module.yaml DEPENDS ArmAuthorization circuit_breaker @@ -76,3 +78,4 @@ px4_add_module( ) px4_add_unit_gtest(SRC mag_calibration_test.cpp LINKLIBS modules__commander) +px4_add_functional_gtest(SRC ModeManagementTest.cpp LINKLIBS modules__commander) diff --git a/src/modules/commander/ModeManagement.cpp b/src/modules/commander/ModeManagement.cpp index 42e8c37e34..14ba7ba28f 100644 --- a/src/modules/commander/ModeManagement.cpp +++ b/src/modules/commander/ModeManagement.cpp @@ -94,14 +94,90 @@ bool Modes::hasFreeExternalModes() const uint8_t Modes::addExternalMode(const Modes::Mode &mode) { + int32_t mode_name_hash = (int32_t)events::util::hash_32_fnv1a_const(mode.name); + + if (mode_name_hash == 0) { // 0 is reserved for unused indexes + mode_name_hash = 1; + } + + // Try to find the index with matching hash (if mode was already registered before), + // so that the same mode always gets the same index (required for RC switch mode assignment) + int first_unused_idx = -1; + int first_invalid_idx = -1; + int matching_idx = -1; + for (int i = 0; i < MAX_NUM; ++i) { - if (!_modes[i].valid) { - _modes[i] = mode; - _modes[i].valid = true; - return i + FIRST_EXTERNAL_NAV_STATE; + char hash_param_name[20]; + snprintf(hash_param_name, sizeof(hash_param_name), "COM_MODE%d_HASH", i); + const param_t handle = param_find(hash_param_name); + int32_t current_hash{}; + + if (handle != PARAM_INVALID && param_get(handle, ¤t_hash) == 0) { + if (!_modes[i].valid && current_hash == 0 && first_unused_idx == -1) { + first_unused_idx = i; + } + + if (current_hash == mode_name_hash) { + matching_idx = i; + } + + if (!_modes[i].valid && first_invalid_idx == -1) { + first_invalid_idx = i; + } } } + bool need_to_update_param = false; + int new_mode_idx = -1; + + if (matching_idx != -1) { + // If we found a match, try to use it but check for hash collisions or duplicate mode name + if (_modes[matching_idx].valid) { + // This can happen when restarting modes while armed + PX4_WARN("Mode '%s' already registered (as '%s')", mode.name, _modes[matching_idx].name); + + if (first_unused_idx != -1) { + new_mode_idx = first_unused_idx; + // Do not update the hash + + } else { + // Need to overwrite a hash. Reset it as we can't store duplicate hashes anyway + new_mode_idx = first_invalid_idx; + need_to_update_param = true; + mode_name_hash = 0; + } + + } else { + new_mode_idx = matching_idx; + } + + } else if (first_unused_idx != -1) { + // Mode registers the first time and there's still unused indexes + need_to_update_param = true; + new_mode_idx = first_unused_idx; + + } else { + // Mode registers the first time but all indexes are used so we need to overwrite one + need_to_update_param = true; + new_mode_idx = first_invalid_idx; + } + + if (new_mode_idx != -1 && !_modes[new_mode_idx].valid) { + if (need_to_update_param) { + char hash_param_name[20]; + snprintf(hash_param_name, sizeof(hash_param_name), "COM_MODE%d_HASH", new_mode_idx); + const param_t handle = param_find(hash_param_name); + + if (handle != PARAM_INVALID) { + param_set_no_notification(handle, &mode_name_hash); + } + } + + _modes[new_mode_idx] = mode; + _modes[new_mode_idx].valid = true; + return new_mode_idx + FIRST_EXTERNAL_NAV_STATE; + } + PX4_ERR("logic error"); return -1; } diff --git a/src/modules/commander/ModeManagementTest.cpp b/src/modules/commander/ModeManagementTest.cpp new file mode 100644 index 0000000000..a84c625137 --- /dev/null +++ b/src/modules/commander/ModeManagementTest.cpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include "ModeManagement.hpp" + +static bool modeValid(uint8_t mode) +{ + return mode >= Modes::FIRST_EXTERNAL_NAV_STATE && mode <= Modes::LAST_EXTERNAL_NAV_STATE; +} + +static int32_t readHash(int idx) +{ + char buffer[20]; + snprintf(buffer, sizeof(buffer), "COM_MODE%u_HASH", idx); + param_t param = param_find(buffer); + int32_t value{}; + param_get(param, &value); + return value; +} + +TEST(ModeManagementTest, Hashes) +{ + param_control_autosave(false); + + // Reset parameters + for (int i = 0; i < Modes::MAX_NUM; ++i) { + char buffer[20]; + snprintf(buffer, sizeof(buffer), "COM_MODE%u_HASH", i); + param_t param = param_find(buffer); + param_reset(param); + } + + // Add full set of modes, which stores the hashes + Modes modes; + Modes::Mode mode; + + for (int i = 0; i < Modes::MAX_NUM; ++i) { + snprintf(mode.name, sizeof(mode.name), "mode %i", i); + EXPECT_EQ(modes.addExternalMode(mode), Modes::FIRST_EXTERNAL_NAV_STATE + i); + EXPECT_EQ(readHash(i), events::util::hash_32_fnv1a_const(mode.name)); + } + + EXPECT_FALSE(modes.hasFreeExternalModes()); + + // Remove all modes, except last + for (int i = 0; i < Modes::MAX_NUM - 1; ++i) { + snprintf(mode.name, sizeof(mode.name), "mode %i", i); + EXPECT_TRUE(modes.removeExternalMode(Modes::FIRST_EXTERNAL_NAV_STATE + i, mode.name)); + } + + // Add some mode, ensure it gets the same index + const int mode_to_add_idx = 3; + snprintf(mode.name, sizeof(mode.name), "mode %i", mode_to_add_idx); + EXPECT_EQ(modes.addExternalMode(mode), Modes::FIRST_EXTERNAL_NAV_STATE + mode_to_add_idx); + + // Try to add another one with the same name: should succeed, with the hash of the added index reset + uint8_t added_mode_nav_state = modes.addExternalMode(mode); + EXPECT_EQ(readHash(added_mode_nav_state - Modes::FIRST_EXTERNAL_NAV_STATE), 0); + + // 3 Modes are used now. Add N-3 new ones which must overwrite previous hashes + for (int i = 0; i < Modes::MAX_NUM - 3; ++i) { + snprintf(mode.name, sizeof(mode.name), "new mode %i", i); + added_mode_nav_state = modes.addExternalMode(mode); + EXPECT_TRUE(modeValid(added_mode_nav_state)); + EXPECT_EQ(readHash(added_mode_nav_state - Modes::FIRST_EXTERNAL_NAV_STATE), + events::util::hash_32_fnv1a_const(mode.name)); + } + + EXPECT_FALSE(modes.hasFreeExternalModes()); +} diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 6503635799..6274b841ec 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -345,150 +345,6 @@ PARAM_DEFINE_INT32(COM_OBL_RC_ACT, 0); */ PARAM_DEFINE_FLOAT(COM_OBC_LOSS_T, 5.0f); -/** - * First flightmode slot (1000-1160) - * - * If the main switch channel is in this range the - * selected flight mode will be applied. - * - * @value -1 Unassigned - * @value 0 Manual - * @value 1 Altitude - * @value 2 Position - * @value 3 Mission - * @value 4 Hold - * @value 10 Takeoff - * @value 11 Land - * @value 5 Return - * @value 6 Acro - * @value 7 Offboard - * @value 8 Stabilized - * @value 12 Follow Me - * @value 13 Precision Land - * @group Commander - */ -PARAM_DEFINE_INT32(COM_FLTMODE1, -1); - -/** - * Second flightmode slot (1160-1320) - * - * If the main switch channel is in this range the - * selected flight mode will be applied. - * - * @value -1 Unassigned - * @value 0 Manual - * @value 1 Altitude - * @value 2 Position - * @value 3 Mission - * @value 4 Hold - * @value 10 Takeoff - * @value 11 Land - * @value 5 Return - * @value 6 Acro - * @value 7 Offboard - * @value 8 Stabilized - * @value 12 Follow Me - * @value 13 Precision Land - * @group Commander - */ -PARAM_DEFINE_INT32(COM_FLTMODE2, -1); - -/** - * Third flightmode slot (1320-1480) - * - * If the main switch channel is in this range the - * selected flight mode will be applied. - * - * @value -1 Unassigned - * @value 0 Manual - * @value 1 Altitude - * @value 2 Position - * @value 3 Mission - * @value 4 Hold - * @value 10 Takeoff - * @value 11 Land - * @value 5 Return - * @value 6 Acro - * @value 7 Offboard - * @value 8 Stabilized - * @value 12 Follow Me - * @value 13 Precision Land - * @group Commander - */ -PARAM_DEFINE_INT32(COM_FLTMODE3, -1); - -/** - * Fourth flightmode slot (1480-1640) - * - * If the main switch channel is in this range the - * selected flight mode will be applied. - * - * @value -1 Unassigned - * @value 0 Manual - * @value 1 Altitude - * @value 2 Position - * @value 3 Mission - * @value 4 Hold - * @value 10 Takeoff - * @value 11 Land - * @value 5 Return - * @value 6 Acro - * @value 7 Offboard - * @value 8 Stabilized - * @value 12 Follow Me - * @value 13 Precision Land - * @group Commander - */ -PARAM_DEFINE_INT32(COM_FLTMODE4, -1); - -/** - * Fifth flightmode slot (1640-1800) - * - * If the main switch channel is in this range the - * selected flight mode will be applied. - * - * @value -1 Unassigned - * @value 0 Manual - * @value 1 Altitude - * @value 2 Position - * @value 3 Mission - * @value 4 Hold - * @value 10 Takeoff - * @value 11 Land - * @value 5 Return - * @value 6 Acro - * @value 7 Offboard - * @value 8 Stabilized - * @value 12 Follow Me - * @value 13 Precision Land - * @group Commander - */ -PARAM_DEFINE_INT32(COM_FLTMODE5, -1); - -/** - * Sixth flightmode slot (1800-2000) - * - * If the main switch channel is in this range the - * selected flight mode will be applied. - * - * @value -1 Unassigned - * @value 0 Manual - * @value 1 Altitude - * @value 2 Position - * @value 3 Mission - * @value 4 Hold - * @value 10 Takeoff - * @value 11 Land - * @value 5 Return - * @value 6 Acro - * @value 7 Offboard - * @value 8 Stabilized - * @value 12 Follow Me - * @value 13 Precision Land - * @group Commander - */ -PARAM_DEFINE_INT32(COM_FLTMODE6, -1); - /** * Maximum EKF position innovation test ratio that will allow arming * diff --git a/src/modules/commander/module.yaml b/src/modules/commander/module.yaml new file mode 100644 index 0000000000..9e2cf1a0b4 --- /dev/null +++ b/src/modules/commander/module.yaml @@ -0,0 +1,50 @@ +module_name: Commander +parameters: + - group: Commander + definitions: + COM_MODE${i}_HASH: + description: + short: External mode identifier ${i} + long: | + This parameter is automatically set to identify external modes. It ensures that modes + get assigned to the same index independent from their startup order, + which is required when mapping an external mode to an RC switch. + type: int32 + num_instances: 8 # Max 8 modes (NAVIGATION_STATE_EXTERNAL8) + default: 0 + volatile: true + category: System + + COM_FLTMODE${i}: + description: + short: Mode slot ${i} + long: | + If the main switch channel is in this range the + selected flight mode will be applied. + type: enum + values: + -1: Unassigned + 0: Manual + 1: Altitude + 2: Position + 3: Mission + 4: Hold + 10: Takeoff + 11: Land + 5: Return + 6: Acro + 7: Offboard + 8: Stabilized + 12: Follow Me + 13: Precision Land + 100: External Mode 1 + 101: External Mode 2 + 102: External Mode 3 + 103: External Mode 4 + 104: External Mode 5 + 105: External Mode 6 + 106: External Mode 7 + 107: External Mode 8 + instance_start: 1 + num_instances: 6 + default: -1 diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index 9c01750f31..ac549d6376 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -555,6 +555,15 @@ int8_t ManualControl::navStateFromParam(int32_t param_value) case 13: return vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND; case 14: return vehicle_status_s::NAVIGATION_STATE_ORBIT; case 15: return vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF; + + case 100: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL1; + case 101: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL2; + case 102: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL3; + case 103: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL4; + case 104: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL5; + case 105: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL6; + case 106: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL7; + case 107: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL8; } return -1; }