diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 824f186025..5077227650 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -94,3 +94,22 @@ uint8 failure_detector_status # Bitmask containing FailureDetector status [0, uint32 onboard_control_sensors_present uint32 onboard_control_sensors_enabled uint32 onboard_control_sensors_health + + +uint8 ARM_DISARM_REASON_TRANSITION_TO_STANDBY = 0 +uint8 ARM_DISARM_REASON_RC_STICK = 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 latest_arming_reason +uint8 latest_disarming_reason diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 2a6332cb20..6110d70253 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -424,11 +424,12 @@ bool Commander::shutdown_if_allowed() { return TRANSITION_DENIED != arming_state_transition(&status, _safety, vehicle_status_s::ARMING_STATE_SHUTDOWN, &armed, false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, _arm_requirements, - hrt_elapsed_time(&_boot_timestamp)); + hrt_elapsed_time(&_boot_timestamp), arm_disarm_reason_t::SHUTDOWN); } transition_result_t -Commander::arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink_log_pub_local, const char *armedBy) +Commander::arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink_log_pub_local, + arm_disarm_reason_t calling_reason) { transition_result_t arming_res = TRANSITION_NOT_CHANGED; @@ -442,10 +443,42 @@ Commander::arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink mavlink_log_pub_local, &status_flags, _arm_requirements, - hrt_elapsed_time(&_boot_timestamp)); + hrt_elapsed_time(&_boot_timestamp), calling_reason); if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_log_pub_local, "%s by %s", arm ? "ARMED" : "DISARMED", armedBy); + const char *reason = ""; + + switch (calling_reason) { + case arm_disarm_reason_t::TRANSITION_TO_STANDBY: reason = ""; break; + + case arm_disarm_reason_t::RC_STICK: reason = "RC"; break; + + case arm_disarm_reason_t::RC_SWITCH: reason = "RC (switch)"; break; + + case arm_disarm_reason_t::COMMAND_INTERNAL: reason = "internal command"; break; + + case arm_disarm_reason_t::COMMAND_EXTERNAL: reason = "external command"; break; + + case arm_disarm_reason_t::MISSION_START: reason = "mission start"; break; + + case arm_disarm_reason_t::SAFETY_BUTTON: reason = "safety button"; break; + + case arm_disarm_reason_t::AUTO_DISARM_LAND: reason = "landing"; break; + + case arm_disarm_reason_t::AUTO_DISARM_PREFLIGHT: reason = "auto preflight disarming"; break; + + case arm_disarm_reason_t::KILL_SWITCH: reason = "kill-switch"; break; + + case arm_disarm_reason_t::LOCKDOWN: reason = "lockdown"; break; + + case arm_disarm_reason_t::FAILURE_DETECTOR: reason = "failure detector"; break; + + case arm_disarm_reason_t::SHUTDOWN: reason = "shutdown request"; break; + + case arm_disarm_reason_t::UNIT_TEST: reason = "unit tests"; break; + } + + mavlink_log_info(mavlink_log_pub_local, "%s by %s", arm ? "Armed" : "Disarmed", reason); } else if (arming_res == TRANSITION_DENIED) { tune_negative(true); @@ -736,7 +769,8 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ } } - transition_result_t arming_res = arm_disarm(cmd_arms, !enforce, &mavlink_log_pub, "Arm/Disarm component command"); + transition_result_t arming_res = arm_disarm(cmd_arms, !enforce, &mavlink_log_pub, + (cmd.from_external ? arm_disarm_reason_t::COMMAND_EXTERNAL : arm_disarm_reason_t::COMMAND_INTERNAL)); if (arming_res == TRANSITION_DENIED) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; @@ -949,7 +983,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ // switch to AUTO_MISSION and ARM if ((TRANSITION_DENIED != main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags, &_internal_state)) - && (TRANSITION_DENIED != arm_disarm(true, true, &mavlink_log_pub, "Mission start command"))) { + && (TRANSITION_DENIED != arm_disarm(true, true, &mavlink_log_pub, arm_disarm_reason_t::MISSION_START))) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; @@ -1495,7 +1529,7 @@ Commander::run() } if (safety_disarm_allowed) { - if (TRANSITION_CHANGED == arm_disarm(false, true, &mavlink_log_pub, "Safety button")) { + if (TRANSITION_CHANGED == arm_disarm(false, true, &mavlink_log_pub, arm_disarm_reason_t::SAFETY_BUTTON)) { _status_changed = true; } } @@ -1615,7 +1649,8 @@ Commander::run() } if (_auto_disarm_landed.get_state()) { - arm_disarm(false, true, &mavlink_log_pub, "Auto disarm initiated"); + arm_disarm(false, true, &mavlink_log_pub, + (_have_taken_off_since_arming ? arm_disarm_reason_t::AUTO_DISARM_LAND : arm_disarm_reason_t::AUTO_DISARM_PREFLIGHT)); } } @@ -1624,10 +1659,10 @@ Commander::run() if (_auto_disarm_killed.get_state()) { if (armed.manual_lockdown) { - arm_disarm(false, true, &mavlink_log_pub, "Kill-switch still engaged"); + arm_disarm(false, true, &mavlink_log_pub, arm_disarm_reason_t::KILL_SWITCH); } else { - arm_disarm(false, true, &mavlink_log_pub, "System in lockdown"); + arm_disarm(false, true, &mavlink_log_pub, arm_disarm_reason_t::LOCKDOWN); } } @@ -1663,7 +1698,8 @@ Commander::run() arming_ret = arming_state_transition(&status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, - _arm_requirements, hrt_elapsed_time(&_boot_timestamp)); + _arm_requirements, hrt_elapsed_time(&_boot_timestamp), + arm_disarm_reason_t::TRANSITION_TO_STANDBY); if (arming_ret == TRANSITION_DENIED) { /* do not complain if not allowed into standby */ @@ -1922,7 +1958,8 @@ Commander::run() if (rc_wants_disarm && (_land_detector.landed || manual_thrust_mode)) { arming_ret = arming_state_transition(&status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, - &mavlink_log_pub, &status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp)); + &mavlink_log_pub, &status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp), + (arm_switch_to_disarm_transition ? arm_disarm_reason_t::RC_SWITCH : arm_disarm_reason_t::RC_STICK)); } _stick_off_counter++; @@ -1976,7 +2013,8 @@ Commander::run() } else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { arming_ret = arming_state_transition(&status, _safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, !in_arming_grace_period /* fRunPreArmChecks */, - &mavlink_log_pub, &status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp)); + &mavlink_log_pub, &status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp), + (arm_switch_to_arm_transition ? arm_disarm_reason_t::RC_SWITCH : arm_disarm_reason_t::RC_STICK)); if (arming_ret != TRANSITION_CHANGED) { px4_usleep(100000); @@ -2182,7 +2220,7 @@ Commander::run() // 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs if (status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) { - arm_disarm(false, true, &mavlink_log_pub, "Failure detector"); + arm_disarm(false, true, &mavlink_log_pub, arm_disarm_reason_t::FAILURE_DETECTOR); _status_changed = true; mavlink_log_critical(&mavlink_log_pub, "ESCs did not respond to arm request"); } @@ -3419,7 +3457,8 @@ void *commander_low_prio_loop(void *arg) if (TRANSITION_DENIED == arming_state_transition(&status, safety_s{}, vehicle_status_s::ARMING_STATE_INIT, &armed, false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, PreFlightCheck::arm_requirements_t{}, // arming requirements not relevant for switching to ARMING_STATE_INIT - 30_s) // time since boot not relevant for switching to ARMING_STATE_INIT + 30_s, // time since boot not relevant for switching to ARMING_STATE_INIT + (cmd.from_external ? arm_disarm_reason_t::COMMAND_EXTERNAL : arm_disarm_reason_t::COMMAND_INTERNAL)) ) { answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub); @@ -3514,7 +3553,8 @@ void *commander_low_prio_loop(void *arg) false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, PreFlightCheck::arm_requirements_t{}, // arming requirements not relevant for switching to ARMING_STATE_STANDBY - 30_s); // time since boot not relevant for switching to ARMING_STATE_STANDBY + 30_s, // time since boot not relevant for switching to ARMING_STATE_STANDBY + (cmd.from_external ? arm_disarm_reason_t::COMMAND_EXTERNAL : arm_disarm_reason_t::COMMAND_INTERNAL)); } else { tune_negative(true); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 5af52af240..998f8e28e1 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -113,7 +113,8 @@ public: private: - transition_result_t arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink_log_pub, const char *armedBy); + transition_result_t arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink_log_pub, + arm_disarm_reason_t calling_reason); void battery_status_check(); diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index bd39edc209..44a3f63a48 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -305,8 +305,8 @@ bool StateMachineHelperTest::armingStateTransitionTest() nullptr /* no mavlink_log_pub */, &status_flags, arm_req, - 2e6 /* 2 seconds after boot, everything should be checked */ - ); + 2e6, /* 2 seconds after boot, everything should be checked */ + arm_disarm_reason_t::UNIT_TEST); // Validate result of transition ut_compare(test->assertMsg, test->expected_transition_result, result); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3320405e32..6d440b4303 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -105,7 +105,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe const arming_state_t new_arming_state, actuator_armed_s *armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, vehicle_status_flags_s *status_flags, const PreFlightCheck::arm_requirements_t &arm_requirements, - const hrt_abstime &time_since_boot) + const hrt_abstime &time_since_boot, arm_disarm_reason_t calling_reason) { // Double check that our static arrays are still valid static_assert(vehicle_status_s::ARMING_STATE_INIT == 0, "ARMING_STATE_INIT == 0"); @@ -216,12 +216,20 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe // Finish up the state transition if (valid_transition) { + bool was_armed = armed->armed; armed->armed = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED); armed->ready_to_arm = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) || (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY); ret = TRANSITION_CHANGED; status->arming_state = new_arming_state; + if (was_armed && !armed->armed) { // disarm transition + status->latest_disarming_reason = (uint8_t)calling_reason; + + } else if (!was_armed && armed->armed) { // arm transition + status->latest_arming_reason = (uint8_t)calling_reason; + } + if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { armed->armed_time_ms = hrt_absolute_time() / 1000; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index f632a4da1f..f6727d19a2 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -97,11 +97,28 @@ enum class position_nav_loss_actions_t { extern const char *const arming_state_names[]; +enum class arm_disarm_reason_t { + TRANSITION_TO_STANDBY = 0, + RC_STICK = 1, + RC_SWITCH = 2, + COMMAND_INTERNAL = 3, + COMMAND_EXTERNAL = 4, + MISSION_START = 5, + SAFETY_BUTTON = 6, + AUTO_DISARM_LAND = 7, + AUTO_DISARM_PREFLIGHT = 8, + KILL_SWITCH = 9, + LOCKDOWN = 10, + FAILURE_DETECTOR = 11, + SHUTDOWN = 12, + UNIT_TEST = 13 +}; + transition_result_t arming_state_transition(vehicle_status_s *status, const safety_s &safety, const arming_state_t new_arming_state, actuator_armed_s *armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, vehicle_status_flags_s *status_flags, const PreFlightCheck::arm_requirements_t &arm_requirements, - const hrt_abstime &time_since_boot); + const hrt_abstime &time_since_boot, arm_disarm_reason_t calling_reason); transition_result_t main_state_transition(const vehicle_status_s &status, const main_state_t new_main_state,