From 5bb811f8f9742145cbebe5b3356751d1aaf9d346 Mon Sep 17 00:00:00 2001 From: Daniel Mesham Date: Fri, 16 Jun 2023 10:21:37 -0400 Subject: [PATCH] commander: define user-selectable flight modes separately from valid flight modes --- src/modules/commander/ModeManagement.cpp | 4 +++- src/modules/commander/ModeManagement.hpp | 4 +++- src/modules/commander/ModeUtil/ui.hpp | 11 +++++++++++ 3 files changed, 17 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/ModeManagement.cpp b/src/modules/commander/ModeManagement.cpp index bad11d0f8a..8f379d3789 100644 --- a/src/modules/commander/ModeManagement.cpp +++ b/src/modules/commander/ModeManagement.cpp @@ -519,7 +519,9 @@ void ModeManagement::checkConfigOverrides() void ModeManagement::getModeStatus(uint32_t &valid_nav_state_mask, uint32_t &can_set_nav_state_mask) const { valid_nav_state_mask = mode_util::getValidNavStates(); - can_set_nav_state_mask = valid_nav_state_mask & ~(1u << vehicle_status_s::NAVIGATION_STATE_TERMINATION); + const uint32_t selectable_nav_state_mask = mode_util::getSelectableNavStates(); + can_set_nav_state_mask = valid_nav_state_mask & selectable_nav_state_mask & ~(1u << + vehicle_status_s::NAVIGATION_STATE_TERMINATION); // Add external modes for (int i = Modes::FIRST_EXTERNAL_NAV_STATE; i <= Modes::LAST_EXTERNAL_NAV_STATE; ++i) { diff --git a/src/modules/commander/ModeManagement.hpp b/src/modules/commander/ModeManagement.hpp index 95aca77686..d4b5ede787 100644 --- a/src/modules/commander/ModeManagement.hpp +++ b/src/modules/commander/ModeManagement.hpp @@ -213,7 +213,9 @@ public: void getModeStatus(uint32_t &valid_nav_state_mask, uint32_t &can_set_nav_state_mask) const { valid_nav_state_mask = mode_util::getValidNavStates(); - can_set_nav_state_mask = valid_nav_state_mask & ~(1u << vehicle_status_s::NAVIGATION_STATE_TERMINATION); + const uint32_t selectable_nav_state_mask = mode_util::getSelectableNavStates(); + can_set_nav_state_mask = valid_nav_state_mask & selectable_nav_state_mask & ~(1u << + vehicle_status_s::NAVIGATION_STATE_TERMINATION); } void updateActiveConfigOverrides(uint8_t nav_state, config_overrides_s &overrides_in_out) { } diff --git a/src/modules/commander/ModeUtil/ui.hpp b/src/modules/commander/ModeUtil/ui.hpp index 7eb3578b50..39852b3a61 100644 --- a/src/modules/commander/ModeUtil/ui.hpp +++ b/src/modules/commander/ModeUtil/ui.hpp @@ -65,6 +65,17 @@ static inline uint32_t getValidNavStates() static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 31, "code requires update"); } +/** + * @return Bitmask with all selectable modes + */ +static inline uint32_t getSelectableNavStates() +{ + return (1u << vehicle_status_s::NAVIGATION_STATE_ALTCTL) | + (1u << vehicle_status_s::NAVIGATION_STATE_POSCTL); + + static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 31, "code requires update"); +} + const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = { "Manual", "Altitude",