mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-23 06:36:45 +08:00
Fix (dis)arm reason enumeration (#25766)
Build all targets / Scan for Board Targets (push) Has been cancelled
Build all targets / Build [${{ matrix.runner }}][${{ matrix.group }}] (push) Has been cancelled
Build all targets / Upload Artifacts (push) Has been cancelled
Checks / build (NO_NINJA_BUILD=1 px4_fmu-v5_default) (push) Has been cancelled
Checks / build (NO_NINJA_BUILD=1 px4_sitl_default) (push) Has been cancelled
Checks / build (check_format) (push) Has been cancelled
Checks / build (check_newlines) (push) Has been cancelled
Checks / build (module_documentation) (push) Has been cancelled
Checks / build (px4_fmu-v2_default stack_check) (push) Has been cancelled
Checks / build (px4_sitl_allyes) (push) Has been cancelled
Checks / build (shellcheck_all) (push) Has been cancelled
Checks / build (tests) (push) Has been cancelled
Checks / build (tests_coverage) (push) Has been cancelled
Checks / build (validate_module_configs) (push) Has been cancelled
Clang Tidy / build (push) Has been cancelled
MacOS build / build (px4_fmu-v5_default) (push) Has been cancelled
MacOS build / build (px4_sitl) (push) Has been cancelled
Ubuntu environment build / Build and Test (ubuntu:22.04) (push) Has been cancelled
Ubuntu environment build / Build and Test (ubuntu:24.04) (push) Has been cancelled
Container build / Set Tags and Variables (push) Has been cancelled
Container build / Build Container (amd64) (push) Has been cancelled
Container build / Build Container (arm64) (push) Has been cancelled
Container build / Deploy To Registry (push) Has been cancelled
EKF Update Change Indicator / unit_tests (push) Has been cancelled
Failsafe Simulator Build / build (failsafe_web) (push) Has been cancelled
FLASH usage analysis / Analyzing px4_fmu-v5x (push) Has been cancelled
FLASH usage analysis / Analyzing px4_fmu-v6x (push) Has been cancelled
FLASH usage analysis / Publish Results (push) Has been cancelled
ITCM check / Checking nxp_tropic-community (push) Has been cancelled
ITCM check / Checking px4_fmu-v5x (push) Has been cancelled
ITCM check / Checking px4_fmu-v6xrt (push) Has been cancelled
MAVROS Mission Tests / build (map[mission:MC_mission_box vehicle:iris]) (push) Has been cancelled
MAVROS Offboard Tests / build (map[test_file:mavros_posix_tests_offboard_posctl.test vehicle:iris]) (push) Has been cancelled
Nuttx Target with extra env config / build (px4_fmu-v5_default) (push) Has been cancelled
Python CI Checks / build (push) Has been cancelled
ROS Integration Tests / build (push) Has been cancelled
ROS Translation Node Tests / Build and test (map[ros_version:humble ubuntu:jammy]) (push) Has been cancelled
ROS Translation Node Tests / Build and test (map[ros_version:jazzy ubuntu:noble]) (push) Has been cancelled
SITL Tests / Testing PX4 tailsitter (push) Has been cancelled
SITL Tests / Testing PX4 iris (push) Has been cancelled
SITL Tests / Testing PX4 standard_vtol (push) Has been cancelled
Handle stale issues and PRs / stale (push) Has been cancelled
Fuzzing / Fuzzing (push) Has been cancelled
Build all targets / Scan for Board Targets (push) Has been cancelled
Build all targets / Build [${{ matrix.runner }}][${{ matrix.group }}] (push) Has been cancelled
Build all targets / Upload Artifacts (push) Has been cancelled
Checks / build (NO_NINJA_BUILD=1 px4_fmu-v5_default) (push) Has been cancelled
Checks / build (NO_NINJA_BUILD=1 px4_sitl_default) (push) Has been cancelled
Checks / build (check_format) (push) Has been cancelled
Checks / build (check_newlines) (push) Has been cancelled
Checks / build (module_documentation) (push) Has been cancelled
Checks / build (px4_fmu-v2_default stack_check) (push) Has been cancelled
Checks / build (px4_sitl_allyes) (push) Has been cancelled
Checks / build (shellcheck_all) (push) Has been cancelled
Checks / build (tests) (push) Has been cancelled
Checks / build (tests_coverage) (push) Has been cancelled
Checks / build (validate_module_configs) (push) Has been cancelled
Clang Tidy / build (push) Has been cancelled
MacOS build / build (px4_fmu-v5_default) (push) Has been cancelled
MacOS build / build (px4_sitl) (push) Has been cancelled
Ubuntu environment build / Build and Test (ubuntu:22.04) (push) Has been cancelled
Ubuntu environment build / Build and Test (ubuntu:24.04) (push) Has been cancelled
Container build / Set Tags and Variables (push) Has been cancelled
Container build / Build Container (amd64) (push) Has been cancelled
Container build / Build Container (arm64) (push) Has been cancelled
Container build / Deploy To Registry (push) Has been cancelled
EKF Update Change Indicator / unit_tests (push) Has been cancelled
Failsafe Simulator Build / build (failsafe_web) (push) Has been cancelled
FLASH usage analysis / Analyzing px4_fmu-v5x (push) Has been cancelled
FLASH usage analysis / Analyzing px4_fmu-v6x (push) Has been cancelled
FLASH usage analysis / Publish Results (push) Has been cancelled
ITCM check / Checking nxp_tropic-community (push) Has been cancelled
ITCM check / Checking px4_fmu-v5x (push) Has been cancelled
ITCM check / Checking px4_fmu-v6xrt (push) Has been cancelled
MAVROS Mission Tests / build (map[mission:MC_mission_box vehicle:iris]) (push) Has been cancelled
MAVROS Offboard Tests / build (map[test_file:mavros_posix_tests_offboard_posctl.test vehicle:iris]) (push) Has been cancelled
Nuttx Target with extra env config / build (px4_fmu-v5_default) (push) Has been cancelled
Python CI Checks / build (push) Has been cancelled
ROS Integration Tests / build (push) Has been cancelled
ROS Translation Node Tests / Build and test (map[ros_version:humble ubuntu:jammy]) (push) Has been cancelled
ROS Translation Node Tests / Build and test (map[ros_version:jazzy ubuntu:noble]) (push) Has been cancelled
SITL Tests / Testing PX4 tailsitter (push) Has been cancelled
SITL Tests / Testing PX4 iris (push) Has been cancelled
SITL Tests / Testing PX4 standard_vtol (push) Has been cancelled
Handle stale issues and PRs / stale (push) Has been cancelled
Fuzzing / Fuzzing (push) Has been cancelled
This commit is contained in:
@@ -13,20 +13,16 @@ 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_RC_BUTTON = 13
|
||||
uint8 ARM_DISARM_REASON_FAILSAFE = 14
|
||||
|
||||
uint64 nav_state_timestamp # time when current nav_state activated
|
||||
|
||||
|
||||
@@ -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,36 +463,20 @@
|
||||
"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",
|
||||
"description": "kill switch"
|
||||
},
|
||||
"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",
|
||||
|
||||
@@ -511,10 +511,30 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[])
|
||||
|
||||
static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_reason)
|
||||
{
|
||||
switch (calling_reason) {
|
||||
case arm_disarm_reason_t::transition_to_standby: return "";
|
||||
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,
|
||||
"(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");
|
||||
|
||||
case arm_disarm_reason_t::stick_gesture: return "Stick gesture";
|
||||
switch (calling_reason) {
|
||||
case arm_disarm_reason_t::stick_gesture: return "stick gesture";
|
||||
|
||||
case arm_disarm_reason_t::rc_switch: return "RC switch";
|
||||
|
||||
@@ -524,21 +544,13 @@ 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";
|
||||
}
|
||||
@@ -2299,35 +2311,19 @@ 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 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 {
|
||||
|
||||
Reference in New Issue
Block a user