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"); }