From 2f06f037284fabbfafa1ed14bd3fb72b8ea118f5 Mon Sep 17 00:00:00 2001
From: Marco Hauswirth <58551738+haumarco@users.noreply.github.com>
Date: Tue, 14 Oct 2025 06:51:25 +0200
Subject: [PATCH] calibration: mag: only allow mag calibration when at least
 one mag is available and enabled (i.e. not prio=0) (#25714)

---
 src/modules/commander/mag_calibration.cpp | 35 +++++++++++++++++++++--
 1 file changed, 32 insertions(+), 3 deletions(-)

diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 7aeadeab55..fb6dfa0282 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -520,12 +520,36 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 
 	const unsigned int calibration_points_maxcount = worker_data.calibration_sides * worker_data.calibration_points_perside;
 
+	uORB::SubscriptionMultiArray<sensor_mag_s, MAX_MAGS> mag_sub{ORB_ID::sensor_mag};
+	int mag_available_enabled_count = 0;
+
 	for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
 
-		uORB::SubscriptionData<sensor_mag_s> mag_sub{ORB_ID(sensor_mag), cur_mag};
+		if (!mag_sub[cur_mag].advertised()) {
+			continue;
+		}
 
-		if (mag_sub.advertised() && (mag_sub.get().device_id != 0) && (mag_sub.get().timestamp > 0)) {
-			worker_data.calibration[cur_mag].set_device_id(mag_sub.get().device_id);
+		sensor_mag_s mag_data;
+
+		if (!mag_sub[cur_mag].copy(&mag_data) || mag_data.device_id == 0) {
+			continue;
+		}
+
+		int calibration_index = calibration::FindCurrentCalibrationIndex("MAG", mag_data.device_id);
+
+		if (calibration_index >= 0) {
+			int priority = calibration::GetCalibrationParamInt32("MAG", "PRIO", calibration_index);
+
+			if (priority != 0) {
+				++mag_available_enabled_count;
+			}
+
+		} else {
+			++mag_available_enabled_count;
+		}
+
+		if ((mag_data.device_id != 0) && (mag_data.timestamp > 0)) {
+			worker_data.calibration[cur_mag].set_device_id(mag_data.device_id);
 		}
 
 		// reset calibration index to match uORB numbering
@@ -547,6 +571,11 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 		}
 	}
 
+	if (mag_available_enabled_count <= 0) {
+		calibration_log_critical(mavlink_log_pub, "Failed: No magnetometer available or enabled");
+		return calibrate_return_error;
+	}
+
 	if (result == calibrate_return_ok) {
 		result = calibrate_from_orientation(mavlink_log_pub,                    // uORB handle to write output
 						    worker_data.side_data_collected,    // Sides to calibrate