From 2d9a981e440897179df6f040629e374ed9b3d04e Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 15 Oct 2025 13:27:50 +0200 Subject: [PATCH 1/4] Commander: add asserts for the match of arm_disarm_reason enums of events and uORB that's the failing test because of which the (dis)arm reason can be wrong based on the message definition. --- src/modules/commander/Commander.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 663223c9bf79..a11f1e93fea6 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -511,6 +511,25 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[]) static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_reason) { + static_assert((uint8_t)arm_disarm_reason_t::transition_to_standby == + vehicle_status_s::ARM_DISARM_REASON_TRANSITION_TO_STANDBY); + static_assert((uint8_t)arm_disarm_reason_t::stick_gesture == vehicle_status_s::ARM_DISARM_REASON_STICK_GESTURE); + static_assert((uint8_t)arm_disarm_reason_t::rc_switch == vehicle_status_s::ARM_DISARM_REASON_RC_SWITCH); + static_assert((uint8_t)arm_disarm_reason_t::command_internal == vehicle_status_s::ARM_DISARM_REASON_COMMAND_INTERNAL); + static_assert((uint8_t)arm_disarm_reason_t::command_external == vehicle_status_s::ARM_DISARM_REASON_COMMAND_EXTERNAL); + static_assert((uint8_t)arm_disarm_reason_t::mission_start == vehicle_status_s::ARM_DISARM_REASON_MISSION_START); + // static_assert((uint8_t)arm_disarm_reason_t:: == vehicle_status_s::ARM_DISARM_REASON_SAFETY_BUTTON); + static_assert((uint8_t)arm_disarm_reason_t::auto_disarm_land == vehicle_status_s::ARM_DISARM_REASON_AUTO_DISARM_LAND); + static_assert((uint8_t)arm_disarm_reason_t::auto_disarm_preflight == + vehicle_status_s::ARM_DISARM_REASON_AUTO_DISARM_PREFLIGHT); + static_assert((uint8_t)arm_disarm_reason_t::kill_switch == vehicle_status_s::ARM_DISARM_REASON_KILL_SWITCH); + static_assert((uint8_t)arm_disarm_reason_t::lockdown == vehicle_status_s::ARM_DISARM_REASON_LOCKDOWN); + static_assert((uint8_t)arm_disarm_reason_t::failure_detector == vehicle_status_s::ARM_DISARM_REASON_FAILURE_DETECTOR); + static_assert((uint8_t)arm_disarm_reason_t::shutdown == vehicle_status_s::ARM_DISARM_REASON_SHUTDOWN); + static_assert((uint8_t)arm_disarm_reason_t::unit_test == vehicle_status_s::ARM_DISARM_REASON_UNIT_TEST); + // static_assert((uint8_t)arm_disarm_reason_t::rc_button == vehicle_status_s::); + // static_assert((uint8_t)arm_disarm_reason_t::failsafe == vehicle_status_s::); + switch (calling_reason) { case arm_disarm_reason_t::transition_to_standby: return ""; From f64f444d4939055abcac4b24be31fd3e41340d46 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 15 Oct 2025 14:39:29 +0200 Subject: [PATCH 2/4] Update (dis)arm reason enum accross uORB, events, Commander - removing deprecated ones - syncing used ones to the enum value events defined already - make naming more consistent --- msg/versioned/VehicleStatus.msg | 15 +++++------- src/lib/events/enums.json | 26 ++++----------------- src/modules/commander/Commander.cpp | 36 +++++++++-------------------- 3 files changed, 22 insertions(+), 55 deletions(-) diff --git a/msg/versioned/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg index 0dffdc2c92de..80ff1618bf07 100644 --- a/msg/versioned/VehicleStatus.msg +++ b/msg/versioned/VehicleStatus.msg @@ -13,20 +13,17 @@ uint8 ARMING_STATE_ARMED = 2 uint8 latest_arming_reason uint8 latest_disarming_reason -uint8 ARM_DISARM_REASON_TRANSITION_TO_STANDBY = 0 uint8 ARM_DISARM_REASON_STICK_GESTURE = 1 uint8 ARM_DISARM_REASON_RC_SWITCH = 2 uint8 ARM_DISARM_REASON_COMMAND_INTERNAL = 3 uint8 ARM_DISARM_REASON_COMMAND_EXTERNAL = 4 uint8 ARM_DISARM_REASON_MISSION_START = 5 -uint8 ARM_DISARM_REASON_SAFETY_BUTTON = 6 -uint8 ARM_DISARM_REASON_AUTO_DISARM_LAND = 7 -uint8 ARM_DISARM_REASON_AUTO_DISARM_PREFLIGHT = 8 -uint8 ARM_DISARM_REASON_KILL_SWITCH = 9 -uint8 ARM_DISARM_REASON_LOCKDOWN = 10 -uint8 ARM_DISARM_REASON_FAILURE_DETECTOR = 11 -uint8 ARM_DISARM_REASON_SHUTDOWN = 12 -uint8 ARM_DISARM_REASON_UNIT_TEST = 13 +uint8 ARM_DISARM_REASON_LANDING = 6 +uint8 ARM_DISARM_REASON_PREFLIGHT_INACTION = 7 +uint8 ARM_DISARM_REASON_KILL_SWITCH = 8 +uint8 ARM_DISARM_REASON_LOCKDOWN = 9 +uint8 ARM_DISARM_REASON_RC_BUTTON = 13 +uint8 ARM_DISARM_REASON_FAILSAFE = 14 uint64 nav_state_timestamp # time when current nav_state activated diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index c3cbc2578a7c..955016465e9c 100644 --- a/src/lib/events/enums.json +++ b/src/lib/events/enums.json @@ -442,13 +442,9 @@ "type": "uint8_t", "description": "Reason for arming/disarming", "entries": { - "0": { - "name": "transition_to_standby", - "description": "Transition to standby" - }, "1": { "name": "stick_gesture", - "description": "Stick gesture" + "description": "stick gesture" }, "2": { "name": "rc_switch", @@ -467,12 +463,12 @@ "description": "mission start" }, "6": { - "name": "auto_disarm_land", + "name": "landing", "description": "landing" }, "7": { - "name": "auto_disarm_preflight", - "description": "auto preflight disarming" + "name": "preflight_inaction", + "description": "preflight inaction" }, "8": { "name": "kill_switch", @@ -482,21 +478,9 @@ "name": "lockdown", "description": "lockdown" }, - "10": { - "name": "failure_detector", - "description": "failure detector" - }, - "11": { - "name": "shutdown", - "description": "shutdown request" - }, - "12": { - "name": "unit_test", - "description": "unit tests" - }, "13": { "name": "rc_button", - "description": "RC (button)" + "description": "RC button" }, "14": { "name": "failsafe", diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index a11f1e93fea6..9d4e933cb864 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -511,29 +511,21 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[]) static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_reason) { - static_assert((uint8_t)arm_disarm_reason_t::transition_to_standby == - vehicle_status_s::ARM_DISARM_REASON_TRANSITION_TO_STANDBY); static_assert((uint8_t)arm_disarm_reason_t::stick_gesture == vehicle_status_s::ARM_DISARM_REASON_STICK_GESTURE); static_assert((uint8_t)arm_disarm_reason_t::rc_switch == vehicle_status_s::ARM_DISARM_REASON_RC_SWITCH); static_assert((uint8_t)arm_disarm_reason_t::command_internal == vehicle_status_s::ARM_DISARM_REASON_COMMAND_INTERNAL); static_assert((uint8_t)arm_disarm_reason_t::command_external == vehicle_status_s::ARM_DISARM_REASON_COMMAND_EXTERNAL); static_assert((uint8_t)arm_disarm_reason_t::mission_start == vehicle_status_s::ARM_DISARM_REASON_MISSION_START); - // static_assert((uint8_t)arm_disarm_reason_t:: == vehicle_status_s::ARM_DISARM_REASON_SAFETY_BUTTON); - static_assert((uint8_t)arm_disarm_reason_t::auto_disarm_land == vehicle_status_s::ARM_DISARM_REASON_AUTO_DISARM_LAND); - static_assert((uint8_t)arm_disarm_reason_t::auto_disarm_preflight == - vehicle_status_s::ARM_DISARM_REASON_AUTO_DISARM_PREFLIGHT); + static_assert((uint8_t)arm_disarm_reason_t::landing == vehicle_status_s::ARM_DISARM_REASON_LANDING); + static_assert( + (uint8_t)arm_disarm_reason_t::preflight_inaction == vehicle_status_s::ARM_DISARM_REASON_PREFLIGHT_INACTION); static_assert((uint8_t)arm_disarm_reason_t::kill_switch == vehicle_status_s::ARM_DISARM_REASON_KILL_SWITCH); static_assert((uint8_t)arm_disarm_reason_t::lockdown == vehicle_status_s::ARM_DISARM_REASON_LOCKDOWN); - static_assert((uint8_t)arm_disarm_reason_t::failure_detector == vehicle_status_s::ARM_DISARM_REASON_FAILURE_DETECTOR); - static_assert((uint8_t)arm_disarm_reason_t::shutdown == vehicle_status_s::ARM_DISARM_REASON_SHUTDOWN); - static_assert((uint8_t)arm_disarm_reason_t::unit_test == vehicle_status_s::ARM_DISARM_REASON_UNIT_TEST); - // static_assert((uint8_t)arm_disarm_reason_t::rc_button == vehicle_status_s::); - // static_assert((uint8_t)arm_disarm_reason_t::failsafe == vehicle_status_s::); + static_assert((uint8_t)arm_disarm_reason_t::rc_button == vehicle_status_s::ARM_DISARM_REASON_RC_BUTTON); + static_assert((uint8_t)arm_disarm_reason_t::failsafe == vehicle_status_s::ARM_DISARM_REASON_FAILSAFE); switch (calling_reason) { - case arm_disarm_reason_t::transition_to_standby: return ""; - - case arm_disarm_reason_t::stick_gesture: return "Stick gesture"; + case arm_disarm_reason_t::stick_gesture: return "stick gesture"; case arm_disarm_reason_t::rc_switch: return "RC switch"; @@ -543,21 +535,15 @@ static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_r case arm_disarm_reason_t::mission_start: return "mission start"; - case arm_disarm_reason_t::auto_disarm_land: return "landing"; + case arm_disarm_reason_t::landing: return "landing"; - case arm_disarm_reason_t::auto_disarm_preflight: return "auto preflight disarming"; + case arm_disarm_reason_t::preflight_inaction: return "auto preflight disarming"; case arm_disarm_reason_t::kill_switch: return "kill-switch"; case arm_disarm_reason_t::lockdown: return "lockdown"; - case arm_disarm_reason_t::failure_detector: return "failure detector"; - - case arm_disarm_reason_t::shutdown: return "shutdown request"; - - case arm_disarm_reason_t::unit_test: return "unit tests"; - - case arm_disarm_reason_t::rc_button: return "RC (button)"; + case arm_disarm_reason_t::rc_button: return "RC button"; case arm_disarm_reason_t::failsafe: return "failsafe"; } @@ -2318,10 +2304,10 @@ void Commander::handleAutoDisarm() if (_auto_disarm_landed.get_state() && !_multicopter_throw_launch.isThrowLaunchInProgress()) { if (_have_taken_off_since_arming) { - disarm(arm_disarm_reason_t::auto_disarm_land); + disarm(arm_disarm_reason_t::landing); } else { - disarm(arm_disarm_reason_t::auto_disarm_preflight); + disarm(arm_disarm_reason_t::preflight_inaction); } } } From 9f4b9c6c2a30c802d3f91ddc4171f9361932358d Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 15 Oct 2025 14:56:32 +0200 Subject: [PATCH 3/4] Commander: remove auto disarm by lockdown 1. It was added because the action for ESCs not arming in time was lockdown and in this case it should "also" disarm (why not disarm directly in the first place?) see a7f2f2908b16edd2c56fae9b93f4d421384dc884 2. but lockdown was also used such that motors don't spin in HITL which then caused auto disarm hence a workaround see 1ac31100cc135569043976976bd6451279245cf5 3. and in the big commander failsafe rewrite (#20172) the ESCs not arming case was treated as a failsafe that disarms and not using lockdown anymore 4. and then also the multicopter throw launch (#21170) started using lockdown to stop motors before throw which had to be excluded similar to the workaround in 2. So as a result the cases where lockdown is used now are HITL and throw launch, both don't want to cause an automatic disarm hence they are both exluded and the lockdown disarm should not be reachable and can be removed. Note: The case where you kill during a throw launch and get an automatic disarm is now correctly handled in my eyes. Before the disarm was skipped. --- msg/versioned/VehicleStatus.msg | 1 - src/lib/events/enums.json | 4 ---- src/modules/commander/Commander.cpp | 23 ++--------------------- 3 files changed, 2 insertions(+), 26 deletions(-) diff --git a/msg/versioned/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg index 80ff1618bf07..185076a5d673 100644 --- a/msg/versioned/VehicleStatus.msg +++ b/msg/versioned/VehicleStatus.msg @@ -21,7 +21,6 @@ uint8 ARM_DISARM_REASON_MISSION_START = 5 uint8 ARM_DISARM_REASON_LANDING = 6 uint8 ARM_DISARM_REASON_PREFLIGHT_INACTION = 7 uint8 ARM_DISARM_REASON_KILL_SWITCH = 8 -uint8 ARM_DISARM_REASON_LOCKDOWN = 9 uint8 ARM_DISARM_REASON_RC_BUTTON = 13 uint8 ARM_DISARM_REASON_FAILSAFE = 14 diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index 955016465e9c..acd73c144708 100644 --- a/src/lib/events/enums.json +++ b/src/lib/events/enums.json @@ -474,10 +474,6 @@ "name": "kill_switch", "description": "kill switch" }, - "9": { - "name": "lockdown", - "description": "lockdown" - }, "13": { "name": "rc_button", "description": "RC button" diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 9d4e933cb864..b50663ed0112 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -520,7 +520,6 @@ static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_r static_assert( (uint8_t)arm_disarm_reason_t::preflight_inaction == vehicle_status_s::ARM_DISARM_REASON_PREFLIGHT_INACTION); static_assert((uint8_t)arm_disarm_reason_t::kill_switch == vehicle_status_s::ARM_DISARM_REASON_KILL_SWITCH); - static_assert((uint8_t)arm_disarm_reason_t::lockdown == vehicle_status_s::ARM_DISARM_REASON_LOCKDOWN); static_assert((uint8_t)arm_disarm_reason_t::rc_button == vehicle_status_s::ARM_DISARM_REASON_RC_BUTTON); static_assert((uint8_t)arm_disarm_reason_t::failsafe == vehicle_status_s::ARM_DISARM_REASON_FAILSAFE); @@ -541,8 +540,6 @@ static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_r case arm_disarm_reason_t::kill_switch: return "kill-switch"; - case arm_disarm_reason_t::lockdown: return "lockdown"; - case arm_disarm_reason_t::rc_button: return "RC button"; case arm_disarm_reason_t::failsafe: return "failsafe"; @@ -2313,26 +2310,10 @@ void Commander::handleAutoDisarm() } // Auto disarm after 5 seconds if kill switch is engaged - bool auto_disarm = _actuator_armed.kill; - - // auto disarm if locked down to avoid user confusion - // skipped in HITL where lockdown is enabled for safety - if (_vehicle_status.hil_state != vehicle_status_s::HIL_STATE_ON) { - auto_disarm |= _actuator_armed.lockdown; - } - - //don't disarm if throw launch is in progress - auto_disarm &= !_multicopter_throw_launch.isThrowLaunchInProgress(); - - _auto_disarm_killed.set_state_and_update(auto_disarm, hrt_absolute_time()); + _auto_disarm_killed.set_state_and_update(_actuator_armed.kill, hrt_absolute_time()); if (_auto_disarm_killed.get_state()) { - if (_actuator_armed.kill) { - disarm(arm_disarm_reason_t::kill_switch, true); - - } else { - disarm(arm_disarm_reason_t::lockdown, true); - } + disarm(arm_disarm_reason_t::kill_switch, true); } } else { From 89825b194de82be8613b40ad15bf8d665541051e Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 15 Oct 2025 18:20:30 +0200 Subject: [PATCH 4/4] Commander: add message to each static_assert to solve: 'static_assert' with no message is a C++17 extension in Mac CI --- src/modules/commander/Commander.cpp | 30 +++++++++++++++++++---------- 1 file changed, 20 insertions(+), 10 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index b50663ed0112..6f6ae4050e77 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -511,17 +511,27 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[]) static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_reason) { - static_assert((uint8_t)arm_disarm_reason_t::stick_gesture == vehicle_status_s::ARM_DISARM_REASON_STICK_GESTURE); - static_assert((uint8_t)arm_disarm_reason_t::rc_switch == vehicle_status_s::ARM_DISARM_REASON_RC_SWITCH); - static_assert((uint8_t)arm_disarm_reason_t::command_internal == vehicle_status_s::ARM_DISARM_REASON_COMMAND_INTERNAL); - static_assert((uint8_t)arm_disarm_reason_t::command_external == vehicle_status_s::ARM_DISARM_REASON_COMMAND_EXTERNAL); - static_assert((uint8_t)arm_disarm_reason_t::mission_start == vehicle_status_s::ARM_DISARM_REASON_MISSION_START); - static_assert((uint8_t)arm_disarm_reason_t::landing == vehicle_status_s::ARM_DISARM_REASON_LANDING); + static_assert((uint8_t)arm_disarm_reason_t::stick_gesture == vehicle_status_s::ARM_DISARM_REASON_STICK_GESTURE, + "(dis)arm enum mismatch"); + static_assert((uint8_t)arm_disarm_reason_t::rc_switch == vehicle_status_s::ARM_DISARM_REASON_RC_SWITCH, + "(dis)arm enum mismatch"); + static_assert((uint8_t)arm_disarm_reason_t::command_internal == vehicle_status_s::ARM_DISARM_REASON_COMMAND_INTERNAL, + "(dis)arm enum mismatch"); + static_assert((uint8_t)arm_disarm_reason_t::command_external == vehicle_status_s::ARM_DISARM_REASON_COMMAND_EXTERNAL, + "(dis)arm enum mismatch"); + static_assert((uint8_t)arm_disarm_reason_t::mission_start == vehicle_status_s::ARM_DISARM_REASON_MISSION_START, + "(dis)arm enum mismatch"); + static_assert((uint8_t)arm_disarm_reason_t::landing == vehicle_status_s::ARM_DISARM_REASON_LANDING, + "(dis)arm enum mismatch"); static_assert( - (uint8_t)arm_disarm_reason_t::preflight_inaction == vehicle_status_s::ARM_DISARM_REASON_PREFLIGHT_INACTION); - static_assert((uint8_t)arm_disarm_reason_t::kill_switch == vehicle_status_s::ARM_DISARM_REASON_KILL_SWITCH); - static_assert((uint8_t)arm_disarm_reason_t::rc_button == vehicle_status_s::ARM_DISARM_REASON_RC_BUTTON); - static_assert((uint8_t)arm_disarm_reason_t::failsafe == vehicle_status_s::ARM_DISARM_REASON_FAILSAFE); + (uint8_t)arm_disarm_reason_t::preflight_inaction == vehicle_status_s::ARM_DISARM_REASON_PREFLIGHT_INACTION, + "(dis)arm enum mismatch"); + static_assert((uint8_t)arm_disarm_reason_t::kill_switch == vehicle_status_s::ARM_DISARM_REASON_KILL_SWITCH, + "(dis)arm enum mismatch"); + static_assert((uint8_t)arm_disarm_reason_t::rc_button == vehicle_status_s::ARM_DISARM_REASON_RC_BUTTON, + "(dis)arm enum mismatch"); + static_assert((uint8_t)arm_disarm_reason_t::failsafe == vehicle_status_s::ARM_DISARM_REASON_FAILSAFE, + "(dis)arm enum mismatch"); switch (calling_reason) { case arm_disarm_reason_t::stick_gesture: return "stick gesture";