From 5be43c8e8a8ec119b09242c31fdc34206e03c3ea Mon Sep 17 00:00:00 2001 From: feoz0 Date: Mon, 4 Aug 2025 12:29:33 +0200 Subject: [PATCH 1/9] increased number of external mode to 16 --- msg/versioned/ArmingCheckReply.msg | 2 +- msg/versioned/VehicleStatus.msg | 10 +- src/lib/events/enums.json | 99 ++++++++++++++----- src/lib/modes/ui.hpp | 26 ++++- src/modules/commander/Commander.cpp | 2 +- .../HealthAndArmingChecks/Common.cpp | 16 +-- .../HealthAndArmingChecks/Common.hpp | 32 +++--- .../checks/externalChecks.hpp | 2 +- src/modules/commander/ModeManagement.hpp | 2 +- .../commander/ModeUtil/conversions.hpp | 18 +++- .../commander/ModeUtil/mode_requirements.cpp | 2 +- src/modules/commander/module.yaml | 8 ++ src/modules/commander/px4_custom_mode.h | 48 +++++++++ .../flight_mode_manager/FlightModeManager.cpp | 2 +- src/modules/manual_control/ManualControl.cpp | 8 ++ .../mavlink/streams/AVAILABLE_MODES.hpp | 2 +- 16 files changed, 221 insertions(+), 58 deletions(-) diff --git a/msg/versioned/ArmingCheckReply.msg b/msg/versioned/ArmingCheckReply.msg index 54e724bdb825..cdfa448bd83c 100644 --- a/msg/versioned/ArmingCheckReply.msg +++ b/msg/versioned/ArmingCheckReply.msg @@ -40,4 +40,4 @@ bool mode_req_home_position # Requires a home position (such as RTL/Return mode) bool mode_req_prevent_arming # Prevent arming (such as in Land mode). bool mode_req_manual_control # Requires a manual controller -uint8 ORB_QUEUE_LENGTH = 4 # +uint8 ORB_QUEUE_LENGTH = 32 # diff --git a/msg/versioned/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg index f433d564e96e..3f40b1885667 100644 --- a/msg/versioned/VehicleStatus.msg +++ b/msg/versioned/VehicleStatus.msg @@ -64,7 +64,15 @@ uint8 NAVIGATION_STATE_EXTERNAL5 = 27 uint8 NAVIGATION_STATE_EXTERNAL6 = 28 uint8 NAVIGATION_STATE_EXTERNAL7 = 29 uint8 NAVIGATION_STATE_EXTERNAL8 = 30 -uint8 NAVIGATION_STATE_MAX = 31 +uint8 NAVIGATION_STATE_EXTERNAL9 = 31 +uint8 NAVIGATION_STATE_EXTERNAL10 = 32 +uint8 NAVIGATION_STATE_EXTERNAL11 = 33 +uint8 NAVIGATION_STATE_EXTERNAL12 = 34 +uint8 NAVIGATION_STATE_EXTERNAL13 = 35 +uint8 NAVIGATION_STATE_EXTERNAL14 = 36 +uint8 NAVIGATION_STATE_EXTERNAL15 = 37 +uint8 NAVIGATION_STATE_EXTERNAL16 = 38 +uint8 NAVIGATION_STATE_MAX = 39 uint8 executor_in_charge # Current mode executor in charge (0=Autopilot) diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index 3b73cff62ca7..c909a4e6ac25 100644 --- a/src/lib/events/enums.json +++ b/src/lib/events/enums.json @@ -47,7 +47,7 @@ "namespace": "px4", "enums": { "navigation_mode_group_t": { - "type": "uint32_t", + "type": "uint64_t", "description": "Navigation/flight mode group bits. All modes within a group share the same arming checks. For now we directly map group == mode.", "is_bitfield": true, "entries": { @@ -142,6 +142,38 @@ "1073741824": { "name": "external8", "description": "External 8" + }, + "2147483648": { + "name": "external9", + "description": "External 9" + }, + "4294967296": { + "name": "external10", + "description": "External 10" + }, + "8589934592": { + "name": "external11", + "description": "External 11" + }, + "17179869184": { + "name": "external12", + "description": "External 12" + }, + "34359738368": { + "name": "external13", + "description": "External 13" + }, + "68719476736": { + "name": "external14", + "description": "External 14" + }, + "137438953472": { + "name": "external15", + "description": "External 15" + }, + "274877906944": { + "name": "external16", + "description": "External 16" } } }, @@ -604,6 +636,38 @@ "name": "external8", "description": "External 8" }, + "24": { + "name": "external9", + "description": "External 9" + }, + "25": { + "name": "external10", + "description": "External 10" + }, + "26": { + "name": "external11", + "description": "External 11" + }, + "27": { + "name": "external12", + "description": "External 12" + }, + "28": { + "name": "external13", + "description": "External 13" + }, + "29": { + "name": "external14", + "description": "External 14" + }, + "30": { + "name": "external15", + "description": "External 15" + }, + "31": { + "name": "external16", + "description": "External 16" + }, "255": { "name": "unknown", "description": "[Unknown]" @@ -727,29 +791,16 @@ }, "navigation_mode_groups": { "groups": { - "0": [65536], - "1": [131072], - "2": [196608], - "3": [67371008], - "4": [50593792], - "5": [84148224], - "10": [327680], - "14": [393216], - "15": [458752], - "17": [33816576], - "18": [100925440], - "19": [134479872], - "20": [151257088], - "21": [16973824], - "22": [168034304], - "23": [184811520], - "24": [201588736], - "25": [218365952], - "26": [235143168], - "27": [251920384], - "28": [268697600], - "29": [285474816], - "30": [302252032] + "30": [302252032], + "31": [536870912], + "32": [1073741824], + "33": [2147483648], + "34": [4294967296], + "35": [8589934592], + "36": [17179869184], + "37": [34359738368], + "38": [68719476736], + "39": [137438953472] } }, "supported_protocols": [ "health_and_arming_check" ] diff --git a/src/lib/modes/ui.hpp b/src/lib/modes/ui.hpp index 9e4156ae007a..edd121df92db 100644 --- a/src/lib/modes/ui.hpp +++ b/src/lib/modes/ui.hpp @@ -63,7 +63,7 @@ static inline uint32_t getValidNavStates() (1u << vehicle_status_s::NAVIGATION_STATE_ORBIT) | (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF); - static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 31, "update valid nav states"); + static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 39, "update valid nav states"); } const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = { @@ -98,6 +98,14 @@ const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = { "External 6", "External 7", "External 8", + "External 9", + "External 10", + "External 11", + "External 12", + "External 13", + "External 14", + "External 15", + "External 16", }; /** @@ -126,6 +134,22 @@ static inline bool isAdvanced(uint8_t nav_state) case vehicle_status_s::NAVIGATION_STATE_EXTERNAL8: return false; + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL9: return false; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL10: return false; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL11: return false; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL12: return false; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL13: return false; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL14: return false; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL15: return false; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL16: return false; + } return true; diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 764d90f8325b..f39ae3f0193d 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -838,7 +838,7 @@ Commander::handle_command(const vehicle_command_s &cmd) desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND; break; - case PX4_CUSTOM_SUB_MODE_EXTERNAL1...PX4_CUSTOM_SUB_MODE_EXTERNAL8: + case PX4_CUSTOM_SUB_MODE_EXTERNAL1...PX4_CUSTOM_SUB_MODE_EXTERNAL16: desired_nav_state = vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 + (custom_sub_mode - PX4_CUSTOM_SUB_MODE_EXTERNAL1); break; diff --git a/src/modules/commander/HealthAndArmingChecks/Common.cpp b/src/modules/commander/HealthAndArmingChecks/Common.cpp index 16e074ba1893..ac42097c0525 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.cpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.cpp @@ -43,11 +43,11 @@ void Report::getHealthReport(health_report_s &report) const for (int i = 0; i < vehicle_status_s::NAVIGATION_STATE_MAX; ++i) { NavModes group = getModeGroup(i); - if ((uint32_t)(current_results.arming_checks.can_arm & group)) { + if ((uint64_t)(current_results.arming_checks.can_arm & group)) { report.can_arm_mode_flags |= 1ull << i; } - if ((uint32_t)(current_results.arming_checks.can_run & group)) { + if ((uint64_t)(current_results.arming_checks.can_run & group)) { report.can_run_mode_flags |= 1ull << i; } } @@ -63,14 +63,14 @@ void Report::healthFailure(NavModes required_modes, HealthComponentIndex compone const events::LogLevels &log_levels, const char *message) { healthFailure(required_modes, component, log_levels.external); - addEvent(event_id, log_levels, message, (uint32_t)reportedModes(required_modes), component.index); + addEvent(event_id, log_levels, message, (uint64_t)reportedModes(required_modes), component.index); } void Report::armingCheckFailure(NavModes required_modes, HealthComponentIndex component, uint32_t event_id, const events::LogLevels &log_levels, const char *message) { armingCheckFailure(required_modes, component, log_levels.external); - addEvent(event_id, log_levels, message, (uint32_t)reportedModes(required_modes), component.index); + addEvent(event_id, log_levels, message, (uint64_t)reportedModes(required_modes), component.index); } void Report::armingCheckFailure(NavModesMessageFail required_modes, HealthComponentIndex component, @@ -78,11 +78,11 @@ void Report::armingCheckFailure(NavModesMessageFail required_modes, HealthCompon { armingCheckFailure(required_modes.fail_modes, component, log_levels.external); addEvent(event_id, log_levels, message, - (uint32_t)reportedModes(required_modes.message_modes | required_modes.fail_modes), component.index); + (uint64_t)reportedModes(required_modes.message_modes | required_modes.fail_modes), component.index); } Report::EventBufferHeader *Report::addEventToBuffer(uint32_t event_id, const events::LogLevels &log_levels, - uint32_t modes, unsigned args_size) + uint64_t modes, unsigned args_size) { unsigned total_size = sizeof(EventBufferHeader) + args_size; EventBufferHeader *header = (EventBufferHeader *)(_event_buffer + _next_buffer_idx); @@ -113,7 +113,7 @@ bool Report::addExternalEvent(const event_s &event, NavModes modes) events::LogLevels log_levels{events::externalLogLevel(event.log_levels), events::internalLogLevel((event.log_levels))}; memcpy(_event_buffer + _next_buffer_idx + sizeof(EventBufferHeader), &event.arguments, args_size); - addEventToBuffer(event.id, log_levels, (uint32_t)modes, args_size); + addEventToBuffer(event.id, log_levels, (uint64_t)modes, args_size); return true; } @@ -216,7 +216,7 @@ void Report::prepare(uint8_t vehicle_type) NavModes Report::getModeGroup(uint8_t nav_state) const { // Note: this needs to match with the json metadata definition "navigation_mode_groups" - return (NavModes)(1u << nav_state); + return static_cast(1ull << nav_state); } bool Report::finalize() diff --git a/src/modules/commander/HealthAndArmingChecks/Common.hpp b/src/modules/commander/HealthAndArmingChecks/Common.hpp index e069788ddf93..53ce6f91b50b 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.hpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.hpp @@ -58,15 +58,15 @@ class HealthAndArmingChecks; using navigation_mode_group_t = events::px4::enums::navigation_mode_group_t; using health_component_t = events::px4::enums::health_component_t; -enum class NavModes : uint32_t { +enum class NavModes : uint64_t { None = 0, ///< Using NavModes = None means arming is still possible (optional check) // Add the modes here as needed, but generally rather use mode requirements instead of checks for individual modes. - Manual = (uint32_t)navigation_mode_group_t::manual, - Stabilized = (uint32_t)navigation_mode_group_t::stab, - PositionControl = (uint32_t)navigation_mode_group_t::posctl, - Mission = (uint32_t)navigation_mode_group_t::mission, - Takeoff = (uint32_t)navigation_mode_group_t::takeoff, + Manual = (uint64_t)navigation_mode_group_t::manual, + Stabilized = (uint64_t)navigation_mode_group_t::stab, + PositionControl = (uint64_t)navigation_mode_group_t::posctl, + Mission = (uint64_t)navigation_mode_group_t::mission, + Takeoff = (uint64_t)navigation_mode_group_t::takeoff, All = 0xffffffff }; @@ -82,17 +82,17 @@ struct NavModesMessageFail { static inline NavModes operator|(NavModes a, NavModes b) { - return static_cast(static_cast(a) | static_cast(b)); + return static_cast(static_cast(a) | static_cast(b)); } static inline NavModes operator&(NavModes a, NavModes b) { - return static_cast(static_cast(a) & static_cast(b)); + return static_cast(static_cast(a) & static_cast(b)); } static inline NavModes operator~(NavModes a) { - return static_cast(~static_cast(a)); + return static_cast(~static_cast(a)); } class HealthComponentIndex @@ -189,8 +189,8 @@ class Report { error = {}; warning = {}; - can_arm = (NavModes) - 1; // bits are cleared for failed checks - can_run = (NavModes) - 1; + can_arm = static_cast(~0ull); // 0xFFFFFFFFFFFFFFFF + can_run = static_cast(~0ull); valid = false; } bool operator!=(const ArmingCheckResults &other) @@ -214,7 +214,7 @@ class Report bool canArm(uint8_t nav_state) const { return _results[_current_result].arming_checks.valid && - (uint32_t)(_results[_current_result].arming_checks.can_arm & getModeGroup(nav_state)) != 0; + (uint64_t)(_results[_current_result].arming_checks.can_arm & getModeGroup(nav_state)) != 0; } /** @@ -223,7 +223,7 @@ class Report bool canRun(uint8_t nav_state) const { return _results[_current_result].arming_checks.valid && - (uint32_t)(_results[_current_result].arming_checks.can_run & getModeGroup(nav_state)) != 0; + (uint64_t)(_results[_current_result].arming_checks.can_run & getModeGroup(nav_state)) != 0; } void getHealthReport(health_report_s &report) const; @@ -318,9 +318,9 @@ class Report void armingCheckFailure(NavModes required_modes, HealthComponentIndex component, events::Log log_level); template - bool addEvent(uint32_t event_id, const events::LogLevels &log_levels, const char *message, uint32_t modes, + bool addEvent(uint32_t event_id, const events::LogLevels &log_levels, const char *message, uint64_t modes, Args... args); - Report::EventBufferHeader *addEventToBuffer(uint32_t event_id, const events::LogLevels &log_levels, uint32_t modes, + Report::EventBufferHeader *addEventToBuffer(uint32_t event_id, const events::LogLevels &log_levels, uint64_t modes, unsigned args_size); NavModes reportedModes(NavModes required_modes); @@ -396,7 +396,7 @@ void Report::armingCheckFailure(NavModes required_modes, HealthComponentIndex co } template -bool Report::addEvent(uint32_t event_id, const events::LogLevels &log_levels, const char *message, uint32_t modes, +bool Report::addEvent(uint32_t event_id, const events::LogLevels &log_levels, const char *message, uint64_t modes, Args... args) { constexpr unsigned args_size = events::util::sizeofArguments(modes, args...); diff --git a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp index 2efa6896c3f2..b857441a14fb 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp @@ -43,7 +43,7 @@ class ExternalChecks : public HealthAndArmingCheckBase { public: - static constexpr int MAX_NUM_REGISTRATIONS = 8; + static constexpr int MAX_NUM_REGISTRATIONS = 16; ExternalChecks() = default; ~ExternalChecks() = default; diff --git a/src/modules/commander/ModeManagement.hpp b/src/modules/commander/ModeManagement.hpp index 4e733e5dc3b8..8ff68b2b5327 100644 --- a/src/modules/commander/ModeManagement.hpp +++ b/src/modules/commander/ModeManagement.hpp @@ -76,7 +76,7 @@ class Modes { public: static constexpr uint8_t FIRST_EXTERNAL_NAV_STATE = vehicle_status_s::NAVIGATION_STATE_EXTERNAL1; - static constexpr uint8_t LAST_EXTERNAL_NAV_STATE = vehicle_status_s::NAVIGATION_STATE_EXTERNAL8; + static constexpr uint8_t LAST_EXTERNAL_NAV_STATE = vehicle_status_s::NAVIGATION_STATE_EXTERNAL16; static constexpr int MAX_NUM = LAST_EXTERNAL_NAV_STATE - FIRST_EXTERNAL_NAV_STATE + 1; struct Mode { diff --git a/src/modules/commander/ModeUtil/conversions.hpp b/src/modules/commander/ModeUtil/conversions.hpp index 045db4570fa9..8efd83873a2b 100644 --- a/src/modules/commander/ModeUtil/conversions.hpp +++ b/src/modules/commander/ModeUtil/conversions.hpp @@ -93,9 +93,25 @@ static inline navigation_mode_t navigation_mode(uint8_t nav_state) case vehicle_status_s::NAVIGATION_STATE_EXTERNAL7: return navigation_mode_t::external7; case vehicle_status_s::NAVIGATION_STATE_EXTERNAL8: return navigation_mode_t::external8; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL9: return navigation_mode_t::external9; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL10: return navigation_mode_t::external10; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL11: return navigation_mode_t::external11; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL12: return navigation_mode_t::external12; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL13: return navigation_mode_t::external13; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL14: return navigation_mode_t::external14; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL15: return navigation_mode_t::external15; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL16: return navigation_mode_t::external16; } - static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 31, "update navigation mode map"); + static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 39, "update navigation mode map"); return navigation_mode_t::unknown; } diff --git a/src/modules/commander/ModeUtil/mode_requirements.cpp b/src/modules/commander/ModeUtil/mode_requirements.cpp index 9e64c529f208..4ca63960cd28 100644 --- a/src/modules/commander/ModeUtil/mode_requirements.cpp +++ b/src/modules/commander/ModeUtil/mode_requirements.cpp @@ -209,7 +209,7 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags) // NAVIGATION_STATE_EXTERNALx: handled outside - static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 31, "update mode requirements"); + static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 39, "update mode requirements"); } } // namespace mode_util diff --git a/src/modules/commander/module.yaml b/src/modules/commander/module.yaml index d2113d935955..9c34dd327c49 100644 --- a/src/modules/commander/module.yaml +++ b/src/modules/commander/module.yaml @@ -46,6 +46,14 @@ parameters: 105: External Mode 6 106: External Mode 7 107: External Mode 8 + 108: External Mode 9 + 109: External Mode 10 + 110: External Mode 11 + 111: External Mode 12 + 112: External Mode 13 + 113: External Mode 14 + 114: External Mode 15 + 115: External Mode 16 instance_start: 1 num_instances: 6 default: -1 diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index e4936982ff1b..75c7ad136db9 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -73,6 +73,14 @@ enum PX4_CUSTOM_SUB_MODE_AUTO { PX4_CUSTOM_SUB_MODE_EXTERNAL6, PX4_CUSTOM_SUB_MODE_EXTERNAL7, PX4_CUSTOM_SUB_MODE_EXTERNAL8, + PX4_CUSTOM_SUB_MODE_EXTERNAL9, + PX4_CUSTOM_SUB_MODE_EXTERNAL10, + PX4_CUSTOM_SUB_MODE_EXTERNAL11, + PX4_CUSTOM_SUB_MODE_EXTERNAL12, + PX4_CUSTOM_SUB_MODE_EXTERNAL13, + PX4_CUSTOM_SUB_MODE_EXTERNAL14, + PX4_CUSTOM_SUB_MODE_EXTERNAL15, + PX4_CUSTOM_SUB_MODE_EXTERNAL16 }; enum PX4_CUSTOM_SUB_MODE_POSCTL { @@ -226,6 +234,46 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state) custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL8; break; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL9: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL9; + break; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL10: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL10; + break; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL11: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL11; + break; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL12: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL12; + break; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL13: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL13; + break; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL14: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL14; + break; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL15: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL15; + break; + + case vehicle_status_s::NAVIGATION_STATE_EXTERNAL16: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL16; + break; } return custom_mode; diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index f17e00aa9629..f2778c1212ee 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -138,7 +138,7 @@ void FlightModeManager::start_flight_task() // Do not run any flight task for VTOLs in fixed-wing mode if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) || ((_vehicle_status_sub.get().nav_state >= vehicle_status_s::NAVIGATION_STATE_EXTERNAL1) - && (_vehicle_status_sub.get().nav_state <= vehicle_status_s::NAVIGATION_STATE_EXTERNAL8))) { + && (_vehicle_status_sub.get().nav_state <= vehicle_status_s::NAVIGATION_STATE_EXTERNAL16))) { switchTask(FlightTaskIndex::None); return; } diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index 082b2ccaa3f3..fb5d0d417df9 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -599,6 +599,14 @@ int8_t ManualControl::navStateFromParam(int32_t param_value) 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; + case 108: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL9; + case 109: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL10; + case 110: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL11; + case 111: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL12; + case 112: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL13; + case 113: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL14; + case 114: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL15; + case 115: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL16; } return -1; } diff --git a/src/modules/mavlink/streams/AVAILABLE_MODES.hpp b/src/modules/mavlink/streams/AVAILABLE_MODES.hpp index ae077c823991..3a57121e1948 100644 --- a/src/modules/mavlink/streams/AVAILABLE_MODES.hpp +++ b/src/modules/mavlink/streams/AVAILABLE_MODES.hpp @@ -58,7 +58,7 @@ class MavlinkStreamAvailableModes : public MavlinkStream } private: - static constexpr int MAX_NUM_EXTERNAL_MODES = vehicle_status_s::NAVIGATION_STATE_EXTERNAL8 - + static constexpr int MAX_NUM_EXTERNAL_MODES = vehicle_status_s::NAVIGATION_STATE_EXTERNAL16 - vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 + 1; explicit MavlinkStreamAvailableModes(Mavlink *mavlink) : MavlinkStream(mavlink) {} From 69382463f347b227b2e8a95ff41aa1d585f5cdce Mon Sep 17 00:00:00 2001 From: feoz0 Date: Tue, 5 Aug 2025 14:42:26 +0200 Subject: [PATCH 2/9] changed fully to uint64 --- msg/FailsafeFlags.msg | 28 +++++++++---------- .../HealthAndArmingChecks/Common.cpp | 2 ++ .../HealthAndArmingChecks/Common.hpp | 4 +-- .../checks/externalChecks.cpp | 9 +++--- .../commander/ModeUtil/mode_requirements.cpp | 4 +-- src/modules/commander/failsafe/framework.cpp | 2 +- src/modules/mavlink/streams/SYS_STATUS.hpp | 2 +- 7 files changed, 26 insertions(+), 25 deletions(-) diff --git a/msg/FailsafeFlags.msg b/msg/FailsafeFlags.msg index 243a4b5c9e20..d7301a799f6f 100644 --- a/msg/FailsafeFlags.msg +++ b/msg/FailsafeFlags.msg @@ -6,20 +6,20 @@ uint64 timestamp # time since system start (microseconds) # Per-mode requirements -uint32 mode_req_angular_velocity -uint32 mode_req_attitude -uint32 mode_req_local_alt -uint32 mode_req_local_position -uint32 mode_req_local_position_relaxed -uint32 mode_req_global_position -uint32 mode_req_global_position_relaxed -uint32 mode_req_mission -uint32 mode_req_offboard_signal -uint32 mode_req_home_position -uint32 mode_req_wind_and_flight_time_compliance # if set, mode cannot be entered if wind or flight time limit exceeded -uint32 mode_req_prevent_arming # if set, cannot arm while in this mode -uint32 mode_req_manual_control -uint32 mode_req_other # other requirements, not covered above (for external modes) +uint64 mode_req_angular_velocity +uint64 mode_req_attitude +uint64 mode_req_local_alt +uint64 mode_req_local_position +uint64 mode_req_local_position_relaxed +uint64 mode_req_global_position +uint64 mode_req_global_position_relaxed +uint64 mode_req_mission +uint64 mode_req_offboard_signal +uint64 mode_req_home_position +uint64 mode_req_wind_and_flight_time_compliance # if set, mode cannot be entered if wind or flight time limit exceeded +uint64 mode_req_prevent_arming # if set, cannot arm while in this mode +uint64 mode_req_manual_control +uint64 mode_req_other # other requirements, not covered above (for external modes) # Mode requirements diff --git a/src/modules/commander/HealthAndArmingChecks/Common.cpp b/src/modules/commander/HealthAndArmingChecks/Common.cpp index ac42097c0525..7ed5a3aa3936 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.cpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.cpp @@ -50,6 +50,8 @@ void Report::getHealthReport(health_report_s &report) const if ((uint64_t)(current_results.arming_checks.can_run & group)) { report.can_run_mode_flags |= 1ull << i; } + + } report.arming_check_error_flags = (uint64_t)current_results.arming_checks.error; diff --git a/src/modules/commander/HealthAndArmingChecks/Common.hpp b/src/modules/commander/HealthAndArmingChecks/Common.hpp index 53ce6f91b50b..e5ed63812521 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.hpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.hpp @@ -384,7 +384,7 @@ void Report::healthFailure(NavModes required_modes, HealthComponentIndex compone const events::LogLevels &log_levels, const char *message, Args... args) { healthFailure(required_modes, component, log_levels.external); - addEvent(event_id, log_levels, message, (uint32_t)reportedModes(required_modes), (uint8_t)component.index, args...); + addEvent(event_id, log_levels, message, (uint64_t)reportedModes(required_modes), (uint8_t)component.index, args...); } template @@ -392,7 +392,7 @@ void Report::armingCheckFailure(NavModes required_modes, HealthComponentIndex co const events::LogLevels &log_levels, const char *message, Args... args) { armingCheckFailure(required_modes, component, log_levels.external); - addEvent(event_id, log_levels, message, (uint32_t)reportedModes(required_modes), (uint8_t)component.index, args...); + addEvent(event_id, log_levels, message, (uint64_t)reportedModes(required_modes), (uint8_t)component.index, args...); } template diff --git a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp index b1f2abc064c6..73d912507989 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp @@ -33,18 +33,17 @@ #include "externalChecks.hpp" -static void setOrClearRequirementBits(bool requirement_set, int8_t nav_state, int8_t replaces_nav_state, uint32_t &bits) +static void setOrClearRequirementBits(bool requirement_set, int8_t nav_state, int8_t replaces_nav_state, uint64_t &bits) { if (requirement_set) { - bits |= 1u << nav_state; + bits |= 1ull << nav_state; } if (replaces_nav_state != -1) { if (requirement_set) { - bits |= 1u << replaces_nav_state; - + bits |= 1ull << replaces_nav_state; } else { - bits &= ~(1u << replaces_nav_state); + bits &= ~(1ull << replaces_nav_state); } } } diff --git a/src/modules/commander/ModeUtil/mode_requirements.cpp b/src/modules/commander/ModeUtil/mode_requirements.cpp index 4ca63960cd28..31262c6d37dc 100644 --- a/src/modules/commander/ModeUtil/mode_requirements.cpp +++ b/src/modules/commander/ModeUtil/mode_requirements.cpp @@ -37,9 +37,9 @@ namespace mode_util { -static inline void setRequirement(uint8_t nav_state, uint32_t &mode_requirement) +static inline void setRequirement(uint8_t nav_state, uint64_t &mode_requirement) { - mode_requirement |= 1u << nav_state; + mode_requirement |= 1ull << nav_state; } diff --git a/src/modules/commander/failsafe/framework.cpp b/src/modules/commander/failsafe/framework.cpp index ab3e30c84728..f1e28ac80176 100644 --- a/src/modules/commander/failsafe/framework.cpp +++ b/src/modules/commander/failsafe/framework.cpp @@ -698,7 +698,7 @@ uint8_t FailsafeBase::modeFromAction(const Action &action, uint8_t user_intended bool FailsafeBase::modeCanRun(const failsafe_flags_s &status_flags, uint8_t mode) { - uint32_t mode_mask = 1u << mode; + uint64_t mode_mask = 1ull << mode; // mode_req_wind_and_flight_time_compliance: does not need to be handled here (these are separate failsafe triggers) // mode_req_manual_control: is handled separately return diff --git a/src/modules/mavlink/streams/SYS_STATUS.hpp b/src/modules/mavlink/streams/SYS_STATUS.hpp index 7a89501793b6..15bf58bb91a3 100644 --- a/src/modules/mavlink/streams/SYS_STATUS.hpp +++ b/src/modules/mavlink/streams/SYS_STATUS.hpp @@ -137,7 +137,7 @@ class MavlinkStreamSysStatus : public MavlinkStream mavlink_sys_status_t msg{}; - if (health_report.can_arm_mode_flags & (1u << status.nav_state)) { + if (health_report.can_arm_mode_flags & (1ull << status.nav_state)) { msg.onboard_control_sensors_health |= MAV_SYS_STATUS_PREARM_CHECK; } From 2e53c54b0875db19ebd1988f3d8852cb69dda5f6 Mon Sep 17 00:00:00 2001 From: feoz0 Date: Tue, 5 Aug 2025 14:45:51 +0200 Subject: [PATCH 3/9] format --- .../commander/HealthAndArmingChecks/checks/externalChecks.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp index 73d912507989..b5a3327f9488 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp @@ -42,6 +42,7 @@ static void setOrClearRequirementBits(bool requirement_set, int8_t nav_state, in if (replaces_nav_state != -1) { if (requirement_set) { bits |= 1ull << replaces_nav_state; + } else { bits &= ~(1ull << replaces_nav_state); } From 024bdfac6998a559b52195e2264dced935ea59ea Mon Sep 17 00:00:00 2001 From: feoz0 Date: Tue, 5 Aug 2025 15:09:20 +0200 Subject: [PATCH 4/9] fixed definition of all modes to uint64 --- src/modules/commander/HealthAndArmingChecks/Common.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/HealthAndArmingChecks/Common.hpp b/src/modules/commander/HealthAndArmingChecks/Common.hpp index e5ed63812521..b23ee3efcc6c 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.hpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.hpp @@ -68,7 +68,7 @@ enum class NavModes : uint64_t { Mission = (uint64_t)navigation_mode_group_t::mission, Takeoff = (uint64_t)navigation_mode_group_t::takeoff, - All = 0xffffffff + All = ~0ull }; static_assert(sizeof(navigation_mode_group_t) == sizeof(NavModes), "type mismatch"); static_assert(vehicle_status_s::NAVIGATION_STATE_MAX <= CHAR_BIT *sizeof(navigation_mode_group_t), From 6a7a768adac27a325a96e4b53af0e67d49e8e159 Mon Sep 17 00:00:00 2001 From: feoz0 Date: Tue, 5 Aug 2025 15:09:54 +0200 Subject: [PATCH 5/9] fixed CI not to force 8 external modes --- .../HealthAndArmingChecks/HealthAndArmingChecksTest.cpp | 9 +++++++-- src/modules/commander/module.yaml | 2 +- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp index 4bd7749ce435..c78c435e2e7f 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp @@ -72,7 +72,10 @@ TEST_F(ReporterTest, basic_no_checks) reporter.report(false); ASSERT_TRUE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)); - ASSERT_EQ((uint8_t)reporter.armingCheckResults().can_arm, 0xff); + ASSERT_EQ(static_cast(reporter.armingCheckResults().can_arm), + static_cast(NavModes::All)); + + ASSERT_EQ((uint64_t)reporter.armingCheckResults().error, 0); ASSERT_EQ((uint64_t)reporter.armingCheckResults().warning, 0); @@ -142,7 +145,9 @@ TEST_F(ReporterTest, arming_checks_mode_category2) ASSERT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)); - ASSERT_EQ((uint8_t)reporter.armingCheckResults().can_arm, (uint8_t)~(NavModes::Mission)); + ASSERT_EQ((uint64_t)reporter.armingCheckResults().can_arm, + static_cast(~NavModes::Mission)); + ASSERT_EQ((uint64_t)reporter.armingCheckResults().error, 0); ASSERT_EQ((uint64_t)reporter.armingCheckResults().warning, 0); diff --git a/src/modules/commander/module.yaml b/src/modules/commander/module.yaml index 9c34dd327c49..149c7769499e 100644 --- a/src/modules/commander/module.yaml +++ b/src/modules/commander/module.yaml @@ -10,7 +10,7 @@ parameters: 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) + num_instances: 16 # Max 8 modes (NAVIGATION_STATE_EXTERNAL8) default: 0 volatile: true category: System From c3bb581bfdff26ebadfdb312205aafa6787e4045 Mon Sep 17 00:00:00 2001 From: feoz0 Date: Wed, 13 Aug 2025 10:54:19 +0200 Subject: [PATCH 6/9] fixes --- msg/versioned/ArmingCheckReply.msg | 2 +- src/lib/events/enums.json | 21 +++++++++++++++++++ src/modules/commander/failsafe/failsafe.cpp | 10 ++++++++- src/modules/commander/module.yaml | 2 +- .../fw_mode_manager/FixedWingModeManager.cpp | 2 +- 5 files changed, 33 insertions(+), 4 deletions(-) diff --git a/msg/versioned/ArmingCheckReply.msg b/msg/versioned/ArmingCheckReply.msg index cdfa448bd83c..3f99fc95f64f 100644 --- a/msg/versioned/ArmingCheckReply.msg +++ b/msg/versioned/ArmingCheckReply.msg @@ -40,4 +40,4 @@ bool mode_req_home_position # Requires a home position (such as RTL/Return mode) bool mode_req_prevent_arming # Prevent arming (such as in Land mode). bool mode_req_manual_control # Requires a manual controller -uint8 ORB_QUEUE_LENGTH = 32 # +uint8 ORB_QUEUE_LENGTH = 16 # diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index c909a4e6ac25..5d743213238f 100644 --- a/src/lib/events/enums.json +++ b/src/lib/events/enums.json @@ -791,6 +791,27 @@ }, "navigation_mode_groups": { "groups": { + "1": [131072], + "2": [196608], + "3": [67371008], + "4": [50593792], + "5": [84148224], + "10": [327680], + "14": [393216], + "15": [458752], + "17": [33816576], + "18": [100925440], + "19": [134479872], + "20": [151257088], + "21": [16973824], + "22": [168034304], + "23": [184811520], + "24": [201588736], + "25": [218365952], + "26": [235143168], + "27": [251920384], + "28": [268697600], + "29": [285474816], "30": [302252032], "31": [536870912], "32": [1073741824], diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index d692745c42e2..98db533f3692 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -480,7 +480,15 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL5 || state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL6 || state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL7 || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL8) + state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL8 || + state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL9 || + state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL10 || + state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL11 || + state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL12 || + state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL13 || + state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL14 || + state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL15 || + state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL16 ) && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::ExternalMode); const bool rc_loss_ignored = rc_loss_ignored_mission || rc_loss_ignored_loiter || rc_loss_ignored_offboard || diff --git a/src/modules/commander/module.yaml b/src/modules/commander/module.yaml index 149c7769499e..5055ec1d25df 100644 --- a/src/modules/commander/module.yaml +++ b/src/modules/commander/module.yaml @@ -10,7 +10,7 @@ parameters: 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: 16 # Max 8 modes (NAVIGATION_STATE_EXTERNAL8) + num_instances: 16 # Max 16 modes (NAVIGATION_STATE_EXTERNAL16) default: 0 volatile: true category: System diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.cpp b/src/modules/fw_mode_manager/FixedWingModeManager.cpp index 5ee3c817fdb2..a809793dcb11 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.cpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.cpp @@ -1968,7 +1968,7 @@ FixedWingModeManager::Run() /* only run controller if position changed and we are not running an external mode*/ const bool is_external_nav_state = (_nav_state >= vehicle_status_s::NAVIGATION_STATE_EXTERNAL1) - && (_nav_state <= vehicle_status_s::NAVIGATION_STATE_EXTERNAL8); + && (_nav_state <= vehicle_status_s::NAVIGATION_STATE_EXTERNAL16); if (is_external_nav_state) { // this will cause the configuration handler to publish immediately the next time an internal flight From afc4c9907e8e2f142ce0b2259a64fc3f6b1536c1 Mon Sep 17 00:00:00 2001 From: feoz0 Date: Wed, 13 Aug 2025 11:05:27 +0200 Subject: [PATCH 7/9] format --- src/modules/commander/failsafe/failsafe.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 98db533f3692..45c2e3eda5bd 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -488,7 +488,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL13 || state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL14 || state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL15 || - state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL16 ) + state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_EXTERNAL16) && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::ExternalMode); const bool rc_loss_ignored = rc_loss_ignored_mission || rc_loss_ignored_loiter || rc_loss_ignored_offboard || From c55b26a7b8685dbdaff2ea8a422294b4afc9f457 Mon Sep 17 00:00:00 2001 From: feoz0 Date: Thu, 14 Aug 2025 09:27:47 +0200 Subject: [PATCH 8/9] fix --- src/lib/events/enums.json | 1 + 1 file changed, 1 insertion(+) diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index 5d743213238f..5b124f8a45a1 100644 --- a/src/lib/events/enums.json +++ b/src/lib/events/enums.json @@ -791,6 +791,7 @@ }, "navigation_mode_groups": { "groups": { + "0": [65536], "1": [131072], "2": [196608], "3": [67371008], From 6cc952e8a4aa78e4a0f9c3faca379abda52b3a50 Mon Sep 17 00:00:00 2001 From: feoz0 Date: Mon, 25 Aug 2025 09:43:35 +0200 Subject: [PATCH 9/9] fix --- msg/versioned/ArmingCheckReply.msg | 1 - 1 file changed, 1 deletion(-) diff --git a/msg/versioned/ArmingCheckReply.msg b/msg/versioned/ArmingCheckReply.msg index e0c1cf518b75..6b23e4ab810b 100644 --- a/msg/versioned/ArmingCheckReply.msg +++ b/msg/versioned/ArmingCheckReply.msg @@ -42,4 +42,3 @@ bool mode_req_prevent_arming # Prevent arming (such as in Land mode). bool mode_req_manual_control # Requires a manual controller uint8 ORB_QUEUE_LENGTH = 16 -