commander: respect control mode for prearm requirements

- preflight tolerate ekf2 warnings if not in an attitude/velocity/position mode (eg manual or acro)

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

@@ -55,8 +55,8 @@ static constexpr unsigned max_mandatory_baro_count = 1;
 static constexpr unsigned max_optional_baro_count = 4;
 
 bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
-				    vehicle_status_flags_s &status_flags, bool report_failures, const bool prearm,
-				    const hrt_abstime &time_since_boot)
+				    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)
 {
 	report_failures = (report_failures && status_flags.condition_system_hotplug_timeout
 			   && !status_flags.condition_calibration_enabled);
@@ -256,7 +256,13 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, true, ekf_healthy, status);
 		}
 
-		failed |= !ekf_healthy;
+
+		if (control_mode.flag_control_attitude_enabled
+		    || control_mode.flag_control_velocity_enabled
+		    || control_mode.flag_control_position_enabled) {
+			// healthy estimator only required for dependent control modes
+			failed |= !ekf_healthy;
+		}
 	}
 
 	/* ---- Failure Detector ---- */

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

@@ -41,8 +41,8 @@
 #pragma once
 
 #include <uORB/topics/safety.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_status_flags.h>
-
 #include <uORB/topics/vehicle_status.h>
 #include <drivers/drv_hrt.h>
 
@@ -78,8 +78,8 @@ 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, bool reportFailures, const bool prearm,
-				   const hrt_abstime &time_since_boot);
+				   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;
@@ -90,6 +90,7 @@ public:
 	};
 
 	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);
 

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

@@ -39,10 +39,46 @@
 #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.condition_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.condition_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.condition_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.condition_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;
+	}
+
 	// 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"); }

src/modules/commander/Commander.cpp

@@ -274,11 +274,16 @@ int Commander::custom_command(int argc, char *argv[])
 		vehicle_status_flags_s vehicle_status_flags{};
 		vehicle_status_flags_sub.copy(&vehicle_status_flags);
 
-		bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, vehicle_status, vehicle_status_flags, true, true,
-					   30_s);
+		uORB::Subscription vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+		vehicle_control_mode_s vehicle_control_mode{};
+		vehicle_control_mode_sub.copy(&vehicle_control_mode);
+
+		bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, vehicle_status, vehicle_status_flags,
+					   vehicle_control_mode,
+					   true, true, 30_s);
 		PX4_INFO("Preflight check: %s", preflight_check_res ? "OK" : "FAILED");
 
-		bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, vehicle_status_flags, safety_s{},
+		bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, vehicle_status_flags, vehicle_control_mode, safety_s{},
 					PreFlightCheck::arm_requirements_t{}, vehicle_status);
 		PX4_INFO("Prearm check: %s", prearm_check_res ? "OK" : "FAILED");
 
@@ -468,7 +473,8 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[])
 
 bool Commander::shutdown_if_allowed()
 {
-	return TRANSITION_DENIED != arming_state_transition(_status, _safety, vehicle_status_s::ARMING_STATE_SHUTDOWN,
+	return TRANSITION_DENIED != arming_state_transition(_status, _vehicle_control_mode, _safety,
+			vehicle_status_s::ARMING_STATE_SHUTDOWN,
 			_armed, false /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags, _arm_requirements,
 			hrt_elapsed_time(&_boot_timestamp), arm_disarm_reason_t::shutdown);
 }
@@ -716,15 +722,10 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
 		}
 	}
 
-	transition_result_t arming_res = arming_state_transition(_status,
-					 _safety,
-					 vehicle_status_s::ARMING_STATE_ARMED,
-					 _armed,
-					 run_preflight_checks,
-					 &_mavlink_log_pub,
-					 _status_flags,
-					 _arm_requirements,
-					 hrt_elapsed_time(&_boot_timestamp), calling_reason);
+	transition_result_t arming_res = arming_state_transition(_status, _vehicle_control_mode, _safety,
+					 vehicle_status_s::ARMING_STATE_ARMED, _armed, run_preflight_checks,
+					 &_mavlink_log_pub, _status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp),
+					 calling_reason);
 
 	if (arming_res == TRANSITION_CHANGED) {
 		mavlink_log_info(&_mavlink_log_pub, "Armed by %s\t", arm_disarm_reason_str(calling_reason));
@@ -763,14 +764,9 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
 		}
 	}
 
-	transition_result_t arming_res = arming_state_transition(_status,
-					 _safety,
-					 vehicle_status_s::ARMING_STATE_STANDBY,
-					 _armed,
-					 false,
-					 &_mavlink_log_pub,
-					 _status_flags,
-					 _arm_requirements,
+	transition_result_t arming_res = arming_state_transition(_status, _vehicle_control_mode, _safety,
+					 vehicle_status_s::ARMING_STATE_STANDBY, _armed, false,
+					 &_mavlink_log_pub, _status_flags, _arm_requirements,
 					 hrt_elapsed_time(&_boot_timestamp), calling_reason);
 
 	if (arming_res == TRANSITION_CHANGED) {
@@ -1334,7 +1330,8 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			} else {
 
 				/* try to go to INIT/PREFLIGHT arming state */
-				if (TRANSITION_DENIED == arming_state_transition(_status, safety_s{}, vehicle_status_s::ARMING_STATE_INIT, _armed,
+				if (TRANSITION_DENIED == arming_state_transition(_status, _vehicle_control_mode, safety_s{},
+						vehicle_status_s::ARMING_STATE_INIT, _armed,
 						false /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags,
 						PreFlightCheck::arm_requirements_t{}, // arming requirements not relevant for switching to ARMING_STATE_INIT
 						30_s, // time since boot not relevant for switching to ARMING_STATE_INIT
@@ -1988,9 +1985,8 @@ Commander::run()
 	arm_auth_init(&_mavlink_log_pub, &_status.system_id);
 
 	// run preflight immediately to find all relevant parameters, but don't report
-	PreFlightCheck::preflightCheck(&_mavlink_log_pub, _status, _status_flags, false,
-				       true,
-				       hrt_elapsed_time(&_boot_timestamp));
+	PreFlightCheck::preflightCheck(&_mavlink_log_pub, _status, _status_flags, _vehicle_control_mode,
+				       false, true, hrt_elapsed_time(&_boot_timestamp));
 
 	while (!should_exit()) {
 
@@ -2347,7 +2343,7 @@ Commander::run()
 		/* If in INIT state, try to proceed to STANDBY state */
 		if (!_status_flags.condition_calibration_enabled && _status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
 
-			arming_state_transition(_status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, _armed,
+			arming_state_transition(_status, _vehicle_control_mode, _safety, vehicle_status_s::ARMING_STATE_STANDBY, _armed,
 						true /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags,
 						_arm_requirements, hrt_elapsed_time(&_boot_timestamp),
 						arm_disarm_reason_t::transition_to_standby);
@@ -2942,13 +2938,14 @@ Commander::run()
 
 			// Evaluate current prearm status
 			if (!_armed.armed && !_status_flags.condition_calibration_enabled) {
-				bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, _status, _status_flags, false, true,
-							   hrt_elapsed_time(&_boot_timestamp));
+				bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, _status, _status_flags, _vehicle_control_mode,
+							   false, true, hrt_elapsed_time(&_boot_timestamp));
 
 				// skip arm authorization check until actual arming attempt
 				PreFlightCheck::arm_requirements_t arm_req = _arm_requirements;
 				arm_req.arm_authorization = false;
-				bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, _status_flags, _safety, arm_req, _status, false);
+				bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, _status_flags, _vehicle_control_mode, _safety, arm_req,
+							_status, false);
 
 				set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK, true, true, (preflight_check_res
 						 && prearm_check_res), _status);
@@ -3584,8 +3581,8 @@ void Commander::data_link_check()
 
 					if (!_armed.armed && !_status_flags.condition_calibration_enabled) {
 						// make sure to report preflight check failures to a connecting GCS
-						PreFlightCheck::preflightCheck(&_mavlink_log_pub, _status, _status_flags, true, false,
-									       hrt_elapsed_time(&_boot_timestamp));
+						PreFlightCheck::preflightCheck(&_mavlink_log_pub, _status, _status_flags, _vehicle_control_mode,
+									       true, false, hrt_elapsed_time(&_boot_timestamp));
 					}
 				}
 

src/modules/commander/commander_tests/state_machine_helper_test.cpp

@@ -296,11 +296,14 @@ bool StateMachineHelperTest::armingStateTransitionTest()
 		status_flags.circuit_breaker_engaged_power_check = true;
 		safety.safety_switch_available = test->safety_switch_available;
 		safety.safety_off = test->safety_off;
+
 		armed.armed = test->current_state.armed;
 		armed.ready_to_arm = test->current_state.ready_to_arm;
 
+		vehicle_control_mode_s control_mode{};
+
 		// Attempt transition
-		transition_result_t result = arming_state_transition(status, safety, test->requested_state, armed,
+		transition_result_t result = arming_state_transition(status, control_mode, safety, test->requested_state, armed,
 					     true /* enable pre-arm checks */,
 					     nullptr /* no mavlink_log_pub */,
 					     status_flags,

src/modules/commander/state_machine_helper.cpp

@@ -154,7 +154,8 @@ void reset_offboard_loss_globals(actuator_armed_s &armed, const bool old_failsaf
 				 const offboard_loss_actions_t offboard_loss_act,
 				 const offboard_loss_rc_actions_t offboard_loss_rc_act);
 
-transition_result_t arming_state_transition(vehicle_status_s &status, const safety_s &safety,
+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,
@@ -186,8 +187,8 @@ transition_result_t arming_state_transition(vehicle_status_s &status, const safe
 		if (fRunPreArmChecks && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
 		    && !hil_enabled) {
 
-			preflight_check_ret = PreFlightCheck::preflightCheck(mavlink_log_pub, status, status_flags, true, true,
-					      time_since_boot);
+			preflight_check_ret = PreFlightCheck::preflightCheck(mavlink_log_pub, status, status_flags, control_mode,
+					      true, true, time_since_boot);
 
 			if (preflight_check_ret) {
 				status_flags.condition_system_sensors_initialized = true;
@@ -206,7 +207,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, false, status.arming_state != vehicle_status_s::ARMING_STATE_ARMED,
+						status_flags, control_mode, false, status.arming_state != vehicle_status_s::ARMING_STATE_ARMED,
 						time_since_boot);
 
 				last_preflight_check = hrt_absolute_time();
@@ -228,7 +229,8 @@ transition_result_t arming_state_transition(vehicle_status_s &status, const safe
 
 					if (fRunPreArmChecks && preflight_check_ret) {
 						// only bother running prearm if preflight was successful
-						prearm_check_ret = PreFlightCheck::preArmCheck(mavlink_log_pub, status_flags, safety, arm_requirements, status);
+						prearm_check_ret = PreFlightCheck::preArmCheck(mavlink_log_pub, status_flags, control_mode, safety, arm_requirements,
+								   status);
 					}
 
 					if (!preflight_check_ret || !prearm_check_ret) {

src/modules/commander/state_machine_helper.h

@@ -108,7 +108,8 @@ enum RCLossExceptionBits {
 };
 
 transition_result_t
-arming_state_transition(vehicle_status_s &status, const safety_s &safety, const arming_state_t new_arming_state,
+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);