Commander: separate out arm state machine to class

Pure refactoring and just the first step to avoid conflicts on the way.

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

@@ -0,0 +1,217 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ArmStateMachine.hpp"
+
+#include <systemlib/mavlink_log.h>
+
+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;
+}
+
+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,
+		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, 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");
+	static_assert(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1,
+		      "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) {
+		ret = TRANSITION_NOT_CHANGED;
+
+	} else {
+
+		/*
+		 * Get sensing state if necessary
+		 */
+		bool preflight_check_ret = true;
+
+		/* only perform the pre-arm check if we have to */
+		if (fRunPreArmChecks && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
+		    && !hil_enabled) {
+
+			preflight_check_ret = PreFlightCheck::preflightCheck(mavlink_log_pub, status, status_flags, control_mode,
+					      true, true, time_since_boot);
+
+			if (preflight_check_ret) {
+				status_flags.system_sensors_initialized = true;
+			}
+
+			feedback_provided = true;
+		}
+
+		/* re-run the pre-flight check as long as sensors are failing */
+		if (!status_flags.system_sensors_initialized
+		    && fRunPreArmChecks
+		    && ((new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
+			|| (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY))
+		    && !hil_enabled) {
+
+			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,
+						time_since_boot);
+
+				_last_preflight_check = hrt_absolute_time();
+			}
+		}
+
+		// Check that we have a valid state transition
+		bool valid_transition = arming_transitions[new_arming_state][status.arming_state];
+
+		if (valid_transition) {
+			// We have a good transition. Now perform any secondary validation.
+			if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+
+				//      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) {
+
+					bool prearm_check_ret = true;
+
+					if (fRunPreArmChecks && preflight_check_ret) {
+						// only bother running prearm if preflight was successful
+						prearm_check_ret = PreFlightCheck::preArmCheck(mavlink_log_pub, status_flags, control_mode, safety, arm_requirements,
+								   status);
+					}
+
+					if (!preflight_check_ret || !prearm_check_ret) {
+						// the prearm and preflight checks already print the rejection reason
+						feedback_provided = true;
+						valid_transition = false;
+					}
+				}
+			}
+		}
+
+		if (hil_enabled) {
+			/* enforce lockdown in HIL */
+			armed.lockdown = true;
+			status_flags.system_sensors_initialized = true;
+
+			/* recover from a prearm fail */
+			if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
+				status.arming_state = vehicle_status_s::ARMING_STATE_STANDBY;
+			}
+
+			// HIL can always go to standby
+			if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
+				valid_transition = true;
+			}
+		}
+
+		if (!hil_enabled &&
+		    (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
+		    (status.arming_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) {
+				feedback_provided = true;
+				valid_transition = false;
+			}
+		}
+
+		// 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) {
+				status.armed_time = hrt_absolute_time();
+
+			} else {
+				status.armed_time = 0;
+			}
+		}
+	}
+
+	if (ret == TRANSITION_DENIED) {
+		/* print to MAVLink and console if we didn't provide any feedback yet */
+		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",
+					     arming_state_names[status.arming_state], arming_state_names[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));
+		}
+	}
+
+	return ret;
+}

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

@@ -0,0 +1,82 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../PreFlightCheck/PreFlightCheck.hpp"
+#include <drivers/drv_hrt.h>
+#include <px4_platform_common/events.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/safety.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_status_flags.h>
+
+#include "../../state_machine_helper.h" // TODO: get independent of transition_result_t
+
+using arm_disarm_reason_t = events::px4::enums::arm_disarm_reason_t;
+
+class ArmStateMachine
+{
+public:
+	ArmStateMachine() = default;
+	~ArmStateMachine() = default;
+
+	transition_result_t
+	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, 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, arm_disarm_reason_t calling_reason);
+
+private:
+	static inline events::px4::enums::arming_state_t eventArmingState(uint8_t arming_state);
+
+	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
+	// are the current state. Using new state and current state you can index into the array which
+	// will be true for a valid transition or false for a invalid transition. In some cases even
+	// though the transition is marked as true additional checks must be made. See arming_state_transition
+	// code for those checks.
+	static constexpr bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX]
+	= {
+		//                                                    INIT,  STANDBY, ARMED, STANDBY_ERROR, SHUTDOWN, IN_AIR_RESTORE
+		{ /* vehicle_status_s::ARMING_STATE_INIT */           true,  true,    false, true,          false,    false },
+		{ /* vehicle_status_s::ARMING_STATE_STANDBY */        true,  true,    true,  false,         false,    false },
+		{ /* vehicle_status_s::ARMING_STATE_ARMED */          false, true,    true,  false,         false,    true },
+		{ /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */  true,  true,    true,  true,          false,    false },
+		{ /* vehicle_status_s::ARMING_STATE_SHUTDOWN */       true,  true,    false, true,          true,     true },
+		{ /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false,   false, false,         false,    false }, // NYI
+	};
+};

src/modules/commander/Arming/ArmStateMachine/CMakeLists.txt

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+
+px4_add_library(ArmStateMachine
+	ArmStateMachine.cpp
+)
+target_include_directories(ArmStateMachine PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_link_libraries(ArmStateMachine PUBLIC PreFlightCheck)

src/modules/commander/Arming/CMakeLists.txt

@@ -31,6 +31,7 @@
 #
 ############################################################################
 
-add_subdirectory(PreFlightCheck)
 add_subdirectory(ArmAuthorization)
+add_subdirectory(ArmStateMachine)
 add_subdirectory(HealthFlags)
+add_subdirectory(PreFlightCheck)

src/modules/commander/CMakeLists.txt

@@ -62,6 +62,7 @@ px4_add_module(
 		hysteresis
 		PreFlightCheck
 		ArmAuthorization
+		ArmStateMachine
 		HealthFlags
 		sensor_calibration
 		world_magnetic_model

src/modules/commander/Commander.cpp

@@ -479,7 +479,7 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[])
 
 bool Commander::shutdown_if_allowed()
 {
-	return TRANSITION_DENIED != arming_state_transition(_status, _vehicle_control_mode, _safety,
+	return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, _safety,
 			vehicle_status_s::ARMING_STATE_SHUTDOWN,
 			_armed, false /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags, _arm_requirements,
 			hrt_elapsed_time(&_boot_timestamp), arm_disarm_reason_t::shutdown);
@@ -732,7 +732,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
 		}
 	}
 
-	transition_result_t arming_res = arming_state_transition(_status, _vehicle_control_mode, _safety,
+	transition_result_t arming_res = _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, _safety,
 					 vehicle_status_s::ARMING_STATE_ARMED, _armed, run_preflight_checks,
 					 &_mavlink_log_pub, _status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp),
 					 calling_reason);
@@ -774,7 +774,7 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
 		}
 	}
 
-	transition_result_t arming_res = arming_state_transition(_status, _vehicle_control_mode, _safety,
+	transition_result_t arming_res = _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, _safety,
 					 vehicle_status_s::ARMING_STATE_STANDBY, _armed, false,
 					 &_mavlink_log_pub, _status_flags, _arm_requirements,
 					 hrt_elapsed_time(&_boot_timestamp), calling_reason);
@@ -1368,7 +1368,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			} else {
 
 				/* try to go to INIT/PREFLIGHT arming state */
-				if (TRANSITION_DENIED == arming_state_transition(_status, _vehicle_control_mode, safety_s{},
+				if (TRANSITION_DENIED == _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, 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
@@ -2375,7 +2375,8 @@ 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) {
 
-			arming_state_transition(_status, _vehicle_control_mode, _safety, vehicle_status_s::ARMING_STATE_STANDBY, _armed,
+			_arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, _safety,
+					vehicle_status_s::ARMING_STATE_STANDBY, _armed,
 					true /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags,
 					_arm_requirements, hrt_elapsed_time(&_boot_timestamp),
 					arm_disarm_reason_t::transition_to_standby);

src/modules/commander/Commander.hpp

@@ -34,6 +34,7 @@
 #pragma once
 
 /*   Helper classes  */
+#include "Arming/ArmStateMachine/ArmStateMachine.hpp"
 #include "Arming/PreFlightCheck/PreFlightCheck.hpp"
 #include "failure_detector/FailureDetector.hpp"
 #include "Safety.hpp"
@@ -294,6 +295,7 @@ private:
 	static constexpr uint64_t HOTPLUG_SENS_TIMEOUT{8_s};	/**< wait for hotplug sensors to come online for upto 8 seconds */
 	static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms};
 
+	ArmStateMachine _arm_state_machine{};
 	PreFlightCheck::arm_requirements_t	_arm_requirements{};
 
 	hrt_abstime	_valid_distance_sensor_time_us{0}; /**< Last time that distance sensor data arrived (usec) */

src/modules/commander/commander_tests/state_machine_helper_test.cpp

@@ -42,6 +42,7 @@
 
 #include "../state_machine_helper.h"
 #include <unit_test.h>
+#include "../Arming/ArmStateMachine/ArmStateMachine.hpp"
 #include "../Arming/PreFlightCheck/PreFlightCheck.hpp"
 
 class StateMachineHelperTest : public UnitTest
@@ -50,6 +51,8 @@ public:
 	StateMachineHelperTest() = default;
 	~StateMachineHelperTest() override = default;
 
+	ArmStateMachine _arm_state_machine{};
+
 	bool run_tests() override;
 
 private:
@@ -303,7 +306,12 @@ bool StateMachineHelperTest::armingStateTransitionTest()
 		vehicle_control_mode_s control_mode{};
 
 		// Attempt transition
-		transition_result_t result = arming_state_transition(status, control_mode, safety, test->requested_state, armed,
+		transition_result_t result = _arm_state_machine.arming_state_transition(
+						     status,
+						     control_mode,
+						     safety,
+						     test->requested_state,
+						     armed,
 						     true /* enable pre-arm checks */,
 						     nullptr /* no mavlink_log_pub */,
 						     status_flags,

src/modules/commander/state_machine_helper.cpp

@@ -61,22 +61,6 @@ static constexpr const char reason_no_datalink[] = "no datalink";
 static constexpr const char reason_no_rc_and_no_datalink[] = "no RC and no datalink";
 static constexpr const char reason_no_gps[] = "no GPS";
 
-// This array defines the arming state transitions. The rows are the new state, and the columns
-// are the current state. Using new state and current state you can index into the array which
-// will be true for a valid transition or false for a invalid transition. In some cases even
-// though the transition is marked as true additional checks must be made. See arming_state_transition
-// code for those checks.
-static constexpr const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX]
-= {
-	//                                                    INIT,  STANDBY, ARMED, STANDBY_ERROR, SHUTDOWN, IN_AIR_RESTORE
-	{ /* vehicle_status_s::ARMING_STATE_INIT */           true,  true,    false, true,          false,    false },
-	{ /* vehicle_status_s::ARMING_STATE_STANDBY */        true,  true,    true,  false,         false,    false },
-	{ /* vehicle_status_s::ARMING_STATE_ARMED */          false, true,    true,  false,         false,    true },
-	{ /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */  true,  true,    true,  true,          false,    false },
-	{ /* vehicle_status_s::ARMING_STATE_SHUTDOWN */       true,  true,    false, true,          true,     true },
-	{ /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false,   false, false,         false,    false }, // NYI
-};
-
 // You can index into the array with an arming_state_t in order to get its textual representation
 const char *const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = {
 	"INIT",
@@ -87,28 +71,6 @@ const char *const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = {
 	"IN_AIR_RESTORE",
 };
 
-static inline events::px4::enums::arming_state_t 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;
-}
-
 // You can index into the array with an navigation_state_t in order to get its textual representation
 const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
 	"MANUAL",
@@ -135,8 +97,6 @@ const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
 	"ORBIT"
 };
 
-static hrt_abstime last_preflight_check = 0;	///< initialize so it gets checked immediately
-
 void set_link_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed,
 			     const vehicle_status_flags_s &status_flags, commander_state_s &internal_state, link_loss_actions_t link_loss_act,
 			     const float ll_delay);
@@ -159,162 +119,6 @@ void reset_offboard_loss_globals(actuator_armed_s &armed, const bool old_failsaf
 				 const offboard_loss_actions_t offboard_loss_act,
 				 const offboard_loss_rc_actions_t offboard_loss_rc_act);
 
-transition_result_t 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,
-		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, 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");
-	static_assert(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1,
-		      "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) {
-		ret = TRANSITION_NOT_CHANGED;
-
-	} else {
-
-		/*
-		 * Get sensing state if necessary
-		 */
-		bool preflight_check_ret = true;
-
-		/* only perform the pre-arm check if we have to */
-		if (fRunPreArmChecks && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
-		    && !hil_enabled) {
-
-			preflight_check_ret = PreFlightCheck::preflightCheck(mavlink_log_pub, status, status_flags, control_mode,
-					      true, true, time_since_boot);
-
-			if (preflight_check_ret) {
-				status_flags.system_sensors_initialized = true;
-			}
-
-			feedback_provided = true;
-		}
-
-		/* re-run the pre-flight check as long as sensors are failing */
-		if (!status_flags.system_sensors_initialized
-		    && fRunPreArmChecks
-		    && ((new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
-			|| (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY))
-		    && !hil_enabled) {
-
-			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,
-						time_since_boot);
-
-				last_preflight_check = hrt_absolute_time();
-			}
-		}
-
-		// Check that we have a valid state transition
-		bool valid_transition = arming_transitions[new_arming_state][status.arming_state];
-
-		if (valid_transition) {
-			// We have a good transition. Now perform any secondary validation.
-			if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
-
-				//      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) {
-
-					bool prearm_check_ret = true;
-
-					if (fRunPreArmChecks && preflight_check_ret) {
-						// only bother running prearm if preflight was successful
-						prearm_check_ret = PreFlightCheck::preArmCheck(mavlink_log_pub, status_flags, control_mode, safety, arm_requirements,
-								   status);
-					}
-
-					if (!preflight_check_ret || !prearm_check_ret) {
-						// the prearm and preflight checks already print the rejection reason
-						feedback_provided = true;
-						valid_transition = false;
-					}
-				}
-			}
-		}
-
-		if (hil_enabled) {
-			/* enforce lockdown in HIL */
-			armed.lockdown = true;
-			status_flags.system_sensors_initialized = true;
-
-			/* recover from a prearm fail */
-			if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
-				status.arming_state = vehicle_status_s::ARMING_STATE_STANDBY;
-			}
-
-			// HIL can always go to standby
-			if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
-				valid_transition = true;
-			}
-		}
-
-		if (!hil_enabled &&
-		    (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
-		    (status.arming_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) {
-				feedback_provided = true;
-				valid_transition = false;
-			}
-		}
-
-		// 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) {
-				status.armed_time = hrt_absolute_time();
-
-			} else {
-				status.armed_time = 0;
-			}
-		}
-	}
-
-	if (ret == TRANSITION_DENIED) {
-		/* print to MAVLink and console if we didn't provide any feedback yet */
-		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",
-					     arming_state_names[status.arming_state], arming_state_names[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));
-		}
-	}
-
-	return ret;
-}
-
 transition_result_t
 main_state_transition(const vehicle_status_s &status, const main_state_t new_main_state,
 		      const vehicle_status_flags_s &status_flags, commander_state_s &internal_state)

src/modules/commander/state_machine_helper.h

@@ -44,13 +44,10 @@
 
 #include <drivers/drv_hrt.h>
 
-#include "Arming/PreFlightCheck/PreFlightCheck.hpp"
-
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/safety.h>
 #include <uORB/topics/commander_state.h>
 #include <uORB/topics/vehicle_status_flags.h>
 #include <px4_platform_common/events.h>
@@ -106,21 +103,12 @@ enum class position_nav_loss_actions_t {
 extern const char *const arming_state_names[];
 extern const char *const nav_state_names[];
 
-using arm_disarm_reason_t = events::px4::enums::arm_disarm_reason_t;
-
 enum RCLossExceptionBits {
 	RCL_EXCEPT_MISSION = (1 << 0),
 	RCL_EXCEPT_HOLD = (1 << 1),
 	RCL_EXCEPT_OFFBOARD = (1 << 2)
 };
 
-transition_result_t
-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, 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, arm_disarm_reason_t calling_reason);
-
 transition_result_t
 main_state_transition(const vehicle_status_s &status, const main_state_t new_main_state,
 		      const vehicle_status_flags_s &status_flags, commander_state_s &internal_state);