vehicle_status: add latest arming/disarming reason

Makes it easier to debug.

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

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);

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();
 

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);

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;
 

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,