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/msg/versioned/ArmingCheckReply.msg b/msg/versioned/ArmingCheckReply.msg index 7ba4ad7bf37b..6b23e4ab810b 100644 --- a/msg/versioned/ArmingCheckReply.msg +++ b/msg/versioned/ArmingCheckReply.msg @@ -28,6 +28,7 @@ uint8 num_events # Number of queued failure messages (Event) in the events fiel Event[5] events # Arming failure reasons (Queue of events to report to GCS). # Mode requirements + bool mode_req_angular_velocity # Requires angular velocity estimate (e.g. from gyroscope). bool mode_req_attitude # Requires an attitude estimate. bool mode_req_local_alt # Requires a local altitude estimate. @@ -40,4 +41,4 @@ bool mode_req_home_position # Requires a home position (such as RTL/R 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 = 16 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..5b124f8a45a1 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]" @@ -749,7 +813,16 @@ "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 0e2c50ec4f79..bc4ef36e61d8 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..7ed5a3aa3936 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.cpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.cpp @@ -43,13 +43,15 @@ 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; } + + } report.arming_check_error_flags = (uint64_t)current_results.arming_checks.error; @@ -63,14 +65,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 +80,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 +115,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 +218,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..b23ee3efcc6c 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.hpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.hpp @@ -58,17 +58,17 @@ 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 + 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), @@ -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); @@ -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,11 +392,11 @@ 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 -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/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/HealthAndArmingChecks/checks/externalChecks.cpp b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp index b1f2abc064c6..b5a3327f9488 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp @@ -33,18 +33,18 @@ #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/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 52eff77dee3e..ed3e055f6fa5 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..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; } @@ -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/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 5c3578fb8943..a8fd4014fa3f 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/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/commander/module.yaml b/src/modules/commander/module.yaml index d2113d935955..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: 8 # Max 8 modes (NAVIGATION_STATE_EXTERNAL8) + num_instances: 16 # Max 16 modes (NAVIGATION_STATE_EXTERNAL16) default: 0 volatile: true category: System @@ -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/fw_mode_manager/FixedWingModeManager.cpp b/src/modules/fw_mode_manager/FixedWingModeManager.cpp index 84767f0368ad..977de4e700c1 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 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 c0d7730188f0..a5f865e3c78c 100644 --- a/src/modules/mavlink/streams/AVAILABLE_MODES.hpp +++ b/src/modules/mavlink/streams/AVAILABLE_MODES.hpp @@ -62,7 +62,7 @@ class MavlinkStreamAvailableModes : public MavlinkStream static constexpr uint32_t MAX_DELAY_US = 2500000 / vehicle_status_s::NAVIGATION_STATE_MAX; /* Only delay if the transmit of one mode takes at least 1ms, this avoids a lot of fast delay calls */ static constexpr uint32_t MIN_DELAY_THRESHOLD = 1000; - 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) {} 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; }