ArmStateMachine: move arm state into the class

src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp

@@ -38,51 +38,6 @@
 constexpr bool
 ArmStateMachine::arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX];
 
-events::px4::enums::arming_state_t ArmStateMachine::eventArmingState(uint8_t arming_state)
-{
-	switch (arming_state) {
-	case vehicle_status_s::ARMING_STATE_INIT: return events::px4::enums::arming_state_t::init;
-
-	case vehicle_status_s::ARMING_STATE_STANDBY: return events::px4::enums::arming_state_t::standby;
-
-	case vehicle_status_s::ARMING_STATE_ARMED: return events::px4::enums::arming_state_t::armed;
-
-	case vehicle_status_s::ARMING_STATE_STANDBY_ERROR: return events::px4::enums::arming_state_t::standby_error;
-
-	case vehicle_status_s::ARMING_STATE_SHUTDOWN: return events::px4::enums::arming_state_t::shutdown;
-
-	case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE: return events::px4::enums::arming_state_t::inair_restore;
-	}
-
-	static_assert(vehicle_status_s::ARMING_STATE_MAX - 1 == (int)events::px4::enums::arming_state_t::inair_restore,
-		      "enum def mismatch");
-
-	return events::px4::enums::arming_state_t::init;
-}
-
-const char *ArmStateMachine::getArmingStateName(uint8_t arming_state)
-{
-	switch (arming_state) {
-
-	case vehicle_status_s::ARMING_STATE_INIT: return "Init";
-
-	case vehicle_status_s::ARMING_STATE_STANDBY: return "Standby";
-
-	case vehicle_status_s::ARMING_STATE_ARMED: return "Armed";
-
-	case vehicle_status_s::ARMING_STATE_STANDBY_ERROR: return "Standby error";
-
-	case vehicle_status_s::ARMING_STATE_SHUTDOWN: return "Shutdown";
-
-	case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE: return "In-air restore";
-
-	default: return "Unknown";
-	}
-
-	static_assert(vehicle_status_s::ARMING_STATE_MAX - 1 == vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE,
-		      "enum def mismatch");
-}
-
 transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &status,
 		const vehicle_control_mode_s &control_mode, const safety_s &safety,
 		const arming_state_t new_arming_state, actuator_armed_s &armed, const bool fRunPreArmChecks,
@@ -96,13 +51,12 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
 		      "ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1");
 
 	transition_result_t ret = TRANSITION_DENIED;
-	arming_state_t current_arming_state = status.arming_state;
 	bool feedback_provided = false;
 
 	const bool hil_enabled = (status.hil_state == vehicle_status_s::HIL_STATE_ON);
 
 	/* only check transition if the new state is actually different from the current one */
-	if (new_arming_state == current_arming_state) {
+	if (new_arming_state == _arm_state) {
 		ret = TRANSITION_NOT_CHANGED;
 
 	} else {
@@ -136,7 +90,7 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
 			if ((_last_preflight_check == 0) || (hrt_elapsed_time(&_last_preflight_check) > 1000 * 1000)) {
 
 				status_flags.system_sensors_initialized = PreFlightCheck::preflightCheck(mavlink_log_pub, status,
-						status_flags, control_mode, false, status.arming_state != vehicle_status_s::ARMING_STATE_ARMED,
+						status_flags, control_mode, false, !isArmed(),
 						time_since_boot);
 
 				_last_preflight_check = hrt_absolute_time();
@@ -144,7 +98,7 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
 		}
 
 		// Check that we have a valid state transition
-		bool valid_transition = arming_transitions[new_arming_state][status.arming_state];
+		bool valid_transition = arming_transitions[new_arming_state][_arm_state];
 
 		if (valid_transition) {
 			// We have a good transition. Now perform any secondary validation.
@@ -152,7 +106,7 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
 
 				//      Do not perform pre-arm checks if coming from in air restore
 				//      Allow if vehicle_status_s::HIL_STATE_ON
-				if (status.arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE) {
+				if (_arm_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE) {
 
 					bool prearm_check_ret = true;
 
@@ -177,8 +131,8 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
 			status_flags.system_sensors_initialized = true;
 
 			/* recover from a prearm fail */
-			if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
+			if (_arm_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
-				status.arming_state = vehicle_status_s::ARMING_STATE_STANDBY;
+				_arm_state = vehicle_status_s::ARMING_STATE_STANDBY;
 			}
 
 			// HIL can always go to standby
@@ -189,7 +143,7 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
 
 		if (!hil_enabled &&
 		    (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
-		    (status.arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) {
+		    (_arm_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) {
 
 			// Sensors need to be initialized for STANDBY state, except for HIL
 			if (!status_flags.system_sensors_initialized) {
@@ -205,7 +159,7 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
 			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;
+			_arm_state = new_arming_state;
 
 			if (was_armed && !armed.armed) { // disarm transition
 				status.latest_disarming_reason = (uint8_t)calling_reason;
@@ -228,13 +182,58 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
 		if (!feedback_provided) {
 			// FIXME: this catch-all does not provide helpful information to the user
 			mavlink_log_critical(mavlink_log_pub, "Transition denied: %s to %s\t",
-					     getArmingStateName(status.arming_state), getArmingStateName(new_arming_state));
+					     getArmStateName(_arm_state), getArmStateName(new_arming_state));
 			events::send<events::px4::enums::arming_state_t, events::px4::enums::arming_state_t>(
 				events::ID("commander_transition_denied"), events::Log::Critical,
 				"Arming state transition denied: {1} to {2}",
-				eventArmingState(status.arming_state), eventArmingState(new_arming_state));
+				getArmStateEvent(_arm_state), getArmStateEvent(new_arming_state));
 		}
 	}
 
 	return ret;
 }
+
+const char *ArmStateMachine::getArmStateName(uint8_t arming_state)
+{
+	switch (arming_state) {
+
+	case vehicle_status_s::ARMING_STATE_INIT: return "Init";
+
+	case vehicle_status_s::ARMING_STATE_STANDBY: return "Standby";
+
+	case vehicle_status_s::ARMING_STATE_ARMED: return "Armed";
+
+	case vehicle_status_s::ARMING_STATE_STANDBY_ERROR: return "Standby error";
+
+	case vehicle_status_s::ARMING_STATE_SHUTDOWN: return "Shutdown";
+
+	case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE: return "In-air restore";
+
+	default: return "Unknown";
+	}
+
+	static_assert(vehicle_status_s::ARMING_STATE_MAX - 1 == vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE,
+		      "enum def mismatch");
+}
+
+events::px4::enums::arming_state_t ArmStateMachine::getArmStateEvent(uint8_t arming_state)
+{
+	switch (arming_state) {
+	case vehicle_status_s::ARMING_STATE_INIT: return events::px4::enums::arming_state_t::init;
+
+	case vehicle_status_s::ARMING_STATE_STANDBY: return events::px4::enums::arming_state_t::standby;
+
+	case vehicle_status_s::ARMING_STATE_ARMED: return events::px4::enums::arming_state_t::armed;
+
+	case vehicle_status_s::ARMING_STATE_STANDBY_ERROR: return events::px4::enums::arming_state_t::standby_error;
+
+	case vehicle_status_s::ARMING_STATE_SHUTDOWN: return events::px4::enums::arming_state_t::shutdown;
+
+	case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE: return events::px4::enums::arming_state_t::inair_restore;
+	}
+
+	static_assert(vehicle_status_s::ARMING_STATE_MAX - 1 == (int)events::px4::enums::arming_state_t::inair_restore,
+		      "enum def mismatch");
+
+	return events::px4::enums::arming_state_t::init;
+}

src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp

@@ -52,7 +52,7 @@ public:
 	ArmStateMachine() = default;
 	~ArmStateMachine() = default;
 
-	static const char *getArmingStateName(uint8_t arming_state);
+	void forceArmState(uint8_t new_arm_state) { _arm_state = new_arm_state; }
 
 	transition_result_t
 	arming_state_transition(vehicle_status_s &status, const vehicle_control_mode_s &control_mode, const safety_s &safety,
@@ -61,9 +61,21 @@ public:
 				vehicle_status_flags_s &status_flags, const PreFlightCheck::arm_requirements_t &arm_requirements,
 				const hrt_abstime &time_since_boot, arm_disarm_reason_t calling_reason);
 
+	// Getters
+	uint8_t getArmState() const { return _arm_state; }
+
+	bool isInit() const { return (_arm_state == vehicle_status_s::ARMING_STATE_INIT); }
+	bool isStandby() const { return (_arm_state == vehicle_status_s::ARMING_STATE_STANDBY); }
+	bool isArmed() const { return (_arm_state == vehicle_status_s::ARMING_STATE_ARMED); }
+	bool isShutdown() const { return (_arm_state == vehicle_status_s::ARMING_STATE_SHUTDOWN); }
+
+	static const char *getArmStateName(uint8_t arming_state);
+	const char *getArmStateName() const { return getArmStateName(_arm_state); }
+
 private:
-	static inline events::px4::enums::arming_state_t eventArmingState(uint8_t arming_state);
+	static inline events::px4::enums::arming_state_t getArmStateEvent(uint8_t arming_state);
 
+	uint8_t _arm_state{vehicle_status_s::ARMING_STATE_INIT};
 	hrt_abstime _last_preflight_check = 0; ///< initialize so it gets checked immediately
 
 	// This array defines the arming state transitions. The rows are the new state, and the columns

src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp

@@ -268,7 +268,7 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 		PreFlightCheck::arm_requirements_t arm_req{};
 
 		// Setup initial machine state
-		status.arming_state = test->current_state.arming_state;
+		arm_state_machine.forceArmState(test->current_state.arming_state);
 		status_flags.system_sensors_initialized = test->system_sensors_initialized;
 		status.hil_state = test->hil_state;
 		// The power status of the test unit is not relevant for the unit test
@@ -297,7 +297,7 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
 
 		// Validate result of transition
 		EXPECT_EQ(result, test->expected_transition_result) << test->assertMsg;
-		EXPECT_EQ(status.arming_state, test->expected_state.arming_state) << test->assertMsg;
+		EXPECT_EQ(arm_state_machine.getArmState(), test->expected_state.arming_state) << test->assertMsg;
 		EXPECT_EQ(armed.armed, test->expected_state.armed) << test->assertMsg;
 		EXPECT_EQ(armed.ready_to_arm, test->expected_state.ready_to_arm) << test->assertMsg;
 	}

src/modules/commander/Commander.cpp

@@ -479,7 +479,7 @@ int Commander::custom_command(int argc, char *argv[])
 
 int Commander::print_status()
 {
-	PX4_INFO("arming: %s", _arm_state_machine.getArmingStateName(_status.arming_state));
+	PX4_INFO("Arm state: %s", _arm_state_machine.getArmStateName());
 	PX4_INFO("navigation: %s", nav_state_names[_status.nav_state]);
 	perf_print_counter(_loop_perf);
 	perf_print_counter(_preflight_check_perf);
@@ -829,7 +829,6 @@ Commander::Commander() :
 	// We want to accept RC inputs as default
 	_status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
 	_status.nav_state_timestamp = hrt_absolute_time();
-	_status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
 
 	/* mark all signals lost as long as they haven't been found */
 	_status.rc_signal_lost = true;
@@ -1027,12 +1026,13 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				const bool forced = (static_cast<int>(lroundf(cmd.param2)) == 21196);
 				const bool cmd_from_io = (static_cast<int>(roundf(cmd.param3)) == 1234);
 
-				if (!forced) {
+				// Flick to in-air restore first if this comes from an onboard system and from IO
-					// Flick to in-air restore first if this comes from an onboard system and from IO
+				if (!forced && cmd_from_io
-					if (cmd.source_system == _status.system_id && cmd.source_component == _status.component_id
+				    && (cmd.source_system == _status.system_id)
-					    && cmd_from_io && (arming_action == vehicle_command_s::ARMING_ACTION_ARM)) {
+				    && (cmd.source_component == _status.component_id)
-						_status.arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE;
+				    && (arming_action == vehicle_command_s::ARMING_ACTION_ARM)) {
-					}
+					// TODO: replace with a proper allowed transition
+					_arm_state_machine.forceArmState(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE);
 				}
 
 				transition_result_t arming_res = TRANSITION_DENIED;
@@ -1386,8 +1386,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
 
-			if ((_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)
+			if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
-			    || _status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN || _worker_thread.isBusy()) {
 
 				// reject if armed or shutting down
 				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
@@ -1506,9 +1505,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 	case vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW: {
 			// Magnetometer quick calibration using world magnetic model and known heading
-			if ((_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)
+			if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
-			    || (_status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN)
-			    || _worker_thread.isBusy()) {
 
 				// reject if armed or shutting down
 				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
@@ -1543,9 +1540,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 	case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: {
 
-			if ((_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)
+			if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
-			    || _status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN
-			    || _worker_thread.isBusy()) {
 
 				// reject if armed or shutting down
 				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
@@ -2455,7 +2450,7 @@ Commander::run()
 		}
 
 		/* If in INIT state, try to proceed to STANDBY state */
-		if (!_status_flags.calibration_enabled && _status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
+		if (!_status_flags.calibration_enabled && _arm_state_machine.isInit()) {
 
 			_arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, _safety,
 					vehicle_status_s::ARMING_STATE_STANDBY, _armed,
@@ -3099,6 +3094,7 @@ Commander::run()
 			update_control_mode();
 
 			// vehicle_status publish (after prearm/preflight updates above)
+			_status.arming_state = _arm_state_machine.getArmState();
 			_status.timestamp = hrt_absolute_time();
 			_status_pub.publish(_status);
 
@@ -3276,14 +3272,14 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
 		uint8_t led_color = led_control_s::COLOR_WHITE;
 		bool set_normal_color = false;
 
-		uint64_t overload_warn_delay = (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ? 1_ms : 250_ms;
+		uint64_t overload_warn_delay = _arm_state_machine.isArmed() ? 1_ms : 250_ms;
 
 		// set mode
 		if (overload && (time_now_us >= _overload_start + overload_warn_delay)) {
 			led_mode = led_control_s::MODE_BLINK_FAST;
 			led_color = led_control_s::COLOR_PURPLE;
 
-		} else if (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+		} else if (_arm_state_machine.isArmed()) {
 			led_mode = led_control_s::MODE_ON;
 			set_normal_color = true;
 
@@ -3291,7 +3287,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
 			led_mode = led_control_s::MODE_BLINK_FAST;
 			led_color = led_control_s::COLOR_RED;
 
-		} else if (_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
+		} else if (_arm_state_machine.isStandby()) {
 			led_mode = led_control_s::MODE_BREATHE;
 			set_normal_color = true;
 
@@ -3299,7 +3295,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
 			led_mode = led_control_s::MODE_BREATHE;
 			set_normal_color = true;
 
-		} else if (_status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
+		} else if (_arm_state_machine.isInit()) {
 			// if in init status it should not be in the error state
 			led_mode = led_control_s::MODE_OFF;
 
@@ -4166,7 +4162,7 @@ void Commander::estimator_check()
 
 			if (run_quality_checks && _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
 
-				if (_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
+				if (!_arm_state_machine.isArmed()) {
 					_nav_test_failed = false;
 					_nav_test_passed = false;