WIP: commander arming simple

msg/vehicle_command.msg

@@ -72,6 +72,7 @@ uint16 VEHICLE_CMD_MISSION_START = 300			# start running a mission |first_item:
 uint16 VEHICLE_CMD_ACTUATOR_TEST = 310			# Actuator testing command|value [-1,1]|timeout [s]|Empty|Empty|output function|
 uint16 VEHICLE_CMD_CONFIGURE_ACTUATOR = 311		# Actuator configuration command|configuration|Empty|Empty|Empty|output function|
 uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400		# Arms / Disarms a component |1 to arm, 0 to disarm
+uint16 VEHICLE_CMD_RUN_PREARM_CHECKS = 401              # Instructs system to run pre-arm checks.
 uint16 VEHICLE_CMD_INJECT_FAILURE = 420			# Inject artificial failure for testing purposes
 uint16 VEHICLE_CMD_START_RX_PAIR = 500			# Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX|
 uint16 VEHICLE_CMD_REQUEST_MESSAGE = 512    # Request to send a single instance of the specified message

msg/vehicle_status.msg

@@ -1,12 +1,8 @@
 uint64 timestamp				# time since system start (microseconds)
 
-uint8 ARMING_STATE_INIT = 0
-uint8 ARMING_STATE_STANDBY = 1
-uint8 ARMING_STATE_ARMED = 2
-uint8 ARMING_STATE_STANDBY_ERROR = 3
-uint8 ARMING_STATE_SHUTDOWN = 4
-uint8 ARMING_STATE_IN_AIR_RESTORE = 5
-uint8 ARMING_STATE_MAX = 6
+uint8 ARMING_STATE_DISARMED = 1
+uint8 ARMING_STATE_ARMED    = 2
+uint8 ARMING_STATE_MAX      = 3
 
 # FailureDetector status
 uint16 FAILURE_NONE = 0

msg/vehicle_status_flags.msg

@@ -2,8 +2,10 @@
 
 uint64 timestamp				# time since system start (microseconds)
 
+bool preflight_check_passing
+bool prearm_check_passing
+
 bool calibration_enabled
-bool system_sensors_initialized
 bool system_hotplug_timeout		# true if the hotplug sensor search is over
 bool auto_mission_available
 bool angular_velocity_valid
@@ -18,6 +20,7 @@ bool power_input_valid                	# set if input power is valid
 bool battery_healthy                	# set if battery is available and not low
 bool escs_error		      		# set to true if one or more ESCs reporting esc_status are offline
 bool escs_failure		      	# set to true if one or more ESCs reporting esc_status has a failure
+bool overload                           # maximum cpuload exceeded or free memory dangerously low
 
 bool position_reliant_on_gps
 bool position_reliant_on_optical_flow

src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.h

@@ -36,7 +36,7 @@
 #include <stdint.h>
 
 #include <drivers/drv_hrt.h>
-#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_command_ack.h>
 
 enum arm_auth_methods {
 	ARM_AUTH_METHOD_ARM_REQ = 0,

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

@@ -1,238 +0,0 @@
-/****************************************************************************
- *
- *   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];
-
-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;
-	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 == _arm_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, !isArmed(),
-						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][_arm_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 (_arm_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 (_arm_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
-				_arm_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) &&
-		    (_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) {
-				feedback_provided = true;
-				valid_transition = false;
-			}
-		}
-
-		// Finish up the state transition
-		if (valid_transition) {
-			ret = TRANSITION_CHANGED;
-
-			// Record arm/disarm reason
-			if (isArmed() && (new_arming_state != vehicle_status_s::ARMING_STATE_ARMED)) { // disarm transition
-				status.latest_disarming_reason = (uint8_t)calling_reason;
-
-			} else if (!isArmed() && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { // arm transition
-				status.latest_arming_reason = (uint8_t)calling_reason;
-			}
-
-			// Switch state
-			_arm_state = new_arming_state;
-
-			if (isArmed()) {
-				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",
-					     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}",
-				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

@@ -1,96 +0,0 @@
-/****************************************************************************
- *
- *   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;
-
-	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,
-				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);
-
-	// 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 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
-	// 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/ArmStateMachineTest.cpp

@@ -1,297 +0,0 @@
-/****************************************************************************
- *
- *   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 <gtest/gtest.h>
-#include <ArmStateMachine.hpp>
-
-TEST(ArmStateMachineTest, ArmingStateTransitionTest)
-{
-	ArmStateMachine arm_state_machine;
-
-	// These are the critical values from vehicle_status_s and actuator_armed_s which must be primed
-	// to simulate machine state prior to testing an arming state transition. This structure is also
-	// use to represent the expected machine state after the transition has been requested.
-	typedef struct {
-		arming_state_t  arming_state;   // vehicle_status_s.arming_state
-		bool            armed;          // actuator_armed_s.armed
-	} ArmingTransitionVolatileState_t;
-
-	// This structure represents a test case for arming_state_transition. It contains the machine
-	// state prior to transition, the requested state to transition to and finally the expected
-	// machine state after transition.
-	typedef struct {
-		const char                     *assertMsg;                              // Text to show when test case fails
-		ArmingTransitionVolatileState_t current_state;                          // Machine state prior to transition
-		hil_state_t                     hil_state;                              // Current vehicle_status_s.hil_state
-		bool
-		system_sensors_initialized;   // Current vehicle_status_s.system_sensors_initialized
-		bool                            safety_switch_available;                // Current safety_s.safety_switch_available
-		bool                            safety_off;                             // Current safety_s.safety_off
-		arming_state_t                  requested_state;                        // Requested arming state to transition to
-		ArmingTransitionVolatileState_t expected_state;                         // Expected machine state after transition
-		transition_result_t             expected_transition_result;             // Expected result from arming_state_transition
-	} ArmingTransitionTest_t;
-
-	// We use these defines so that our test cases are more readable
-#define ATT_ARMED true
-#define ATT_DISARMED false
-#define ATT_SENSORS_INITIALIZED true
-#define ATT_SENSORS_NOT_INITIALIZED false
-#define ATT_SAFETY_AVAILABLE true
-#define ATT_SAFETY_NOT_AVAILABLE true
-#define ATT_SAFETY_OFF true
-#define ATT_SAFETY_ON false
-
-	// These are test cases for arming_state_transition
-	static const ArmingTransitionTest_t rgArmingTransitionTests[] = {
-		// TRANSITION_NOT_CHANGED tests
-
-		{
-			"no transition: identical states",
-			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-			vehicle_status_s::ARMING_STATE_INIT,
-			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, TRANSITION_NOT_CHANGED
-		},
-
-		// TRANSITION_CHANGED tests
-
-		// Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s
-
-		{
-			"transition: init to standby",
-			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-			vehicle_status_s::ARMING_STATE_STANDBY,
-			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED
-		},
-
-		{
-			"transition: init to standby error",
-			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-			vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
-			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_CHANGED
-		},
-
-		{
-			"transition: init to reboot",
-			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-			vehicle_status_s::ARMING_STATE_SHUTDOWN,
-			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED
-		},
-
-		{
-			"transition: standby to init",
-			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-			vehicle_status_s::ARMING_STATE_INIT,
-			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, TRANSITION_CHANGED
-		},
-
-		{
-			"transition: standby to standby error",
-			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-			vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
-			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_CHANGED
-		},
-
-		{
-			"transition: standby to reboot",
-			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-			vehicle_status_s::ARMING_STATE_SHUTDOWN,
-			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED
-		},
-
-		{
-			"transition: armed to standby",
-			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-			vehicle_status_s::ARMING_STATE_STANDBY,
-			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED
-		},
-
-		{
-			"transition: standby error to reboot",
-			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-			vehicle_status_s::ARMING_STATE_SHUTDOWN,
-			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED
-		},
-
-		{
-			"transition: in air restore to armed",
-			{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-			vehicle_status_s::ARMING_STATE_ARMED,
-			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED
-		},
-
-		{
-			"transition: in air restore to reboot",
-			{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-			vehicle_status_s::ARMING_STATE_SHUTDOWN,
-			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED
-		},
-
-		// hil on tests, standby error to standby not normally allowed
-
-		{
-			"transition: standby error to standby, hil on",
-			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-			vehicle_status_s::ARMING_STATE_STANDBY,
-			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED
-		},
-
-		// Safety switch arming tests
-
-		{
-			"transition: standby to armed, no safety switch",
-			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
-			vehicle_status_s::ARMING_STATE_ARMED,
-			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED
-		},
-
-		{
-			"transition: standby to armed, safety switch off",
-			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
-			vehicle_status_s::ARMING_STATE_ARMED,
-			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED
-		},
-
-		// TRANSITION_DENIED tests
-
-		// Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s
-
-		{
-			"no transition: init to armed",
-			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-			vehicle_status_s::ARMING_STATE_ARMED,
-			{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, TRANSITION_DENIED
-		},
-
-		{
-			"no transition: armed to init",
-			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-			vehicle_status_s::ARMING_STATE_INIT,
-			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_DENIED
-		},
-
-		{
-			"no transition: armed to reboot",
-			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-			vehicle_status_s::ARMING_STATE_SHUTDOWN,
-			{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_DENIED
-		},
-
-		{
-			"no transition: standby error to armed",
-			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-			vehicle_status_s::ARMING_STATE_ARMED,
-			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_DENIED
-		},
-
-		{
-			"no transition: standby error to standby",
-			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-			vehicle_status_s::ARMING_STATE_STANDBY,
-			{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_DENIED
-		},
-
-		{
-			"no transition: reboot to armed",
-			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-			vehicle_status_s::ARMING_STATE_ARMED,
-			{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_DENIED
-		},
-
-		{
-			"no transition: in air restore to standby",
-			{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-			vehicle_status_s::ARMING_STATE_STANDBY,
-			{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, TRANSITION_DENIED
-		},
-
-		// Sensor tests
-
-		//{ "transition to standby error: init to standby - sensors not initialized",
-		//    { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-		//    vehicle_status_s::ARMING_STATE_STANDBY,
-		//    { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
-
-		// Safety switch arming tests
-
-		{
-			"no transition: init to armed, safety switch on",
-			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
-			vehicle_status_s::ARMING_STATE_ARMED,
-			{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_DENIED
-		},
-	};
-
-	struct vehicle_status_s status {};
-	struct vehicle_status_flags_s status_flags {};
-	struct safety_s safety {};
-	struct actuator_armed_s armed {};
-
-	size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
-
-	for (size_t i = 0; i < cArmingTransitionTests; i++) {
-		const ArmingTransitionTest_t *test = &rgArmingTransitionTests[i];
-
-		PreFlightCheck::arm_requirements_t arm_req{};
-
-		// Setup initial machine 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
-		status_flags.circuit_breaker_engaged_power_check = true;
-		safety.safety_switch_available = test->safety_switch_available;
-		safety.safety_off = test->safety_off;
-
-		vehicle_control_mode_s control_mode{};
-
-		// Attempt transition
-		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,
-						     arm_req,
-						     2e6, /* 2 seconds after boot, everything should be checked */
-						     arm_disarm_reason_t::unit_test);
-
-		// Validate result of transition
-		EXPECT_EQ(result, test->expected_transition_result) << test->assertMsg;
-		EXPECT_EQ(arm_state_machine.getArmState(), test->expected_state.arming_state) << test->assertMsg;
-		EXPECT_EQ(arm_state_machine.isArmed(), test->expected_state.armed) << test->assertMsg;
-	}
-}

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

@@ -1,41 +0,0 @@
-############################################################################
-#
-#   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)
-
-px4_add_functional_gtest(SRC ArmStateMachineTest.cpp LINKLIBS ArmStateMachine)

src/modules/commander/Arming/CMakeLists.txt

@@ -32,6 +32,5 @@
 ############################################################################
 
 add_subdirectory(ArmAuthorization)
-add_subdirectory(ArmStateMachine)
 add_subdirectory(HealthFlags)
 add_subdirectory(PreFlightCheck)

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

@@ -33,7 +33,6 @@
 
 px4_add_library(PreFlightCheck
 	PreFlightCheck.cpp
-	checks/preArmCheck.cpp
 	checks/magnetometerCheck.cpp
 	checks/magConsistencyCheck.cpp
 	checks/accelerometerCheck.cpp
@@ -46,7 +45,6 @@ px4_add_library(PreFlightCheck
 	checks/powerCheck.cpp
 	checks/airframeCheck.cpp
 	checks/ekf2Check.cpp
-	checks/failureDetectorCheck.cpp
 	checks/manualControlCheck.cpp
 	checks/modeCheck.cpp
 	checks/cpuResourceCheck.cpp

src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp

@@ -52,10 +52,9 @@ static constexpr unsigned max_mandatory_baro_count = 1;
 
 bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
 				    vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode,
-				    bool report_failures, const bool prearm, const hrt_abstime &time_since_boot)
+				    bool report_failures)
 {
-	report_failures = (report_failures && status_flags.system_hotplug_timeout
-			   && !status_flags.calibration_enabled);
+	report_failures = (report_failures && !status_flags.calibration_enabled);
 
 	bool failed = false;
 
@@ -142,7 +141,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 
 		const float arming_max_airspeed_allowed = airspeed_trim / 2.0f; // set to half of trim airspeed
 
-		if (!airspeedCheck(mavlink_log_pub, status, optional, report_failures, prearm, (bool)max_airspeed_check_en,
+		if (!airspeedCheck(mavlink_log_pub, status, optional, report_failures, (max_airspeed_check_en == 1),
 				   arming_max_airspeed_allowed)
 		    && !(bool)optional) {
 			failed = true;
@@ -174,7 +173,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 
 	/* ---- SYSTEM POWER ---- */
 	if (status_flags.power_input_valid && !status_flags.circuit_breaker_engaged_power_check) {
-		if (!powerCheck(mavlink_log_pub, status, report_failures, prearm)) {
+		if (!powerCheck(mavlink_log_pub, status, report_failures)) {
 			failed = true;
 		}
 	}
@@ -193,22 +192,10 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 
 	if (estimator_type == 2) {
 
-		const bool in_grace_period = time_since_boot < 10_s;
-		const bool do_report_ekf2_failures = report_failures && (!in_grace_period || prearm);
-		const bool ekf_healthy = ekf2Check(mavlink_log_pub, status, false, do_report_ekf2_failures) &&
-					 ekf2CheckSensorBias(mavlink_log_pub, do_report_ekf2_failures);
-
-		// For the first 10 seconds the ekf2 can be unhealthy, and we just mark it
-		// as not present.
-		// After that or if we're forced to report, we'll set the flags as is.
-
-		if (!ekf_healthy && !do_report_ekf2_failures) {
-			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, false, false, status);
-
-		} else {
-			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, true, ekf_healthy, status);
-		}
+		const bool ekf_healthy = ekf2Check(mavlink_log_pub, status, report_failures) &&
+					 ekf2CheckSensorBias(mavlink_log_pub, report_failures);
 
+		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, true, ekf_healthy, status);
 
 		if (control_mode.flag_control_attitude_enabled
 		    || control_mode.flag_control_velocity_enabled
@@ -218,11 +205,6 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 		}
 	}
 
-	/* ---- Failure Detector ---- */
-	if (!failureDetectorCheck(mavlink_log_pub, status, report_failures, prearm)) {
-		failed = true;
-	}
-
 	failed = failed || !manualControlCheck(mavlink_log_pub, report_failures);
 	failed = failed || !modeCheck(mavlink_log_pub, report_failures, status);
 	failed = failed || !cpuResourceCheck(mavlink_log_pub, report_failures);

src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp

@@ -59,44 +59,10 @@ public:
 	/**
 	* Runs a preflight check on all sensors to see if they are properly calibrated and healthy
 	*
-	* The function won't fail the test if optional sensors are not found, however,
-	* it will fail the test if optional sensors are found but not in working condition.
-	*
-	* @param mavlink_log_pub
-	*   Mavlink output orb handle reference for feedback when a sensor fails
-	* @param checkMag
-	*   true if the magneteometer should be checked
-	* @param checkAcc
-	*   true if the accelerometers should be checked
-	* @param checkGyro
-	*   true if the gyroscopes should be checked
-	* @param checkBaro
-	*   true if the barometer should be checked
-	* @param checkAirspeed
-	*   true if the airspeed sensor should be checked
-	* @param checkRC
-	*   true if the Remote Controller should be checked
-	* @param checkGNSS
-	*   true if the GNSS receiver should be checked
-	* @param checkPower
-	*   true if the system power should be checked
 	**/
 	static bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
 				   vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode,
-				   bool reportFailures, const bool prearm, const hrt_abstime &time_since_boot);
-
-	struct arm_requirements_t {
-		bool arm_authorization = false;
-		bool esc_check = false;
-		bool global_position = false;
-		bool mission = false;
-		bool geofence = false;
-	};
-
-	static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
-				const vehicle_control_mode_s &control_mode,
-				const safety_s &safety, const arm_requirements_t &arm_requirements, vehicle_status_s &status,
-				bool report_fail = true);
+				   bool reportFailures);
 
 private:
 	static bool sensorAvailabilityCheck(const bool report_failure,
@@ -119,18 +85,13 @@ private:
 				  const bool is_mandatory, bool &report_fail);
 	static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status);
 	static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool optional,
-				  const bool report_fail, const bool prearm, const bool max_airspeed_check_en, const float arming_max_airspeed_allowed);
+				  const bool report_fail, const bool max_airspeed_check_en, const float arming_max_airspeed_allowed);
 	static int rcCalibrationCheck(orb_advert_t *mavlink_log_pub, bool report_fail);
-	static bool powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail,
-			       const bool prearm);
-	static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional,
-			      const bool report_fail);
+	static bool powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail);
+	static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool report_fail);
 
 	static bool ekf2CheckSensorBias(orb_advert_t *mavlink_log_pub, const bool report_fail);
 
-	static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail,
-					 const bool prearm);
-
 	static bool manualControlCheck(orb_advert_t *mavlink_log_pub, const bool report_fail);
 	static bool modeCheck(orb_advert_t *mavlink_log_pub, const bool report_fail, const vehicle_status_s &status);
 	static bool airframeCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status);

src/modules/commander/Arming/PreFlightCheck/checks/airspeedCheck.cpp

@@ -43,7 +43,7 @@
 using namespace time_literals;
 
 bool PreFlightCheck::airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool optional,
-				   const bool report_fail, const bool prearm, const bool max_airspeed_check_en, const float arming_max_airspeed_allowed)
+				   const bool report_fail, const bool max_airspeed_check_en, const float arming_max_airspeed_allowed)
 {
 	bool present = true;
 	bool success = true;
@@ -83,8 +83,7 @@ bool PreFlightCheck::airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status
 	 * Negative and positive offsets are considered. Do not check anymore while arming because pitot cover
 	 * might have been removed.
 	 */
-	if (max_airspeed_check_en && fabsf(airspeed_validated.calibrated_airspeed_m_s) > arming_max_airspeed_allowed
-	    && prearm) {
+	if (max_airspeed_check_en && fabsf(airspeed_validated.calibrated_airspeed_m_s) > arming_max_airspeed_allowed) {
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: check Airspeed Cal or pitot");
 		}

src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp

@@ -44,8 +44,7 @@
 
 using namespace time_literals;
 
-bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional,
-			       const bool report_fail)
+bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool report_fail)
 {
 	bool success = true; // start with a pass and change to a fail if any test fails
 

src/modules/commander/Arming/PreFlightCheck/checks/failureDetectorCheck.cpp

@@ -1,73 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2019 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 "../PreFlightCheck.hpp"
-
-#include <systemlib/mavlink_log.h>
-
-bool PreFlightCheck::failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
-		const bool report_fail, const bool prearm)
-{
-	// Ignore failure detector check after arming
-	if (!prearm) {
-		return true;
-	}
-
-	if (status.failure_detector_status != vehicle_status_s::FAILURE_NONE) {
-		if (report_fail) {
-			if (status.failure_detector_status & vehicle_status_s::FAILURE_ROLL) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Roll failure detected");
-			}
-
-			if (status.failure_detector_status & vehicle_status_s::FAILURE_PITCH) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Pitch failure detected");
-			}
-
-			if (status.failure_detector_status & vehicle_status_s::FAILURE_ALT) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Altitude failure detected");
-			}
-
-			if (status.failure_detector_status & vehicle_status_s::FAILURE_EXT) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Parachute failure detected");
-			}
-
-			if (status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: ESC failure detected");
-			}
-		}
-
-		return false;
-	}
-
-	return true;
-}

src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp

@@ -42,16 +42,10 @@
 
 using namespace time_literals;
 
-bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail,
-				const bool prearm)
+bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail)
 {
 	bool success = true;
 
-	if (!prearm) {
-		// Ignore power check after arming.
-		return true;
-	}
-
 	if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
 		// Ignore power check in HITL.
 		return true;

src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp

@@ -1,232 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2019-2020 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 "../PreFlightCheck.hpp"
-
-#include <ArmAuthorization.h>
-#include <systemlib/mavlink_log.h>
-#include <uORB/topics/vehicle_command_ack.h>
-#include <HealthFlags.h>
-
-bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
-				 const vehicle_control_mode_s &control_mode,
-				 const safety_s &safety, const arm_requirements_t &arm_requirements, vehicle_status_s &status, bool report_fail)
-{
-	bool prearm_ok = true;
-
-	// rate control mode require valid angular velocity
-	if (control_mode.flag_control_rates_enabled && !status_flags.angular_velocity_valid) {
-		if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! angular velocity invalid"); }
-
-		prearm_ok = false;
-	}
-
-	// attitude control mode require valid attitude
-	if (control_mode.flag_control_attitude_enabled && !status_flags.attitude_valid) {
-		if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! attitude invalid"); }
-
-		prearm_ok = false;
-	}
-
-	// velocity control mode require valid velocity
-	if (control_mode.flag_control_velocity_enabled && !status_flags.local_velocity_valid) {
-		if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! velocity invalid"); }
-
-		prearm_ok = false;
-	}
-
-	// position control mode require valid position
-	if (control_mode.flag_control_position_enabled && !status_flags.local_position_valid) {
-		if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! position invalid"); }
-
-		prearm_ok = false;
-	}
-
-	// manual control mode require valid manual control (rc)
-	if (control_mode.flag_control_manual_enabled && status.rc_signal_lost) {
-		if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! manual control lost"); }
-
-		prearm_ok = false;
-	}
-
-	if (status_flags.flight_terminated) {
-		if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Flight termination active"); }
-
-		prearm_ok = false;
-	}
-
-	// USB not connected
-	if (!status_flags.circuit_breaker_engaged_usb_check && status_flags.usb_connected) {
-		if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Flying with USB is not safe"); }
-
-		prearm_ok = false;
-	}
-
-	// battery and system power status
-	if (!status_flags.circuit_breaker_engaged_power_check) {
-
-		// Fail transition if power is not good
-		if (!status_flags.power_input_valid) {
-			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Connect power module"); }
-
-			prearm_ok = false;
-		}
-
-		// main battery level
-		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_SENSORBATTERY, true, true,
-				 status_flags.battery_healthy, status);
-
-		// Only arm if healthy
-		if (!status_flags.battery_healthy) {
-			if (prearm_ok) {
-				if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Check battery"); }
-			}
-
-			prearm_ok = false;
-		}
-	}
-
-	// Arm Requirements: mission
-	if (arm_requirements.mission) {
-
-		if (!status_flags.auto_mission_available) {
-			if (prearm_ok) {
-				if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! No valid mission"); }
-			}
-
-			prearm_ok = false;
-		}
-
-		if (!status_flags.global_position_valid) {
-			if (prearm_ok) {
-				if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Missions require a global position"); }
-			}
-
-			prearm_ok = false;
-		}
-	}
-
-	if (arm_requirements.global_position && !status_flags.circuit_breaker_engaged_posfailure_check) {
-
-		if (!status_flags.global_position_valid) {
-			if (prearm_ok) {
-				if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Global position required"); }
-			}
-
-			prearm_ok = false;
-		}
-
-		if (!status_flags.home_position_valid) {
-			if (prearm_ok) {
-				if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Home position invalid"); }
-			}
-
-			prearm_ok = false;
-		}
-	}
-
-	// safety button
-	if (safety.safety_switch_available && !safety.safety_off) {
-		// Fail transition if we need safety switch press
-		if (prearm_ok) {
-			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Press safety switch first"); }
-		}
-
-		prearm_ok = false;
-	}
-
-	if (status_flags.avoidance_system_required && !status_flags.avoidance_system_valid) {
-		if (prearm_ok) {
-			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Avoidance system not ready"); }
-		}
-
-		prearm_ok = false;
-
-	}
-
-	if (arm_requirements.esc_check && status_flags.escs_error) {
-		if (prearm_ok) {
-			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! One or more ESCs are offline"); }
-
-			prearm_ok = false;
-		}
-	}
-
-	if (arm_requirements.esc_check && status_flags.escs_failure) {
-		if (prearm_ok) {
-			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! One or more ESCs have a failure"); }
-
-			prearm_ok = false;
-		}
-	}
-
-	if (status.is_vtol) {
-
-		if (status.in_transition_mode) {
-			if (prearm_ok) {
-				if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Vehicle is in transition state"); }
-
-				prearm_ok = false;
-			}
-		}
-
-		if (!status_flags.circuit_breaker_vtol_fw_arming_check
-		    && status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-			if (prearm_ok) {
-				if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Vehicle is not in multicopter mode"); }
-
-				prearm_ok = false;
-			}
-		}
-	}
-
-	if (arm_requirements.geofence && status.geofence_violated) {
-		if (report_fail) {
-			mavlink_log_critical(mavlink_log_pub, "Arming denied, vehicle outside geofence");
-		}
-
-		prearm_ok = false;
-	}
-
-	// Arm Requirements: authorization
-	// check last, and only if everything else has passed
-	if (arm_requirements.arm_authorization && prearm_ok) {
-		if (arm_auth_check() != vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED) {
-			// feedback provided in arm_auth_check
-			prearm_ok = false;
-		}
-	}
-
-
-	return prearm_ok;
-}

src/modules/commander/CMakeLists.txt

@@ -55,6 +55,7 @@ px4_add_module(
 		Safety.cpp
 		state_machine_helper.cpp
 		worker_thread.cpp
+		preArmCheck.cpp
 	DEPENDS
 		circuit_breaker
 		failure_detector
@@ -62,7 +63,6 @@ px4_add_module(
 		hysteresis
 		PreFlightCheck
 		ArmAuthorization
-		ArmStateMachine
 		HealthFlags
 		sensor_calibration
 		world_magnetic_model

src/modules/commander/Commander.hpp

@@ -34,7 +34,6 @@
 #pragma once
 
 /*   Helper classes  */
-#include "Arming/ArmStateMachine/ArmStateMachine.hpp"
 #include "Arming/PreFlightCheck/PreFlightCheck.hpp"
 #include "failure_detector/FailureDetector.hpp"
 #include "Safety.hpp"
@@ -98,6 +97,8 @@ using systemlib::Hysteresis;
 
 using namespace time_literals;
 
+using arm_disarm_reason_t = events::px4::enums::arm_disarm_reason_t;
+
 class Commander : public ModuleBase<Commander>, public ModuleParams
 {
 public:
@@ -129,7 +130,7 @@ public:
 private:
 	void answer_command(const vehicle_command_s &cmd, uint8_t result);
 
-	transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true);
+	transition_result_t arm(arm_disarm_reason_t calling_reason, bool force = false);
 	transition_result_t disarm(arm_disarm_reason_t calling_reason, bool forced = false);
 
 	void battery_status_check();
@@ -146,9 +147,8 @@ private:
 	void data_link_check();
 
 	void avoidance_check();
-
+	void cpuload_check();
 	void esc_status_check();
-
 	void estimator_check();
 
 	bool handle_command(const vehicle_command_s &cmd);
@@ -175,14 +175,26 @@ private:
 
 	void update_control_mode();
 
-	bool shutdown_if_allowed();
-
 	bool stabilization_required();
 
 	void send_parachute_command();
 
 	void checkWindSpeedThresholds();
 
+	struct arm_requirements_t {
+		bool arm_authorization = false;
+		bool esc_check = false;
+		bool global_position = false;
+		bool mission = false;
+		bool geofence = false;
+	};
+
+	bool preArmCheck(const vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode,
+			 const safety_s &safety, const arm_requirements_t &arm_requirements, const vehicle_status_s &status,
+			 bool report_fail = true);
+
+	void runPreflight(bool report_preflight = true, bool report_prearm = true);
+
 	void updateParameters();
 
 	DEFINE_PARAMETERS(
@@ -257,6 +269,7 @@ private:
 		(ParamInt<px4::params::CBRK_VELPOSERR>) _param_cbrk_velposerr,
 		(ParamInt<px4::params::CBRK_VTOLARMING>) _param_cbrk_vtolarming,
 
+		(ParamFloat<px4::params::COM_CPU_MAX>) _param_com_cpu_max,
 		(ParamInt<px4::params::COM_FLT_TIME_MAX>) _param_com_flt_time_max,
 		(ParamFloat<px4::params::COM_WIND_MAX>) _param_com_wind_max
 	)
@@ -291,10 +304,8 @@ private:
 	static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};
 
 	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{};
+	arm_requirements_t _arm_requirements{};
 
 	hrt_abstime	_valid_distance_sensor_time_us{0}; /**< Last time that distance sensor data arrived (usec) */
 
@@ -371,6 +382,7 @@ private:
 
 	hrt_abstime	_boot_timestamp{0};
 	hrt_abstime	_last_disarmed_timestamp{0};
+	hrt_abstime	_last_preflight_check{0};
 	hrt_abstime	_overload_start{0};		///< time when CPU overload started
 
 	hrt_abstime _led_armed_state_toggle{0};
@@ -381,11 +393,7 @@ private:
 	uint8_t		_heading_reset_counter{0};
 
 	bool		_status_changed{true};
-	bool		_arm_tune_played{false};
-	bool		_was_armed{false};
-	bool		_failsafe_old{false};	///< check which state machines for changes, clear "changed" flag
 	bool		_have_taken_off_since_arming{false};
-	bool		_system_power_usb_connected{false};
 
 	geofence_result_s	_geofence_result{};
 	vehicle_land_detected_s	_vehicle_land_detected{};

src/modules/commander/commander_params.c

@@ -967,7 +967,7 @@ PARAM_DEFINE_FLOAT(COM_KILL_DISARM, 5.0f);
  * @max 100
  * @increment 1
  */
-PARAM_DEFINE_FLOAT(COM_CPU_MAX, 90.0f);
+PARAM_DEFINE_FLOAT(COM_CPU_MAX, 95.0f);
 
 /**
  * Required number of redundant power modules

src/modules/commander/commander_tests/state_machine_helper_test.cpp

@@ -42,7 +42,6 @@
 
 #include "../state_machine_helper.h"
 #include <unit_test.h>
-#include "../Arming/ArmStateMachine/ArmStateMachine.hpp"
 #include "../Arming/PreFlightCheck/PreFlightCheck.hpp"
 
 class StateMachineHelperTest : public UnitTest

src/modules/commander/preArmCheck.cpp

@@ -0,0 +1,282 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019-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 "Commander.hpp"
+
+#include <systemlib/mavlink_log.h>
+
+bool Commander::preArmCheck(const vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode,
+			    const safety_s &safety, const arm_requirements_t &arm_requirements, const vehicle_status_s &status,
+			    bool report_fail)
+{
+	bool prearm_ok = true;
+
+	// rate control mode require valid angular velocity
+	if (control_mode.flag_control_rates_enabled && !status_flags.angular_velocity_valid) {
+		if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! angular velocity invalid"); }
+
+		prearm_ok = false;
+	}
+
+	// attitude control mode require valid attitude
+	if (control_mode.flag_control_attitude_enabled && !status_flags.attitude_valid) {
+		if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! attitude invalid"); }
+
+		prearm_ok = false;
+	}
+
+	// velocity control mode require valid velocity
+	if (control_mode.flag_control_velocity_enabled && !status_flags.local_velocity_valid) {
+		if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! velocity invalid"); }
+
+		prearm_ok = false;
+	}
+
+	// position control mode require valid position
+	if (control_mode.flag_control_position_enabled && !status_flags.local_position_valid) {
+		if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! position invalid"); }
+
+		prearm_ok = false;
+	}
+
+	// manual control mode require valid manual control (rc)
+	if (control_mode.flag_control_manual_enabled && status.rc_signal_lost) {
+		if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! manual control lost"); }
+
+		prearm_ok = false;
+	}
+
+	if (status_flags.flight_terminated) {
+		if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Flight termination active"); }
+
+		prearm_ok = false;
+	}
+
+	// USB not connected
+	if (!status_flags.circuit_breaker_engaged_usb_check && status_flags.usb_connected) {
+		if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Flying with USB is not safe"); }
+
+		prearm_ok = false;
+	}
+
+	// battery and system power status
+	if (!status_flags.circuit_breaker_engaged_power_check) {
+
+		// Fail transition if power is not good
+		if (!status_flags.power_input_valid) {
+			if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Connect power module"); }
+
+			prearm_ok = false;
+		}
+
+		// Only arm if healthy
+		if (!status_flags.battery_healthy) {
+			if (prearm_ok) {
+				if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Check battery"); }
+			}
+
+			prearm_ok = false;
+		}
+	}
+
+	// Arm Requirements: mission
+	if (arm_requirements.mission) {
+
+		if (!status_flags.auto_mission_available) {
+			if (prearm_ok) {
+				if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! No valid mission"); }
+			}
+
+			prearm_ok = false;
+		}
+
+		if (!status_flags.global_position_valid) {
+			if (prearm_ok) {
+				if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Missions require a global position"); }
+			}
+
+			prearm_ok = false;
+		}
+	}
+
+	if (arm_requirements.global_position && !status_flags.circuit_breaker_engaged_posfailure_check) {
+
+		if (!status_flags.global_position_valid) {
+			if (prearm_ok) {
+				if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Global position required"); }
+			}
+
+			prearm_ok = false;
+		}
+
+		if (!status_flags.home_position_valid) {
+			if (prearm_ok) {
+				if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Home position invalid"); }
+			}
+
+			prearm_ok = false;
+		}
+	}
+
+	// safety button
+	if (safety.safety_switch_available && !safety.safety_off) {
+		// Fail transition if we need safety switch press
+		if (prearm_ok) {
+			if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Press safety switch first"); }
+		}
+
+		prearm_ok = false;
+	}
+
+	if (status_flags.avoidance_system_required && !status_flags.avoidance_system_valid) {
+		if (prearm_ok) {
+			if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Avoidance system not ready"); }
+		}
+
+		prearm_ok = false;
+
+	}
+
+
+	if (control_mode.flag_control_manual_enabled) {
+		if (control_mode.flag_control_climb_rate_enabled &&
+		    !_status.rc_signal_lost && _is_throttle_above_center) {
+
+			if (report_fail) {
+				mavlink_log_critical(&_mavlink_log_pub, "Arming denied: throttle above center\t");
+				events::send(events::ID("commander_arm_denied_throttle_center"), {events::Log::Critical, events::LogInternal::Info},
+					     "Arming denied: throttle above center");
+			}
+
+			prearm_ok = false;
+		}
+
+		if (!control_mode.flag_control_climb_rate_enabled &&
+		    !_status.rc_signal_lost && !_is_throttle_low && _status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROVER) {
+
+			if (report_fail) {
+				mavlink_log_critical(&_mavlink_log_pub, "Arming denied: high throttle\t");
+				events::send(events::ID("commander_arm_denied_throttle_high"), {events::Log::Critical, events::LogInternal::Info},
+					     "Arming denied: high throttle");
+			}
+
+			prearm_ok = false;
+		}
+
+	}
+
+	if ((_geofence_result.geofence_action == geofence_result_s::GF_ACTION_RTL)
+	    && !status_flags.home_position_valid) {
+
+		if (report_fail) {
+			mavlink_log_critical(&_mavlink_log_pub, "Arming denied: Geofence RTL requires valid home\t");
+			events::send(events::ID("commander_arm_denied_geofence_rtl"), {events::Log::Critical, events::LogInternal::Info},
+				     "Arming denied: Geofence RTL requires valid home");
+		}
+
+		prearm_ok = false;
+	}
+
+	// failure detector
+	if (status.failure_detector_status != vehicle_status_s::FAILURE_NONE) {
+		if (report_fail) {
+			if (status.failure_detector_status & vehicle_status_s::FAILURE_ROLL) {
+				mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Roll failure detected");
+			}
+
+			if (status.failure_detector_status & vehicle_status_s::FAILURE_PITCH) {
+				mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Pitch failure detected");
+			}
+
+			if (status.failure_detector_status & vehicle_status_s::FAILURE_ALT) {
+				mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Altitude failure detected");
+			}
+
+			if (status.failure_detector_status & vehicle_status_s::FAILURE_EXT) {
+				mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Parachute failure detected");
+			}
+
+			if (status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) {
+				mavlink_log_critical(&_mavlink_log_pub, "Arming denied! ESC failure detected");
+			}
+		}
+
+		prearm_ok = false;
+	}
+
+	if (arm_requirements.esc_check && status_flags.escs_error) {
+		if (prearm_ok) {
+			if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! One or more ESCs are offline"); }
+
+			prearm_ok = false;
+		}
+	}
+
+	if (arm_requirements.esc_check && status_flags.escs_failure) {
+		if (prearm_ok) {
+			if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! One or more ESCs have a failure"); }
+
+			prearm_ok = false;
+		}
+	}
+
+	if (status.is_vtol) {
+
+		if (status.in_transition_mode) {
+			if (prearm_ok) {
+				if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Vehicle is in transition state"); }
+
+				prearm_ok = false;
+			}
+		}
+
+		if (!status_flags.circuit_breaker_vtol_fw_arming_check
+		    && status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+			if (prearm_ok) {
+				if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Vehicle is not in multicopter mode"); }
+
+				prearm_ok = false;
+			}
+		}
+	}
+
+	if (arm_requirements.geofence && status.geofence_violated) {
+		if (report_fail) {
+			mavlink_log_critical(&_mavlink_log_pub, "Arming denied, vehicle outside geofence");
+		}
+
+		prearm_ok = false;
+	}
+
+	return prearm_ok;
+}

src/modules/mavlink/streams/HEARTBEAT.hpp

@@ -112,27 +112,28 @@ private:
 			// uint8_t system_status (MAV_STATE) - System status flag.
 			uint8_t system_status = MAV_STATE_UNINIT;
 
-			switch (vehicle_status.arming_state) {
-			case vehicle_status_s::ARMING_STATE_ARMED:
-				system_status = vehicle_status.failsafe ? MAV_STATE_CRITICAL : MAV_STATE_ACTIVE;
-				break;
+			if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+				if (!vehicle_status.failsafe) {
+					system_status = MAV_STATE_ACTIVE;
 
-			case vehicle_status_s::ARMING_STATE_STANDBY:
-				system_status = MAV_STATE_STANDBY;
-				break;
+				} else {
+					system_status = MAV_STATE_CRITICAL;
+				}
 
-			case vehicle_status_s::ARMING_STATE_SHUTDOWN:
-				system_status = MAV_STATE_POWEROFF;
-				break;
+			} else {
+				// disarmed
+				if (vehicle_status_flags.calibration_enabled) {
+					system_status = MAV_STATE_CALIBRATING;
+
+				} else {
+					system_status = MAV_STATE_STANDBY;
+				}
 			}
 
 			// system_status overrides
 			if (actuator_armed.force_failsafe || actuator_armed.lockdown || actuator_armed.manual_lockdown
 			    || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_TERMINATION) {
 				system_status = MAV_STATE_FLIGHT_TERMINATION;
-
-			} else if (vehicle_status_flags.calibration_enabled) {
-				system_status = MAV_STATE_CALIBRATING;
 			}