PreFlightCheck: mark sensor required if used by an EKF2 instance

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

@@ -73,9 +73,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 
 		if (sys_has_mag == 1) {
 			failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_mag_count, max_optional_mag_count,
-							   mavlink_log_pub, status, magnetometerCheck);
-
-			// TODO: highest priority mag
+							   mavlink_log_pub, status, magnetometerCheck, isMagRequired);
 
 			/* mag consistency checks (need to be performed after the individual checks) */
 			if (!magConsistencyCheck(mavlink_log_pub, status, report_failures)) {
@@ -87,7 +85,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 	/* ---- ACCEL ---- */
 	{
 		failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_accel_count, max_optional_accel_count,
-						   mavlink_log_pub, status, accelerometerCheck);
+						   mavlink_log_pub, status, accelerometerCheck, isAccelRequired);
 
 		// TODO: highest priority (from params)
 	}
@@ -95,7 +93,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 	/* ---- GYRO ---- */
 	{
 		failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_gyro_count, max_optional_gyro_count,
-						   mavlink_log_pub, status, gyroCheck);
+						   mavlink_log_pub, status, gyroCheck, isGyroRequired);
 
 		// TODO: highest priority (from params)
 	}
@@ -107,7 +105,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 
 		if (sys_has_baro == 1) {
 			static_cast<void>(sensorAvailabilityCheck(report_failures, max_mandatory_baro_count, max_optional_baro_count,
-					  mavlink_log_pub, status, baroCheck));
+					  mavlink_log_pub, status, baroCheck, isBaroRequired));
 		}
 	}
 
@@ -126,7 +124,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 
 		if (sys_has_num_dist_sens > 0) {
 			static_cast<void>(sensorAvailabilityCheck(report_failures, sys_has_num_dist_sens, sys_has_num_dist_sens,
-					  mavlink_log_pub, status, distSensCheck));
+					  mavlink_log_pub, status, distSensCheck, isDistSensRequired));
 		}
 
 	}
@@ -240,14 +238,14 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 
 bool PreFlightCheck::sensorAvailabilityCheck(const bool report_failure, const uint8_t max_mandatory_count,
 		const uint8_t max_optional_count, orb_advert_t *mavlink_log_pub,
-		vehicle_status_s &status, sens_check_func_t sens_check)
+		vehicle_status_s &status, sens_check_func_t sens_check, is_sens_req_func_t isSensorRequired)
 {
 	bool pass_check = true;
 	bool report_fail = report_failure;
 
 	/* check all sensors, but fail only for mandatory ones */
 	for (uint8_t i = 0u; i < max_optional_count; i++) {
-		const bool required = (i < max_mandatory_count);
+		const bool required = (i < max_mandatory_count) || isSensorRequired(i);
 		int32_t device_id = -1;
 
 		if (!sens_check(mavlink_log_pub, status, i, !required, device_id, report_fail)) {

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

@@ -49,6 +49,7 @@
 
 typedef bool (*sens_check_func_t)(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
 				  const bool optional, int32_t &device_id, const bool report_fail);
+typedef bool (*is_sens_req_func_t)(uint8_t instance);
 
 class PreFlightCheck
 {
@@ -101,16 +102,21 @@ public:
 private:
 	static bool sensorAvailabilityCheck(const bool report_failure, const uint8_t max_mandatory_count,
 					    const uint8_t max_optional_count, orb_advert_t *mavlink_log_pub,
-					    vehicle_status_s &status, sens_check_func_t sens_check);
+					    vehicle_status_s &status, sens_check_func_t sens_check, is_sens_req_func_t isSensorRequired);
+	static bool isMagRequired(uint8_t instance);
 	static bool magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
 				      const bool optional, int32_t &device_id, const bool report_fail);
 	static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status);
+	static bool isAccelRequired(uint8_t instance);
 	static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
 				       const bool optional, int32_t &device_id, const bool report_fail);
+	static bool isGyroRequired(uint8_t instance);
 	static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
 			      const bool optional, int32_t &device_id, const bool report_fail);
+	static bool isBaroRequired(uint8_t instance);
 	static bool baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
 			      const bool optional, int32_t &device_id, const bool report_fail);
+	static bool isDistSensRequired(uint8_t instance);
 	static bool distSensCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
 				  const bool optional, int32_t &device_id, const bool report_fail);
 	static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status);

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

@@ -40,10 +40,27 @@
 #include <lib/sensor_calibration/Utilities.hpp>
 #include <lib/systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
+#include <uORB/topics/estimator_status.h>
 #include <uORB/topics/sensor_accel.h>
 
 using namespace time_literals;
 
+bool PreFlightCheck::isAccelRequired(const uint8_t instance)
+{
+	uORB::SubscriptionData<sensor_accel_s> accel{ORB_ID(sensor_accel), instance};
+	const uint32_t device_id = static_cast<uint32_t>(accel.get().device_id);
+
+	for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+		uORB::SubscriptionData<estimator_status_s> estimator_status_sub{ORB_ID(estimator_status), i};
+
+		if (device_id > 0 && estimator_status_sub.get().accel_device_id == device_id) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
 bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
 					const bool optional, int32_t &device_id, const bool report_fail)
 {

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

@@ -38,10 +38,27 @@
 #include <px4_defines.h>
 #include <systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
+#include <uORB/topics/estimator_status.h>
 #include <uORB/topics/sensor_baro.h>
 
 using namespace time_literals;
 
+bool PreFlightCheck::isBaroRequired(const uint8_t instance)
+{
+	uORB::SubscriptionData<sensor_baro_s> baro{ORB_ID(sensor_baro), instance};
+	const uint32_t device_id = static_cast<uint32_t>(baro.get().device_id);
+
+	for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+		uORB::SubscriptionData<estimator_status_s> estimator_status_sub{ORB_ID(estimator_status), i};
+
+		if (device_id > 0 && estimator_status_sub.get().baro_device_id == device_id) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
 bool PreFlightCheck::baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
 			       const bool optional, int32_t &device_id, const bool report_fail)
 {

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

@@ -42,6 +42,11 @@
 
 using namespace time_literals;
 
+bool PreFlightCheck::isDistSensRequired(const uint8_t instance)
+{
+	return false;
+}
+
 bool PreFlightCheck::distSensCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
 				   const bool optional, int32_t &device_id, const bool report_fail)
 {

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

@@ -39,10 +39,27 @@
 #include <lib/sensor_calibration/Utilities.hpp>
 #include <lib/systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
+#include <uORB/topics/estimator_status.h>
 #include <uORB/topics/sensor_gyro.h>
 
 using namespace time_literals;
 
+bool PreFlightCheck::isGyroRequired(const uint8_t instance)
+{
+	uORB::SubscriptionData<sensor_gyro_s> gyro{ORB_ID(sensor_gyro), instance};
+	const uint32_t device_id = static_cast<uint32_t>(gyro.get().device_id);
+
+	for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+		uORB::SubscriptionData<estimator_status_s> estimator_status_sub{ORB_ID(estimator_status), i};
+
+		if (device_id > 0 && estimator_status_sub.get().gyro_device_id == device_id) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
 bool PreFlightCheck::gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
 			       const bool optional, int32_t &device_id, const bool report_fail)
 {

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

@@ -44,6 +44,22 @@
 
 using namespace time_literals;
 
+bool PreFlightCheck::isMagRequired(const uint8_t instance)
+{
+	uORB::SubscriptionData<sensor_mag_s> magnetometer{ORB_ID(sensor_mag), instance};
+	const uint32_t device_id = static_cast<uint32_t>(magnetometer.get().device_id);
+
+	for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+		uORB::SubscriptionData<estimator_status_s> estimator_status_sub{ORB_ID(estimator_status), i};
+
+		if (device_id > 0 && estimator_status_sub.get().mag_device_id == device_id) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
 bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
 				       const bool optional, int32_t &device_id, const bool report_fail)
 {