dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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.
This commit is contained in:
Beat Küng 2023-07-28 07:33:47 +02:00
parent 22acb08406
commit fb7e973dfd
6 changed files with 243 additions and 148 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/commander/CMakeLists.txt
View File

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

84
src/modules/commander/ModeManagement.cpp
View File

@ -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, &current_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;
}

101
src/modules/commander/ModeManagementTest.cpp Normal file
View File

@ -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 <gtest/gtest.h>
#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());
}

144
src/modules/commander/commander_params.c
View File

@ -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
*

50
src/modules/commander/module.yaml Normal file
View File

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

9
src/modules/manual_control/ManualControl.cpp
View File

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