commander: move most static variables and parameters to class

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

@@ -78,19 +78,18 @@ public:
 	*   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,
+				   vehicle_status_flags_s &status_flags, const bool checkGNSS, bool reportFailures, const bool prearm,
-				   const bool checkGNSS, bool reportFailures, const bool prearm, const hrt_abstime &time_since_boot);
+				   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;
+	};
 
 	static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
-				const safety_s &safety, const uint8_t arm_requirements, const vehicle_status_s &status);
+				const safety_s &safety, const arm_requirements_t &arm_requirements, const vehicle_status_s &status);
-
-	typedef enum {
-		ARM_REQ_NONE = 0,
-		ARM_REQ_MISSION_BIT = (1 << 0),
-		ARM_REQ_ARM_AUTH_BIT = (1 << 1),
-		ARM_REQ_GPS_BIT = (1 << 2),
-		ARM_REQ_ESCS_CHECK_BIT = (1 << 3)
-	} arm_requirements_t;
 
 private:
 	static bool magnometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,

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

@@ -38,7 +38,7 @@
 #include <uORB/topics/vehicle_command_ack.h>
 
 bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
-				 const safety_s &safety, const uint8_t arm_requirements, const vehicle_status_s &status)
+				 const safety_s &safety, const arm_requirements_t &arm_requirements, const vehicle_status_s &status)
 {
 	bool prearm_ok = true;
 
@@ -70,7 +70,7 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
 	}
 
 	// Arm Requirements: mission
-	if (arm_requirements & ARM_REQ_MISSION_BIT) {
+	if (arm_requirements.mission) {
 
 		if (!status_flags.condition_auto_mission_available) {
 			if (prearm_ok) {
@@ -90,7 +90,7 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
 	}
 
 	// Arm Requirements: global position
-	if (arm_requirements & ARM_REQ_GPS_BIT) {
+	if (arm_requirements.global_position) {
 
 		if (!status_flags.condition_global_position_valid) {
 			if (prearm_ok) {
@@ -120,7 +120,7 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
 
 	}
 
-	if (status_flags.condition_escs_error && (arm_requirements & ARM_REQ_ESCS_CHECK_BIT)) {
+	if (arm_requirements.esc_check && status_flags.condition_escs_error) {
 		if (prearm_ok) {
 			mavlink_log_critical(mavlink_log_pub, "Arming denied! One or more ESCs are offline");
 			prearm_ok = false;
@@ -136,7 +136,7 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
 
 	// Arm Requirements: authorization
 	// check last, and only if everything else has passed
-	if ((arm_requirements & ARM_REQ_ARM_AUTH_BIT) && prearm_ok) {
+	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;

src/modules/commander/Commander.hpp

@@ -34,42 +34,55 @@
 #ifndef COMMANDER_HPP_
 #define COMMANDER_HPP_
 
-#include "state_machine_helper.h"
+#include "Arming/PreFlightCheck/PreFlightCheck.hpp"
 #include "failure_detector/FailureDetector.hpp"
+#include "state_machine_helper.h"
 
 #include <lib/controllib/blocks.hpp>
+#include <lib/hysteresis/hysteresis.h>
 #include <lib/mathlib/mathlib.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
-#include <lib/hysteresis/hysteresis.h>
 
 // publications
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationQueued.hpp>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/home_position.h>
+#include <uORB/topics/test_motor.h>
 #include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_status_flags.h>
-#include <uORB/topics/test_motor.h>
 
 // subscriptions
 #include <uORB/Subscription.hpp>
+#include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/airspeed.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/cpuload.h>
+#include <uORB/topics/esc_status.h>
 #include <uORB/topics/estimator_status.h>
+#include <uORB/topics/geofence_result.h>
 #include <uORB/topics/iridiumsbd_status.h>
+#include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/offboard_control_mode.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/power_button_state.h>
+#include <uORB/topics/safety.h>
+#include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/system_power.h>
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/vehicle_acceleration.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/esc_status.h>
+#include <uORB/topics/vtol_vehicle_status.h>
 
 using math::constrain;
+using systemlib::Hysteresis;
 
 using namespace time_literals;
 
@@ -95,13 +108,65 @@ public:
 
 	void enable_hil();
 
-	// TODO: only temporarily static until low priority thread is removed
-	static bool preflight_check(bool report);
-
 	void get_circuit_breaker_params();
 
 private:
 
+	transition_result_t arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink_log_pub, const char *armedBy);
+
+	void battery_status_check();
+
+	void check_valid(const hrt_abstime &timestamp, const hrt_abstime &timeout, const bool valid_in, bool *valid_out,
+			 bool *changed);
+
+	bool check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy,
+				   const hrt_abstime &data_timestamp_us, hrt_abstime *last_fail_time_us, hrt_abstime *probation_time_us, bool *valid_state,
+				   bool *validity_changed);
+
+	void control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed,
+				 const uint8_t battery_warning);
+
+	/**
+	 * Checks the status of all available data links and handles switching between different system telemetry states.
+	 */
+	void data_link_check();
+
+	void esc_status_check(const esc_status_s &esc_status);
+
+	void estimator_check();
+
+	bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd, actuator_armed_s *armed,
+			    uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub);
+
+	unsigned handle_command_motor_test(const vehicle_command_s &cmd);
+
+	void mission_init();
+
+	void offboard_control_update();
+
+	void print_reject_arm(const char *msg);
+	void print_reject_mode(const char *msg);
+
+	void reset_posvel_validity(bool *changed);
+
+	bool set_home_position();
+	bool set_home_position_alt_only();
+
+	void update_control_mode();
+
+	// Set the main system state based on RC and override device inputs
+	transition_result_t set_main_state(const vehicle_status_s &status, bool *changed);
+
+	// Enable override (manual reversion mode) on the system
+	transition_result_t set_main_state_override_on(const vehicle_status_s &status, bool *changed);
+
+	// Set the system main state based on the current RC inputs
+	transition_result_t set_main_state_rc(const vehicle_status_s &status, bool *changed);
+
+	bool shutdown_if_allowed();
+
+	bool stabilization_required();
+
 	DEFINE_PARAMETERS(
 
 		(ParamInt<px4::params::NAV_DLL_ACT>) _param_nav_dll_act,
@@ -129,28 +194,63 @@ private:
 		(ParamFloat<px4::params::COM_DISARM_LAND>) _param_com_disarm_land,
 		(ParamFloat<px4::params::COM_DISARM_PRFLT>) _param_com_disarm_preflight,
 
-		(ParamInt<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid,
+		(ParamBool<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid,
 		(ParamInt<px4::params::COM_OA_BOOT_T>) _param_com_oa_boot_t,
 
 		(ParamInt<px4::params::COM_FLT_PROFILE>) _param_com_flt_profile,
 
-
+		// Offboard
 		(ParamFloat<px4::params::COM_OF_LOSS_T>) _param_com_of_loss_t,
 		(ParamInt<px4::params::COM_OBL_ACT>) _param_com_obl_act,
 		(ParamInt<px4::params::COM_OBL_RC_ACT>) _param_com_obl_rc_act,
 
 		(ParamInt<px4::params::COM_PREARM_MODE>) _param_com_prearm_mode,
-		(ParamInt<px4::params::COM_MOT_TEST_EN>) _param_com_mot_test_en,
+		(ParamBool<px4::params::COM_MOT_TEST_EN>) _param_com_mot_test_en,
 
 		(ParamFloat<px4::params::COM_KILL_DISARM>) _param_com_kill_disarm,
 
+		// Engine failure
+		(ParamFloat<px4::params::COM_EF_THROT>) _param_ef_throttle_thres,
+		(ParamFloat<px4::params::COM_EF_C2T>) _param_ef_current2throttle_thres,
+		(ParamFloat<px4::params::COM_EF_TIME>) _param_ef_time_thres,
+
+		(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_arm_without_gps,
+		(ParamBool<px4::params::COM_ARM_SWISBTN>) _param_arm_switch_is_button,
+		(ParamBool<px4::params::COM_ARM_MIS_REQ>) _param_arm_mission_required,
+		(ParamBool<px4::params::COM_ARM_AUTH_REQ>) _param_arm_auth_required,
+		(ParamBool<px4::params::COM_ARM_CHK_ESCS>) _param_escs_checks_required,
+
+		(ParamInt<px4::params::COM_FLIGHT_UUID>) _param_flight_uuid,
+		(ParamInt<px4::params::COM_TAKEOFF_ACT>) _param_takeoff_finished_action,
+
+		(ParamBool<px4::params::COM_RC_OVERRIDE>) _param_rc_override,
+		(ParamInt<px4::params::COM_RC_IN_MODE>) _param_rc_in_off,
+		(ParamInt<px4::params::COM_RC_ARM_HYST>) _param_rc_arm_hyst,
+		(ParamFloat<px4::params::COM_RC_STICK_OV>) _param_min_stick_change,
+
+		(ParamInt<px4::params::COM_FLTMODE1>) _param_fltmode_1,
+		(ParamInt<px4::params::COM_FLTMODE2>) _param_fltmode_2,
+		(ParamInt<px4::params::COM_FLTMODE3>) _param_fltmode_3,
+		(ParamInt<px4::params::COM_FLTMODE4>) _param_fltmode_4,
+		(ParamInt<px4::params::COM_FLTMODE5>) _param_fltmode_5,
+		(ParamInt<px4::params::COM_FLTMODE6>) _param_fltmode_6,
+
+		// Circuit breakers
 		(ParamInt<px4::params::CBRK_SUPPLY_CHK>) _param_cbrk_supply_chk,
 		(ParamInt<px4::params::CBRK_USB_CHK>) _param_cbrk_usb_chk,
 		(ParamInt<px4::params::CBRK_AIRSPD_CHK>) _param_cbrk_airspd_chk,
 		(ParamInt<px4::params::CBRK_ENGINEFAIL>) _param_cbrk_enginefail,
 		(ParamInt<px4::params::CBRK_GPSFAIL>) _param_cbrk_gpsfail,
 		(ParamInt<px4::params::CBRK_FLIGHTTERM>) _param_cbrk_flightterm,
-		(ParamInt<px4::params::CBRK_VELPOSERR>) _param_cbrk_velposerr
+		(ParamInt<px4::params::CBRK_VELPOSERR>) _param_cbrk_velposerr,
+
+		// Geofrence
+		(ParamInt<px4::params::GF_ACTION>) _param_geofence_action,
+
+		// Mavlink
+		(ParamInt<px4::params::MAV_COMP_ID>) _param_mav_comp_id,
+		(ParamInt<px4::params::MAV_SYS_ID>) _param_mav_sys_id,
+		(ParamInt<px4::params::MAV_TYPE>) _param_mav_type
 	)
 
 	enum class PrearmedMode {
@@ -159,9 +259,22 @@ private:
 		ALWAYS = 2
 	};
 
+	/* Decouple update interval and hysteresis counters, all depends on intervals */
+	static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};
+	static constexpr float COMMANDER_MONITORING_LOOPSPERMSEC{1 / (COMMANDER_MONITORING_INTERVAL / 1000.0f)};
+
+	static constexpr float STICK_ON_OFF_LIMIT{0.9f};
+
+	static constexpr uint64_t OFFBOARD_TIMEOUT{500_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 PRINT_MODE_REJECT_INTERVAL{500_ms};
+	static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms};
+
 	const int64_t POSVEL_PROBATION_MIN = 1_s;	/**< minimum probation duration (usec) */
 	const int64_t POSVEL_PROBATION_MAX = 100_s;	/**< maximum probation duration (usec) */
 
+	PreFlightCheck::arm_requirements_t	_arm_requirements{};
+
 	hrt_abstime	_last_gpos_fail_time_us{0};	/**< Last time that the global position validity recovery check failed (usec) */
 	hrt_abstime	_last_lpos_fail_time_us{0};	/**< Last time that the local position validity recovery check failed (usec) */
 	hrt_abstime	_last_lvel_fail_time_us{0};	/**< Last time that the local velocity validity recovery check failed (usec) */
@@ -177,89 +290,99 @@ private:
 	bool		_nav_test_passed{false};	/**< true if the post takeoff navigation test has passed */
 	bool		_nav_test_failed{false};	/**< true if the post takeoff navigation test has failed */
 
-	bool _geofence_loiter_on{false};
+	bool		_geofence_loiter_on{false};
-	bool _geofence_rtl_on{false};
+	bool		_geofence_rtl_on{false};
-	bool _geofence_warning_action_on{false};
+	bool		_geofence_warning_action_on{false};
-	bool _geofence_violated_prev{false};
+	bool		_geofence_violated_prev{false};
 
-	FailureDetector _failure_detector;
+	FailureDetector	_failure_detector;
-	bool _flight_termination_triggered{false};
+	bool		_flight_termination_triggered{false};
 
-	bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd, actuator_armed_s *armed,
-			    uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub, bool *changed);
-
-	unsigned handle_command_motor_test(const vehicle_command_s &cmd);
-
-	bool set_home_position();
-	bool set_home_position_alt_only();
-
-	// Set the main system state based on RC and override device inputs
-	transition_result_t set_main_state(const vehicle_status_s &status, bool *changed);
-
-	// Enable override (manual reversion mode) on the system
-	transition_result_t set_main_state_override_on(const vehicle_status_s &status, bool *changed);
-
-	// Set the system main state based on the current RC inputs
-	transition_result_t set_main_state_rc(const vehicle_status_s &status, bool *changed);
-
-	void update_control_mode();
-
-	void check_valid(const hrt_abstime &timestamp, const hrt_abstime &timeout, const bool valid_in, bool *valid_out,
-			 bool *changed);
-
-	bool check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy,
-				   const hrt_abstime &data_timestamp_us, hrt_abstime *last_fail_time_us, hrt_abstime *probation_time_us, bool *valid_state,
-				   bool *validity_changed);
-
-	void reset_posvel_validity(bool *changed);
-
-	void mission_init();
-
-	void estimator_check(bool *status_changed);
-
-	void offboard_control_update(bool &status_changed);
-
-	void battery_status_check();
-
-	void esc_status_check(const esc_status_s &esc_status);
-
-	/**
-	 * Checks the status of all available data links and handles switching between different system telemetry states.
-	 */
-	void		data_link_check(bool &status_changed);
-
-	uORB::Subscription _telemetry_status_sub{ORB_ID(telemetry_status)};
 
 	hrt_abstime	_datalink_last_heartbeat_gcs{0};
-
-	hrt_abstime	_datalink_last_heartbeat_onboard_controller{0};
-	bool 				_onboard_controller_lost{false};
-
 	hrt_abstime	_datalink_last_heartbeat_avoidance_system{0};
-	bool				_avoidance_system_lost{false};
+	hrt_abstime	_datalink_last_heartbeat_onboard_controller{0};
-
+	bool		_onboard_controller_lost{false};
+	bool		_avoidance_system_lost{false};
 	bool		_avoidance_system_status_change{false};
-	uint8_t	_datalink_last_status_avoidance_system{telemetry_status_s::MAV_STATE_UNINIT};
+	uint8_t		_datalink_last_status_avoidance_system{telemetry_status_s::MAV_STATE_UNINIT};
-
-	uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
 
 	hrt_abstime	_high_latency_datalink_heartbeat{0};
 	hrt_abstime	_high_latency_datalink_lost{0};
 
-	int  _last_esc_online_flags{-1};
+	int		_last_esc_online_flags{-1};
 
-	uORB::Subscription _battery_sub{ORB_ID(battery_status)};
+	uint8_t		_battery_warning{battery_status_s::BATTERY_WARNING_NONE};
-	uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
+	float		_battery_current{0.0f};
-	float _battery_current{0.0f};
 
-	systemlib::Hysteresis	_auto_disarm_landed{false};
+	Hysteresis	_auto_disarm_landed{false};
-	systemlib::Hysteresis	_auto_disarm_killed{false};
+	Hysteresis	_auto_disarm_killed{false};
 
-	bool _print_avoidance_msg_once{false};
+	bool		_print_avoidance_msg_once{false};
+
+	hrt_abstime	_last_print_mode_reject_time{0};	///< To remember when last notification was sent
+
+	float		_eph_threshold_adj{INFINITY};	///< maximum allowable horizontal position uncertainty after adjustment for flight condition
+	bool		_skip_pos_accuracy_check{false};
+	bool		_last_condition_local_altitude_valid{false};
+	bool		_last_condition_local_position_valid{false};
+	bool		_last_condition_global_position_valid{false};
+
+	bool		_last_overload{false};
+
+	unsigned int	_leds_counter{0};
+
+	manual_control_setpoint_s	_sp_man{};		///< the current manual control setpoint
+	manual_control_setpoint_s	_last_sp_man{};	///< the manual control setpoint valid at the last mode switch
+	hrt_abstime	_rc_signal_lost_timestamp{0};		///< Time at which the RC reception was lost
+	int32_t		_flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_NUM] {};
+	uint8_t		_last_sp_man_arm_switch{0};
+	float		_min_stick_change{};
+	uint32_t	_stick_off_counter{0};
+	uint32_t	_stick_on_counter{0};
+
+	hrt_abstime	_boot_timestamp{0};
+	hrt_abstime	_last_disarmed_timestamp{0};
+	hrt_abstime	_timestamp_engine_healthy{0}; ///< absolute time when engine was healty
+
+	uint32_t	_counter{0};
+
+	bool		_status_changed{true};
+	bool		_arm_tune_played{false};
+	bool		_was_landed{true};
+	bool		_was_falling{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		_flight_termination_printed{false};
+
+	main_state_t	_main_state_pre_offboard{commander_state_s::MAIN_STATE_MANUAL};
+
+	commander_state_s	_internal_state{};
+	cpuload_s		_cpuload{};
+	geofence_result_s	_geofence_result{};
+	vehicle_land_detected_s	_land_detector{};
+	safety_s		_safety{};
+	vtol_vehicle_status_s	_vtol_status{};
 
 	// Subscriptions
+	uORB::Subscription					_actuator_controls_sub{ORB_ID_VEHICLE_ATTITUDE_CONTROLS};
+	uORB::Subscription					_battery_sub{ORB_ID(battery_status)};
+	uORB::Subscription					_cmd_sub{ORB_ID(vehicle_command)};
+	uORB::Subscription					_cpuload_sub{ORB_ID(cpuload)};
+	uORB::Subscription					_esc_status_sub{ORB_ID(esc_status)};
+	uORB::Subscription					_geofence_result_sub{ORB_ID(geofence_result)};
+	uORB::Subscription					_iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
+	uORB::Subscription					_land_detector_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription					_parameter_update_sub{ORB_ID(parameter_update)};
+	uORB::Subscription					_power_button_state_sub{ORB_ID(power_button_state)};
+	uORB::Subscription					_safety_sub{ORB_ID(safety)};
+	uORB::Subscription					_sp_man_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription					_subsys_sub{ORB_ID(subsystem_info)};
+	uORB::Subscription					_system_power_sub{ORB_ID(system_power)};
+	uORB::Subscription					_telemetry_status_sub{ORB_ID(telemetry_status)};
 	uORB::Subscription					_vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
+	uORB::Subscription					_vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
 
 	uORB::SubscriptionData<airspeed_s>			_airspeed_sub{ORB_ID(airspeed)};
 	uORB::SubscriptionData<estimator_status_s>		_estimator_status_sub{ORB_ID(estimator_status)};
@@ -269,15 +392,17 @@ private:
 	uORB::SubscriptionData<vehicle_local_position_s>	_local_position_sub{ORB_ID(vehicle_local_position)};
 
 	// Publications
-	uORB::Publication<vehicle_control_mode_s>		_control_mode_pub{ORB_ID(vehicle_control_mode)};
-	uORB::Publication<vehicle_status_s>			_status_pub{ORB_ID(vehicle_status)};
 	uORB::Publication<actuator_armed_s>			_armed_pub{ORB_ID(actuator_armed)};
 	uORB::Publication<commander_state_s>			_commander_state_pub{ORB_ID(commander_state)};
-	uORB::Publication<vehicle_status_flags_s>		_vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)};
 	uORB::Publication<test_motor_s>				_test_motor_pub{ORB_ID(test_motor)};
+	uORB::Publication<vehicle_control_mode_s>		_control_mode_pub{ORB_ID(vehicle_control_mode)};
+	uORB::Publication<vehicle_status_flags_s>		_vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)};
+	uORB::Publication<vehicle_status_s>			_status_pub{ORB_ID(vehicle_status)};
 
 	uORB::PublicationData<home_position_s>			_home_pub{ORB_ID(home_position)};
 
+	uORB::PublicationQueued<vehicle_command_ack_s>		_command_ack_pub{ORB_ID(vehicle_command_ack)};
+
 };
 
 #endif /* COMMANDER_HPP_ */

src/modules/commander/commander_params.c

@@ -303,10 +303,7 @@ PARAM_DEFINE_INT32(COM_ARM_WO_GPS, 1);
  * If parameter set button gets handled like stick arming.
  *
  * @group Commander
- * @min 0
+ * @boolean
- * @max 1
- * @value 0 Arm switch is a switch that stays on when armed
- * @value 1 Arm switch is a button that only triggers arming and disarming
  */
 PARAM_DEFINE_INT32(COM_ARM_SWISBTN, 0);
 

src/modules/commander/commander_tests/state_machine_helper_test.cpp

@@ -55,7 +55,6 @@ public:
 private:
 	bool armingStateTransitionTest();
 	bool mainStateTransitionTest();
-	bool isSafeTest();
 };
 
 bool StateMachineHelperTest::armingStateTransitionTest()
@@ -287,7 +286,7 @@ bool StateMachineHelperTest::armingStateTransitionTest()
 	for (size_t i = 0; i < cArmingTransitionTests; i++) {
 		const ArmingTransitionTest_t *test = &rgArmingTransitionTests[i];
 
-		const bool check_gps = false;
+		PreFlightCheck::arm_requirements_t arm_req{};
 
 		// Setup initial machine state
 		status.arming_state = test->current_state.arming_state;
@@ -305,7 +304,7 @@ bool StateMachineHelperTest::armingStateTransitionTest()
 					     true /* enable pre-arm checks */,
 					     nullptr /* no mavlink_log_pub */,
 					     &status_flags,
-					     (check_gps ? PreFlightCheck::ARM_REQ_GPS_BIT : 0),
+					     arm_req,
 					     2e6 /* 2 seconds after boot, everything should be checked */
 								    );
 
@@ -536,49 +535,10 @@ bool StateMachineHelperTest::mainStateTransitionTest()
 	return true;
 }
 
-bool StateMachineHelperTest::isSafeTest()
-{
-	struct safety_s safety = {};
-	struct actuator_armed_s armed = {};
-
-	armed.armed = false;
-	armed.lockdown = false;
-	safety.safety_switch_available = true;
-	safety.safety_off = false;
-	ut_compare("is safe: not armed", is_safe(safety, armed), true);
-
-	armed.armed = false;
-	armed.lockdown = true;
-	safety.safety_switch_available = true;
-	safety.safety_off = true;
-	ut_compare("is safe: software lockdown", is_safe(safety, armed), true);
-
-	armed.armed = true;
-	armed.lockdown = false;
-	safety.safety_switch_available = true;
-	safety.safety_off = true;
-	ut_compare("not safe: safety off", is_safe(safety, armed), false);
-
-	armed.armed = true;
-	armed.lockdown = false;
-	safety.safety_switch_available = true;
-	safety.safety_off = false;
-	ut_compare("is safe: safety off", is_safe(safety, armed), true);
-
-	armed.armed = true;
-	armed.lockdown = false;
-	safety.safety_switch_available = false;
-	safety.safety_off = false;
-	ut_compare("not safe: no safety switch", is_safe(safety, armed), false);
-
-	return true;
-}
-
 bool StateMachineHelperTest::run_tests()
 {
 	ut_run_test(armingStateTransitionTest);
 	ut_run_test(mainStateTransitionTest);
-	ut_run_test(isSafeTest);
 
 	return (_tests_failed == 0);
 }

src/modules/commander/state_machine_helper.cpp

@@ -46,7 +46,6 @@
 #include <systemlib/mavlink_log.h>
 #include <drivers/drv_hrt.h>
 
-#include "Arming/PreFlightCheck/PreFlightCheck.hpp"
 #include "state_machine_helper.h"
 #include "commander_helper.h"
 
@@ -105,7 +104,8 @@ void reset_offboard_loss_globals(actuator_armed_s *armed, const bool old_failsaf
 
 transition_result_t arming_state_transition(vehicle_status_s *status, const safety_s &safety,
 		const arming_state_t new_arming_state, actuator_armed_s *armed, const bool fRunPreArmChecks,
-		orb_advert_t *mavlink_log_pub, vehicle_status_flags_s *status_flags, const uint8_t arm_requirements,
+		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)
 {
 	// Double check that our static arrays are still valid
@@ -130,14 +130,12 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
 		 */
 		bool preflight_check_ret = true;
 
-		const bool checkGNSS = (arm_requirements & PreFlightCheck::ARM_REQ_GPS_BIT);
-
 		/* 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, checkGNSS, true, true,
+			preflight_check_ret = PreFlightCheck::preflightCheck(mavlink_log_pub, *status, *status_flags,
-					      time_since_boot);
+					      arm_requirements.global_position, true, true, time_since_boot);
 
 			if (preflight_check_ret) {
 				status_flags->condition_system_sensors_initialized = true;
@@ -156,8 +154,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
 			if ((last_preflight_check == 0) || (hrt_elapsed_time(&last_preflight_check) > 1000 * 1000)) {
 
 				status_flags->condition_system_sensors_initialized = PreFlightCheck::preflightCheck(mavlink_log_pub, *status,
-						*status_flags,
+						*status_flags, arm_requirements.global_position, false, false, time_since_boot);
-						checkGNSS, false, false, time_since_boot);
 
 				last_preflight_check = hrt_absolute_time();
 			}
@@ -245,17 +242,6 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
 	return ret;
 }
 
-bool is_safe(const safety_s &safety, const actuator_armed_s &armed)
-{
-	// System is safe if:
-	// 1) Not armed
-	// 2) Armed, but in software lockdown (HIL)
-	// 3) Safety switch is present AND engaged -> actuators locked
-	const bool lockdown = (armed.lockdown || armed.manual_lockdown);
-
-	return !armed.armed || (armed.armed && lockdown) || (safety.safety_switch_available && !safety.safety_off);
-}
-
 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,6 +44,8 @@
 
 #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>
@@ -96,12 +98,11 @@ enum class position_nav_loss_actions_t {
 
 extern const char *const arming_state_names[];
 
-bool is_safe(const safety_s &safety, const actuator_armed_s &armed);
-
 transition_result_t
 arming_state_transition(vehicle_status_s *status, const safety_s &safety, const arming_state_t new_arming_state,
 			actuator_armed_s *armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub,
-			vehicle_status_flags_s *status_flags, const uint8_t arm_requirements, const hrt_abstime &time_since_boot);
+			vehicle_status_flags_s *status_flags, const PreFlightCheck::arm_requirements_t &arm_requirements,
+			const hrt_abstime &time_since_boot);
 
 transition_result_t
 main_state_transition(const vehicle_status_s &status, const main_state_t new_main_state,