PreFlightCheck: refactor sensors preflight checks

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

@@ -45,14 +45,10 @@
 
 using namespace time_literals;
 
+static constexpr unsigned max_mandatory_mag_count = 1;
 static constexpr unsigned max_mandatory_gyro_count = 1;
-static constexpr unsigned max_optional_gyro_count = 4;
 static constexpr unsigned max_mandatory_accel_count = 1;
-static constexpr unsigned max_optional_accel_count = 4;
-static constexpr unsigned max_mandatory_mag_count = 1;
-static constexpr unsigned max_optional_mag_count = 4;
 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, const vehicle_control_mode_s &control_mode,
@@ -72,8 +68,8 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 		param_get(param_find("SYS_HAS_MAG"), &sys_has_mag);
 
 		if (sys_has_mag == 1) {
-			failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_mag_count, max_optional_mag_count,
+			failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_mag_count,
-							   mavlink_log_pub, status, magnetometerCheck, isMagRequired);
+							   mavlink_log_pub, status, magnetometerCheck);
 
 			/* mag consistency checks (need to be performed after the individual checks) */
 			if (!magConsistencyCheck(mavlink_log_pub, status, report_failures)) {
@@ -84,16 +80,16 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 
 	/* ---- ACCEL ---- */
 	{
-		failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_accel_count, max_optional_accel_count,
+		failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_accel_count,
-						   mavlink_log_pub, status, accelerometerCheck, isAccelRequired);
+						   mavlink_log_pub, status, accelerometerCheck);
 
 		// TODO: highest priority (from params)
 	}
 
 	/* ---- GYRO ---- */
 	{
-		failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_gyro_count, max_optional_gyro_count,
+		failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_gyro_count,
-						   mavlink_log_pub, status, gyroCheck, isGyroRequired);
+						   mavlink_log_pub, status, gyroCheck);
 
 		// TODO: highest priority (from params)
 	}
@@ -104,8 +100,8 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 		param_get(param_find("SYS_HAS_BARO"), &sys_has_baro);
 
 		if (sys_has_baro == 1) {
-			static_cast<void>(sensorAvailabilityCheck(report_failures, max_mandatory_baro_count, max_optional_baro_count,
+			static_cast<void>(sensorAvailabilityCheck(report_failures, max_mandatory_baro_count,
-					  mavlink_log_pub, status, baroCheck, isBaroRequired));
+					  mavlink_log_pub, status, baroCheck));
 		}
 	}
 
@@ -123,8 +119,8 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 		param_get(param_find("SYS_HAS_NUM_DIST"), &sys_has_num_dist_sens);
 
 		if (sys_has_num_dist_sens > 0) {
-			static_cast<void>(sensorAvailabilityCheck(report_failures, sys_has_num_dist_sens, sys_has_num_dist_sens,
+			static_cast<void>(sensorAvailabilityCheck(report_failures, sys_has_num_dist_sens,
-					  mavlink_log_pub, status, distSensCheck, isDistSensRequired));
+					  mavlink_log_pub, status, distSensCheck));
 		}
 
 	}
@@ -236,24 +232,20 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 	return !failed;
 }
 
-bool PreFlightCheck::sensorAvailabilityCheck(const bool report_failure, const uint8_t max_mandatory_count,
+bool PreFlightCheck::sensorAvailabilityCheck(const bool report_failure,
-		const uint8_t max_optional_count, orb_advert_t *mavlink_log_pub,
+		const uint8_t nb_mandatory_instances, orb_advert_t *mavlink_log_pub,
-		vehicle_status_s &status, sens_check_func_t sens_check, is_sens_req_func_t isSensorRequired)
+		vehicle_status_s &status, sens_check_func_t sens_check)
 {
 	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++) {
+	for (uint8_t i = 0u; i < ORB_MULTI_MAX_INSTANCES; i++) {
-		const bool required = (i < max_mandatory_count) || isSensorRequired(i);
+		const bool is_mandatory = i < nb_mandatory_instances;
 
-		if (!sens_check(mavlink_log_pub, status, i, !required, report_fail)) {
+		if (!sens_check(mavlink_log_pub, status, i, is_mandatory, report_fail)) {
-			if (required) {
 			pass_check = false;
 		}
-
-			report_fail = false; // only report the first failure
-		}
 	}
 
 	return pass_check;

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

@@ -48,8 +48,7 @@
 #include <drivers/drv_hrt.h>
 
 typedef bool (*sens_check_func_t)(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
-				  const bool optional, const bool report_fail);
+				  const bool is_mandatory, bool &report_fail);
-typedef bool (*is_sens_req_func_t)(uint8_t instance);
 
 class PreFlightCheck
 {
@@ -100,25 +99,24 @@ public:
 				bool report_fail = true);
 
 private:
-	static bool sensorAvailabilityCheck(const bool report_failure, const uint8_t max_mandatory_count,
+	static bool sensorAvailabilityCheck(const bool report_failure,
-					    const uint8_t max_optional_count, orb_advert_t *mavlink_log_pub,
+					    const uint8_t nb_mandatory_instances, orb_advert_t *mavlink_log_pub,
-					    vehicle_status_s &status, sens_check_func_t sens_check, is_sens_req_func_t isSensorRequired);
+					    vehicle_status_s &status, sens_check_func_t sens_check);
 	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, const bool report_fail);
+				      const bool is_mandatory, 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, const bool report_fail);
+				       const bool is_mandatory, 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, const bool report_fail);
+			      const bool is_mandatory, 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, const bool report_fail);
+			      const bool is_mandatory, 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, const bool report_fail);
+				  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);

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

@@ -50,70 +50,71 @@ 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);
 
+	bool is_used_by_nav = false;
+
 	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;
+			is_used_by_nav = true;
+			break;
 		}
 	}
 
-	return false;
+	return is_used_by_nav;
 }
 
 bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
-					const bool optional, const bool report_fail)
+					const bool is_mandatory, bool &report_fail)
 {
 	const bool exists = (orb_exists(ORB_ID(sensor_accel), instance) == PX4_OK);
-	bool calibration_valid = false;
+	const bool is_required = is_mandatory || isAccelRequired(instance);
-	bool valid = true;
 
-	if (exists) {
+	bool is_valid = false;
+	bool is_calibration_valid = false;
+	bool is_value_valid = false;
 
+	if (exists) {
 		uORB::SubscriptionData<sensor_accel_s> accel{ORB_ID(sensor_accel), instance};
 
-		valid = (accel.get().device_id != 0) && (accel.get().timestamp != 0);
+		is_valid = (accel.get().device_id != 0) && (accel.get().timestamp != 0);
-
-		if (!valid) {
-			if (report_fail) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Accel %u", instance);
-			}
-		}
 
 		if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
-			calibration_valid = true;
+			is_calibration_valid = true;
 
 		} else {
-			calibration_valid = (calibration::FindCurrentCalibrationIndex("ACC", accel.get().device_id) >= 0);
+			is_calibration_valid = (calibration::FindCurrentCalibrationIndex("ACC", accel.get().device_id) >= 0);
 		}
 
-		if (!calibration_valid) {
-			if (report_fail) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel %u uncalibrated", instance);
-			}
-
-		} else {
 		const float accel_magnitude = sqrtf(accel.get().x * accel.get().x
 						    + accel.get().y * accel.get().y
 						    + accel.get().z * accel.get().z);
 
-			if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
+		if (accel_magnitude > 4.0f && accel_magnitude < 15.0f /* m/s^2 */) {
-				if (report_fail) {
+			is_value_valid = true;
-					mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Range, hold still on arming");
-				}
-
-				// this is fatal
-				valid = false;
 		}
 	}
 
-	} else {
+	if (report_fail && is_required) {
-		if (!optional && report_fail) {
+		if (!exists) {
 			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Sensor %u missing", instance);
+			report_fail = false;
+
+		} else if (!is_valid) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Accel %u", instance);
+			report_fail = false;
+
+		} else if (!is_calibration_valid) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel %u uncalibrated", instance);
+			report_fail = false;
+
+		} else if (!is_value_valid) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Range, hold still on arming");
+			report_fail = false;
 		}
 	}
 
-	const bool success = calibration_valid && valid;
+	const bool is_sensor_ok = is_valid && is_calibration_valid && is_value_valid;
 
-	return success;
+	return is_sensor_ok || !is_required;
 }

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

@@ -48,43 +48,48 @@ 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);
 
+	bool is_used_by_nav = false;
+
 	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;
+			is_used_by_nav = true;
+			break;
 		}
 	}
 
-	return false;
+	return is_used_by_nav;
 }
 
 bool PreFlightCheck::baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
-			       const bool optional, const bool report_fail)
+			       const bool is_mandatory, bool &report_fail)
 {
 	const bool exists = (orb_exists(ORB_ID(sensor_baro), instance) == PX4_OK);
+	const bool is_required = is_mandatory || isBaroRequired(instance);
+
 	bool valid = false;
 
 	if (exists) {
 		uORB::SubscriptionData<sensor_baro_s> baro{ORB_ID(sensor_baro), instance};
 
 		valid = (baro.get().device_id != 0) && (baro.get().timestamp != 0);
-
-		if (!valid) {
-			if (report_fail) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Baro %u", instance);
 	}
+
+	if (instance == 0) {
+		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, exists, is_required, valid, status);
 	}
 
-	} else {
+	if (report_fail && is_required) {
-		if (!optional && report_fail) {
+		if (!exists) {
 			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Baro Sensor %u missing", instance);
-		}
+			report_fail = false;
-	}
 
-	if (instance == 0) {
+		} else if (!valid) {
-		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, exists, !optional, valid, status);
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Baro %u", instance);
+			report_fail = false;
+		}
 	}
 
-	return valid;
+	return valid || !is_required;
 }

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

@@ -42,35 +42,30 @@
 
 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, const bool report_fail)
+				   const bool is_mandatory, bool &report_fail)
 {
 	const bool exists = (orb_exists(ORB_ID(distance_sensor), instance) == PX4_OK);
-	bool check_valid = false;
+	bool valid = false;
 
 	if (exists) {
 		uORB::SubscriptionData<distance_sensor_s> dist_sens_sub{ORB_ID(distance_sensor), instance};
 		dist_sens_sub.update();
 		const distance_sensor_s &dist_sens_data = dist_sens_sub.get();
 
-		check_valid = (hrt_elapsed_time(&dist_sens_data.timestamp) < 1_s);
+		valid = (hrt_elapsed_time(&dist_sens_data.timestamp) < 1_s);
-
-		if (!check_valid) {
-			if (report_fail) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from distance sensor %u", instance);
-			}
 	}
 
-	} else {
+	if (report_fail && is_mandatory) {
-		if (!optional && report_fail) {
+		if (!exists) {
 			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: distance sensor %u missing", instance);
+			report_fail = false;
+
+		} else if (!valid) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from distance sensor %u", instance);
+			report_fail = false;
 		}
 	}
 
-	return check_valid;
+	return valid || !is_mandatory;
 }

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

@@ -49,54 +49,58 @@ 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);
 
+	bool is_used_by_nav = false;
+
 	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;
+			is_used_by_nav = true;
+			break;
 		}
 	}
 
-	return false;
+	return is_used_by_nav;
 }
 
 bool PreFlightCheck::gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
-			       const bool optional, const bool report_fail)
+			       const bool is_mandatory, bool &report_fail)
 {
 	const bool exists = (orb_exists(ORB_ID(sensor_gyro), instance) == PX4_OK);
-	bool calibration_valid = false;
+	const bool is_required = is_mandatory || isGyroRequired(instance);
-	bool valid = false;
 
-	if (exists) {
+	bool is_valid = false;
+	bool is_calibration_valid = false;
 
+	if (exists) {
 		uORB::SubscriptionData<sensor_gyro_s> gyro{ORB_ID(sensor_gyro), instance};
 
-		valid = (gyro.get().device_id != 0) && (gyro.get().timestamp != 0);
+		is_valid = (gyro.get().device_id != 0) && (gyro.get().timestamp != 0);
-
-		if (!valid) {
-			if (report_fail) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Gyro %u", instance);
-			}
-		}
 
 		if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
-			calibration_valid = true;
+			is_calibration_valid = true;
 
 		} else {
-			calibration_valid = (calibration::FindCurrentCalibrationIndex("GYRO", gyro.get().device_id) >= 0);
+			is_calibration_valid = (calibration::FindCurrentCalibrationIndex("GYRO", gyro.get().device_id) >= 0);
-		}
-
-		if (!calibration_valid) {
-			if (report_fail) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro %u uncalibrated", instance);
 		}
 	}
 
-	} else {
+	if (report_fail && is_required) {
-		if (!optional && report_fail) {
+		if (!exists) {
 			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro Sensor %u missing", instance);
+			report_fail = false;
+
+		} else if (!is_valid) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Gyro %u", instance);
+			report_fail = false;
+
+		} else if (!is_calibration_valid) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro %u uncalibrated", instance);
+			report_fail = false;
 		}
 	}
 
-	return calibration_valid && valid;
+	const bool is_sensor_ok = is_calibration_valid && is_valid;
+
+	return is_sensor_ok || !is_required;
 }

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

@@ -49,48 +49,40 @@ 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);
 
+	bool is_used_by_nav = false;
+
 	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;
+			is_used_by_nav = true;;
+			break;
 		}
 	}
 
-	return false;
+	return is_used_by_nav;
 }
 
 bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
-				       const bool optional, const bool report_fail)
+				       const bool is_mandatory, bool &report_fail)
 {
 	const bool exists = (orb_exists(ORB_ID(sensor_mag), instance) == PX4_OK);
-	bool calibration_valid = false;
+	const bool is_required = is_mandatory || isMagRequired(instance);
-	bool valid = false;
+
+	bool is_valid = false;
+	bool is_calibration_valid = false;
 	bool is_mag_fault = false;
 
 	if (exists) {
-
 		uORB::SubscriptionData<sensor_mag_s> magnetometer{ORB_ID(sensor_mag), instance};
 
-		valid = (magnetometer.get().device_id != 0) && (magnetometer.get().timestamp != 0);
+		is_valid = (magnetometer.get().device_id != 0) && (magnetometer.get().timestamp != 0);
-
-		if (!valid) {
-			if (report_fail) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Compass %u", instance);
-			}
-		}
 
 		if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
-			calibration_valid = true;
+			is_calibration_valid = true;
 
 		} else {
-			calibration_valid = (calibration::FindCurrentCalibrationIndex("MAG", magnetometer.get().device_id) >= 0);
+			is_calibration_valid = (calibration::FindCurrentCalibrationIndex("MAG", magnetometer.get().device_id) >= 0);
-		}
-
-		if (!calibration_valid) {
-			if (report_fail) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass %u uncalibrated", instance);
-			}
 		}
 
 		for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
@@ -103,25 +95,35 @@ bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_st
 				}
 			}
 		}
+	}
 
-		if (is_mag_fault && report_fail) {
+	const bool is_sensor_ok = is_valid && is_calibration_valid && !is_mag_fault;
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass #%u fault", instance);
+
+	if (instance == 0) {
+		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, exists, is_required, is_sensor_ok, status);
+
+	} else if (instance == 1) {
+		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, exists, is_required, is_sensor_ok, status);
 	}
 
-	} else {
+	if (report_fail && is_required) {
-		if (!optional && report_fail) {
+		if (!exists) {
 			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass Sensor %u missing", instance);
-		}
+			report_fail = false;
-	}
 
-	const bool success = calibration_valid && valid && !is_mag_fault;
+		} else if (!is_valid) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Compass %u", instance);
+			report_fail = false;
 
-	if (instance == 0) {
+		} else if (!is_calibration_valid) {
-		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, exists, !optional, success, status);
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass %u uncalibrated", instance);
+			report_fail = false;
 
-	} else if (instance == 1) {
+		} else if (is_mag_fault) {
-		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, exists, !optional, success, status);
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass #%u fault", instance);
+			report_fail = false;
+		}
 	}
 
-	return success;
+	return is_sensor_ok || !is_required;
 }