sensor_calibration: refactor and centralize calibration slot logic

- centralize logic for selecting a preferred calibration slot
   - automatically use existing calibration slot if it exists, otherwise
find first available slot, with a preference for a requested index
 - existing commander calibration methods rewrite all calibration slots
to match current sensor ordering

src/lib/sensor_calibration/Accelerometer.cpp

@@ -144,15 +144,31 @@ void Accelerometer::set_rotation(Rotation rotation)
 	_rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(rotation);
 }
 
+bool Accelerometer::set_calibration_index(int calibration_index)
+{
+	if ((calibration_index >= 0) && (calibration_index < MAX_SENSOR_COUNT)) {
+		_calibration_index = calibration_index;
+		return true;
+	}
+
+	return false;
+}
+
 void Accelerometer::ParametersUpdate()
 {
 	if (_device_id == 0) {
 		return;
 	}
 
-	_calibration_index = FindCalibrationIndex(SensorString(), _device_id);
+	_calibration_index = FindCurrentCalibrationIndex(SensorString(), _device_id);
 
-	ParametersLoad();
+	if (_calibration_index == -1) {
+		// no saved calibration available
+		Reset();
+
+	} else {
+		ParametersLoad();
+	}
 }
 
 bool Accelerometer::ParametersLoad()
@@ -236,9 +252,25 @@ void Accelerometer::Reset()
 	_calibration_count = 0;
 }
 
-bool Accelerometer::ParametersSave()
+bool Accelerometer::ParametersSave(int desired_calibration_index, bool force)
 {
-	if (_calibration_index >= 0) {
+	if (force && desired_calibration_index >= 0 && desired_calibration_index < MAX_SENSOR_COUNT) {
+		_calibration_index = desired_calibration_index;
+
+	} else if (!force || (_calibration_index < 0)
+		   || (desired_calibration_index != -1 && desired_calibration_index != _calibration_index)) {
+
+		// ensure we have a valid calibration slot (matching existing or first available slot)
+		int8_t calibration_index_prev = _calibration_index;
+		_calibration_index = FindAvailableCalibrationIndex(SensorString(), _device_id, desired_calibration_index);
+
+		if (calibration_index_prev >= 0 && (calibration_index_prev != _calibration_index)) {
+			PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id,
+				 calibration_index_prev, _calibration_index);
+		}
+	}
+
+	if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
 		// save calibration
 		bool success = true;
 		success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id);

src/lib/sensor_calibration/Accelerometer.hpp

@@ -59,7 +59,7 @@ public:
 
 	void PrintStatus();
 
-	void set_calibration_index(uint8_t calibration_index) { _calibration_index = calibration_index; }
+	bool set_calibration_index(int calibration_index);
 	void set_device_id(uint32_t device_id);
 	bool set_offset(const matrix::Vector3f &offset);
 	bool set_scale(const matrix::Vector3f &scale);
@@ -92,7 +92,7 @@ public:
 	}
 
 	bool ParametersLoad();
-	bool ParametersSave();
+	bool ParametersSave(int desired_calibration_index = -1, bool force = false);
 	void ParametersUpdate();
 
 	void Reset();

src/lib/sensor_calibration/Gyroscope.cpp

@@ -129,15 +129,31 @@ void Gyroscope::set_rotation(Rotation rotation)
 	_rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(rotation);
 }
 
+bool Gyroscope::set_calibration_index(int calibration_index)
+{
+	if ((calibration_index >= 0) && (calibration_index < MAX_SENSOR_COUNT)) {
+		_calibration_index = calibration_index;
+		return true;
+	}
+
+	return false;
+}
+
 void Gyroscope::ParametersUpdate()
 {
 	if (_device_id == 0) {
 		return;
 	}
 
-	_calibration_index = FindCalibrationIndex(SensorString(), _device_id);
+	_calibration_index = FindCurrentCalibrationIndex(SensorString(), _device_id);
 
-	ParametersLoad();
+	if (_calibration_index == -1) {
+		// no saved calibration available
+		Reset();
+
+	} else {
+		ParametersLoad();
+	}
 }
 
 bool Gyroscope::ParametersLoad()
@@ -217,9 +233,25 @@ void Gyroscope::Reset()
 	_calibration_count = 0;
 }
 
-bool Gyroscope::ParametersSave()
+bool Gyroscope::ParametersSave(int desired_calibration_index, bool force)
 {
-	if (_calibration_index >= 0) {
+	if (force && desired_calibration_index >= 0 && desired_calibration_index < MAX_SENSOR_COUNT) {
+		_calibration_index = desired_calibration_index;
+
+	} else if (!force || (_calibration_index < 0)
+		   || (desired_calibration_index != -1 && desired_calibration_index != _calibration_index)) {
+
+		// ensure we have a valid calibration slot (matching existing or first available slot)
+		int8_t calibration_index_prev = _calibration_index;
+		_calibration_index = FindAvailableCalibrationIndex(SensorString(), _device_id, desired_calibration_index);
+
+		if (calibration_index_prev >= 0 && (calibration_index_prev != _calibration_index)) {
+			PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id,
+				 calibration_index_prev, _calibration_index);
+		}
+	}
+
+	if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
 		// save calibration
 		bool success = true;
 		success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id);

src/lib/sensor_calibration/Gyroscope.hpp

@@ -59,7 +59,7 @@ public:
 
 	void PrintStatus();
 
-	void set_calibration_index(uint8_t calibration_index) { _calibration_index = calibration_index; }
+	bool set_calibration_index(int calibration_index);
 	void set_device_id(uint32_t device_id);
 	bool set_offset(const matrix::Vector3f &offset);
 	void set_rotation(Rotation rotation);
@@ -96,7 +96,7 @@ public:
 	}
 
 	bool ParametersLoad();
-	bool ParametersSave();
+	bool ParametersSave(int desired_calibration_index = -1, bool force = false);
 	void ParametersUpdate();
 
 	void Reset();

src/lib/sensor_calibration/Magnetometer.cpp

@@ -129,15 +129,31 @@ void Magnetometer::set_rotation(Rotation rotation)
 	_rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(rotation);
 }
 
+bool Magnetometer::set_calibration_index(int calibration_index)
+{
+	if ((calibration_index >= 0) && (calibration_index < MAX_SENSOR_COUNT)) {
+		_calibration_index = calibration_index;
+		return true;
+	}
+
+	return false;
+}
+
 void Magnetometer::ParametersUpdate()
 {
 	if (_device_id == 0) {
 		return;
 	}
 
-	_calibration_index = FindCalibrationIndex(SensorString(), _device_id);
+	_calibration_index = FindCurrentCalibrationIndex(SensorString(), _device_id);
 
-	ParametersLoad();
+	if (_calibration_index == -1) {
+		// no saved calibration available
+		Reset();
+
+	} else {
+		ParametersLoad();
+	}
 }
 
 bool Magnetometer::ParametersLoad()
@@ -229,9 +245,25 @@ void Magnetometer::Reset()
 	_calibration_count = 0;
 }
 
-bool Magnetometer::ParametersSave()
+bool Magnetometer::ParametersSave(int desired_calibration_index, bool force)
 {
-	if (_calibration_index >= 0) {
+	if (force && desired_calibration_index >= 0 && desired_calibration_index < MAX_SENSOR_COUNT) {
+		_calibration_index = desired_calibration_index;
+
+	} else if (!force || (_calibration_index < 0)
+		   || (desired_calibration_index != -1 && desired_calibration_index != _calibration_index)) {
+
+		// ensure we have a valid calibration slot (matching existing or first available slot)
+		int8_t calibration_index_prev = _calibration_index;
+		_calibration_index = FindAvailableCalibrationIndex(SensorString(), _device_id, desired_calibration_index);
+
+		if (calibration_index_prev >= 0 && (calibration_index_prev != _calibration_index)) {
+			PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id,
+				 calibration_index_prev, _calibration_index);
+		}
+	}
+
+	if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
 		// save calibration
 		bool success = true;
 		success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id);

src/lib/sensor_calibration/Magnetometer.hpp

@@ -60,7 +60,7 @@ public:
 
 	void PrintStatus();
 
-	void set_calibration_index(uint8_t calibration_index) { _calibration_index = calibration_index; }
+	bool set_calibration_index(int calibration_index);
 	void set_device_id(uint32_t device_id);
 	bool set_offset(const matrix::Vector3f &offset);
 	bool set_scale(const matrix::Vector3f &scale);
@@ -94,7 +94,7 @@ public:
 	}
 
 	bool ParametersLoad();
-	bool ParametersSave();
+	bool ParametersSave(int desired_calibration_index = -1, bool force = false);
 	void ParametersUpdate();
 
 	void Reset();

src/lib/sensor_calibration/Utilities.cpp

@@ -54,13 +54,15 @@ using matrix::Vector3f;
 namespace calibration
 {
 
-int8_t FindCalibrationIndex(const char *sensor_type, uint32_t device_id)
+static constexpr int MAX_SENSOR_COUNT = 4; // TODO: per sensor?
+
+int8_t FindCurrentCalibrationIndex(const char *sensor_type, uint32_t device_id)
 {
 	if (device_id == 0) {
 		return -1;
 	}
 
-	for (unsigned i = 0; i < 4; ++i) {
+	for (unsigned i = 0; i < MAX_SENSOR_COUNT; ++i) {
 		char str[20] {};
 		sprintf(str, "CAL_%s%u_ID", sensor_type, i);
 
@@ -85,6 +87,53 @@ int8_t FindCalibrationIndex(const char *sensor_type, uint32_t device_id)
 	return -1;
 }
 
+int8_t FindAvailableCalibrationIndex(const char *sensor_type, uint32_t device_id, int8_t preferred_index)
+{
+	// if this device is already using a calibration slot then keep it
+	int calibration_index = FindCurrentCalibrationIndex(sensor_type, device_id);
+
+	if (calibration_index >= 0) {
+		return calibration_index;
+	}
+
+
+	// device isn't currently using a calibration slot, select user preference (preferred_index)
+	//  if available, otherwise use the first available slot
+	uint32_t cal_device_ids[MAX_SENSOR_COUNT] {};
+
+	for (unsigned i = 0; i < MAX_SENSOR_COUNT; ++i) {
+		char str[20] {};
+		sprintf(str, "CAL_%s%u_ID", sensor_type, i);
+		int32_t device_id_val = 0;
+
+		if (param_get(param_find_no_notification(str), &device_id_val) == PX4_OK) {
+			cal_device_ids[i] = device_id_val;
+		}
+	}
+
+	// use preferred_index if it's available
+	if ((preferred_index >= 0) && (preferred_index < MAX_SENSOR_COUNT)
+	    && (cal_device_ids[preferred_index] == 0)) {
+
+		calibration_index = preferred_index;
+
+	} else {
+		// otherwise select first available slot
+		for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+			if (cal_device_ids[i] == 0) {
+				calibration_index = i;
+				break;
+			}
+		}
+	}
+
+	if (calibration_index == -1) {
+		PX4_ERR("no %s calibration slots available", sensor_type);
+	}
+
+	return calibration_index;
+}
+
 int32_t GetCalibrationParamInt32(const char *sensor_type, const char *cal_type, uint8_t instance)
 {
 	// eg CAL_MAGn_ID/CAL_MAGn_ROT

src/lib/sensor_calibration/Utilities.hpp

@@ -50,7 +50,17 @@ namespace calibration
  * @param device_id
  * @return int8_t Valid calibration index on success, -1 otherwise
  */
-int8_t FindCalibrationIndex(const char *sensor_type, uint32_t device_id);
+int8_t FindCurrentCalibrationIndex(const char *sensor_type, uint32_t device_id);
+
+/**
+ * @brief Find sensor's calibration index if it exists, otherwise select an available slot.
+ *
+ * @param sensor_type Calibration parameter abbreviated sensor string ("ACC", "GYRO", "MAG")
+ * @param device_id
+ * @param preferred_index preferred index (optional)
+ * @return int8_t Valid calibration index on success, -1 otherwise
+ */
+int8_t FindAvailableCalibrationIndex(const char *sensor_type, uint32_t device_id, int8_t preferred_index = -1);
 
 /**
  * @brief Get sensor calibration parameter value.

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

@@ -69,7 +69,7 @@ bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_s
 			calibration_valid = true;
 
 		} else {
-			calibration_valid = (calibration::FindCalibrationIndex("ACC", device_id) >= 0);
+			calibration_valid = (calibration::FindCurrentCalibrationIndex("ACC", device_id) >= 0);
 		}
 
 		if (!calibration_valid) {

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

@@ -68,7 +68,7 @@ bool PreFlightCheck::gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 			calibration_valid = true;
 
 		} else {
-			calibration_valid = (calibration::FindCalibrationIndex("GYRO", device_id) >= 0);
+			calibration_valid = (calibration::FindCurrentCalibrationIndex("GYRO", device_id) >= 0);
 		}
 
 		if (!calibration_valid) {

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

@@ -70,7 +70,7 @@ bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_st
 			calibration_valid = true;
 
 		} else {
-			calibration_valid = (calibration::FindCalibrationIndex("MAG", device_id) >= 0);
+			calibration_valid = (calibration::FindCurrentCalibrationIndex("MAG", device_id) >= 0);
 		}
 
 		if (!calibration_valid) {

src/modules/commander/accelerometer_calibration.cpp

@@ -350,9 +350,6 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 		} else {
 			calibrations[cur_accel].Reset();
 		}
-
-		// reset calibration index to match uORB numbering
-		calibrations[cur_accel].set_calibration_index(cur_accel);
 	}
 
 	if (active_sensors == 0) {
@@ -435,7 +432,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 				calibrations[i].PrintStatus();
 
 
-				if (calibrations[i].ParametersSave()) {
+				if (calibrations[i].ParametersSave(i, true)) {
 					param_save = true;
 					failed = false;
 
@@ -587,9 +584,6 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
 
 			calibration::Accelerometer calibration{arp.device_id};
 
-			// reset cal index to uORB
-			calibration.set_calibration_index(accel_index);
-
 			if (!calibrated || (offset.norm() > CONSTANTS_ONE_G)
 			    || !PX4_ISFINITE(offset(0))
 			    || !PX4_ISFINITE(offset(1))
@@ -601,7 +595,7 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
 				calibration.set_offset(offset);
 				calibration.set_temperature(temperature_avg);
 
-				if (calibration.ParametersSave()) {
+				if (calibration.ParametersSave(accel_index)) {
 					calibration.PrintStatus();
 					param_save = true;
 					failed = false;

src/modules/commander/gyro_calibration.cpp

@@ -202,9 +202,6 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 		if (gyro_sub.advertised() && (gyro_sub.get().device_id != 0) && (gyro_sub.get().timestamp > 0)) {
 			worker_data.calibrations[cur_gyro].set_device_id(gyro_sub.get().device_id);
 		}
-
-		// reset calibration index to match uORB numbering
-		worker_data.calibrations[cur_gyro].set_calibration_index(cur_gyro);
 	}
 
 	unsigned try_count = 0;
@@ -274,11 +271,9 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 				calibration.set_offset(worker_data.offset[uorb_index]);
 				calibration.set_temperature(worker_data.temperature[uorb_index]);
 
-				calibration.set_calibration_index(uorb_index);
-
 				calibration.PrintStatus();
 
-				if (calibration.ParametersSave()) {
+				if (calibration.ParametersSave(uorb_index, true)) {
 					param_save = true;
 					failed = false;
 

src/modules/commander/mag_calibration.cpp

@@ -914,11 +914,9 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 
 				current_cal.set_temperature(worker_data.temperature[cur_mag]);
 
-				current_cal.set_calibration_index(cur_mag);
-
 				current_cal.PrintStatus();
 
-				if (current_cal.ParametersSave()) {
+				if (current_cal.ParametersSave(cur_mag, true)) {
 					param_save = true;
 					failed = false;
 
@@ -1018,16 +1016,13 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
 
 				calibration::Magnetometer cal{mag.device_id};
 
-				// force calibration index to uORB index
-				cal.set_calibration_index(cur_mag);
-
 				// use any existing scale and store the offset to the expected earth field
 				const Vector3f offset = Vector3f{mag.x, mag.y, mag.z} - (cal.scale().I() * cal.rotation().transpose() * expected_field);
 				cal.set_offset(offset);
 				cal.set_temperature(mag.temperature);
 
 				// save new calibration
-				if (cal.ParametersSave()) {
+				if (cal.ParametersSave(cur_mag)) {
 					cal.PrintStatus();
 					param_save = true;
 					failed = false;

src/modules/gyro_calibration/GyroCalibration.cpp

@@ -243,8 +243,6 @@ void GyroCalibration::Run()
 					return;
 				}
 
-				_gyro_calibration[gyro].set_calibration_index(gyro);
-
 				const Vector3f old_offset{_gyro_calibration[gyro].offset()};
 
 				if (_gyro_calibration[gyro].set_offset(_gyro_mean[gyro].mean())) {
@@ -252,7 +250,7 @@ void GyroCalibration::Run()
 
 					calibration_updated = true;
 
-					PX4_INFO("gyro %d (%" PRIu32 ") updating calibration, [%.4f, %.4f, %.4f] -> [%.4f, %.4f, %.4f] %.1f°C",
+					PX4_INFO("gyro %d (%" PRIu32 ") updating offsets [%.3f, %.3f, %.3f]->[%.3f, %.3f, %.3f] %.1f degC",
 						 gyro, _gyro_calibration[gyro].device_id(),
 						 (double)old_offset(0), (double)old_offset(1), (double)old_offset(2),
 						 (double)_gyro_mean[gyro].mean()(0), (double)_gyro_mean[gyro].mean()(1), (double)_gyro_mean[gyro].mean()(2),
@@ -269,7 +267,7 @@ void GyroCalibration::Run()
 
 			for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) {
 				if (_gyro_calibration[gyro].device_id() != 0) {
-					if (_gyro_calibration[gyro].ParametersSave()) {
+					if (_gyro_calibration[gyro].ParametersSave(gyro)) {
 						param_save = true;
 					}
 				}
@@ -312,7 +310,7 @@ int GyroCalibration::print_status()
 {
 	for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) {
 		if (_gyro_calibration[gyro].device_id() != 0) {
-			PX4_INFO_RAW("gyro %d (%" PRIu32 "), [%.5f, %.5f, %.5f] var: [%.9f, %.9f, %.9f] %.1f°C (count %d)\n",
+			PX4_INFO_RAW("gyro %d (%" PRIu32 "), [%.5f, %.5f, %.5f] var: [%.9f, %.9f, %.9f] %.1f degC (count %d)\n",
 				     gyro, _gyro_calibration[gyro].device_id(),
 				     (double)_gyro_mean[gyro].mean()(0), (double)_gyro_mean[gyro].mean()(1), (double)_gyro_mean[gyro].mean()(2),
 				     (double)_gyro_mean[gyro].variance()(0), (double)_gyro_mean[gyro].variance()(1), (double)_gyro_mean[gyro].variance()(2),

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -35,6 +35,7 @@
 
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/events.h>
+#include <lib/sensor_calibration/Utilities.hpp>
 #include <lib/systemlib/mavlink_log.h>
 
 #include <float.h>
@@ -792,46 +793,8 @@ void VehicleIMU::SensorCalibrationSaveAccel()
 					 (double)cal_orig(0), (double)cal_orig(1), (double)cal_orig(2),
 					 (double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2));
 
-				// find appropriate calibration slot if not already set
-				if (_accel_calibration.calibration_index() < 0) {
-					uint32_t cal_device_ids[calibration::Accelerometer::MAX_SENSOR_COUNT] {};
-					bool cal_slot_match = false;
-
-					for (unsigned cal_index = 0; cal_index < calibration::Accelerometer::MAX_SENSOR_COUNT; cal_index++) {
-						char str[20] {};
-						sprintf(str, "CAL_%s%u_ID", "ACC", cal_index);
-						int32_t device_id_val = 0;
-
-						if (param_get(param_find_no_notification(str), &device_id_val) == PX4_OK) {
-							cal_device_ids[cal_index] = device_id_val;
-
-							if (cal_device_ids[cal_index] == _accel_calibration.device_id()) {
-								cal_slot_match = true;
-								_accel_calibration.set_calibration_index(cal_index);
-								break;
-							}
-						}
-					}
-
-					if (!cal_slot_match) {
-						// prefer slot that matches sensor instance
-						int accel_index = _sensor_accel_sub.get_instance();
-
-						if (cal_device_ids[accel_index] == 0) {
-							_accel_calibration.set_calibration_index(accel_index);
-
-						} else {
-							for (int cal_index = 0; cal_index < calibration::Accelerometer::MAX_SENSOR_COUNT; cal_index++) {
-								if (cal_device_ids[accel_index] == 0) {
-									_accel_calibration.set_calibration_index(cal_index);
-									break;
-								}
-							}
-						}
-					}
-				}
-
-				if (_accel_calibration.ParametersSave()) {
+				// save parameters with preferred calibration slot to current sensor index
+				if (_accel_calibration.ParametersSave(_sensor_accel_sub.get_instance())) {
 					param_notify_changes();
 				}
 			}
@@ -880,46 +843,8 @@ void VehicleIMU::SensorCalibrationSaveGyro()
 					 (double)cal_orig(0), (double)cal_orig(1), (double)cal_orig(2),
 					 (double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2));
 
-				// find appropriate calibration slot if not already set
-				if (_gyro_calibration.calibration_index() < 0) {
-					uint32_t cal_device_ids[calibration::Gyroscope::MAX_SENSOR_COUNT] {};
-					bool cal_slot_match = false;
-
-					for (unsigned cal_index = 0; cal_index < calibration::Gyroscope::MAX_SENSOR_COUNT; cal_index++) {
-						char str[20] {};
-						sprintf(str, "CAL_%s%u_ID", "GYRO", cal_index);
-						int32_t device_id_val = 0;
-
-						if (param_get(param_find_no_notification(str), &device_id_val) == PX4_OK) {
-							cal_device_ids[cal_index] = device_id_val;
-
-							if (cal_device_ids[cal_index] == _gyro_calibration.device_id()) {
-								cal_slot_match = true;
-								_gyro_calibration.set_calibration_index(cal_index);
-								break;
-							}
-						}
-					}
-
-					if (!cal_slot_match) {
-						// prefer slot that matches sensor instance
-						int gyro_index = _sensor_gyro_sub.get_instance();
-
-						if (cal_device_ids[gyro_index] == 0) {
-							_gyro_calibration.set_calibration_index(gyro_index);
-
-						} else {
-							for (int cal_index = 0; cal_index < calibration::Gyroscope::MAX_SENSOR_COUNT; cal_index++) {
-								if (cal_device_ids[gyro_index] == 0) {
-									_gyro_calibration.set_calibration_index(cal_index);
-									break;
-								}
-							}
-						}
-					}
-				}
-
-				if (_gyro_calibration.ParametersSave()) {
+				// save parameters with preferred calibration slot to current sensor index
+				if (_gyro_calibration.ParametersSave(_sensor_gyro_sub.get_instance())) {
 					param_notify_changes();
 				}
 			}

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@@ -186,53 +186,18 @@ void VehicleMagnetometer::UpdateMagBiasEstimate()
 					if (_param_sens_mag_autocal.get() && !_armed && mag_bias_est.stable[mag_index]
 					    && (_calibration[mag_index].device_id() != 0) && !_calibration[mag_index].calibrated()) {
 
-						if (_calibration[mag_index].calibration_index() < 0) {
-							// find available slots
-							uint32_t cal_device_ids[MAX_SENSOR_COUNT] {};
-							bool cal_slot_match = false;
+						// set initial mag calibration
+						const Vector3f offset = _calibration[mag_index].BiasCorrectedSensorOffset(_calibration_estimator_bias[mag_index]);
 
-							for (unsigned cal_index = 0; cal_index < MAX_SENSOR_COUNT; cal_index++) {
-								char str[20] {};
-								sprintf(str, "CAL_%s%u_ID", "MAG", cal_index);
-								int32_t device_id_val = 0;
+						if (_calibration[mag_index].set_offset(offset)) {
+							_calibration[mag_index].set_temperature(_last_data[mag_index].temperature);
 
-								if (param_get(param_find_no_notification(str), &device_id_val) == PX4_OK) {
-									cal_device_ids[cal_index] = device_id_val;
+							// save parameters with preferred calibration slot to current sensor index
+							_calibration[mag_index].ParametersSave(mag_index);
 
-									if (cal_device_ids[cal_index] == _calibration[mag_index].device_id()) {
-										cal_slot_match = true;
-										_calibration[mag_index].set_calibration_index(cal_index);
-									}
-								}
-							}
+							_calibration_estimator_bias[mag_index].zero();
 
-							if (!cal_slot_match) {
-								// prefer slot that matches sensor instance
-								if (cal_device_ids[mag_index] == 0) {
-									_calibration[mag_index].set_calibration_index(mag_index);
-
-								} else {
-									for (int cal_index = 0; cal_index < MAX_SENSOR_COUNT; cal_index++) {
-										if (cal_device_ids[mag_index] == 0) {
-											_calibration[mag_index].set_calibration_index(cal_index);
-											break;
-										}
-									}
-								}
-							}
-						}
-
-						// set initial mag calibration if calibration index is set successfully
-						if (_calibration[mag_index].calibration_index() >= 0) {
-							const Vector3f offset = _calibration[mag_index].BiasCorrectedSensorOffset(_calibration_estimator_bias[mag_index]);
-
-							if (_calibration[mag_index].set_offset(offset)) {
-								_calibration[mag_index].set_temperature(_last_data[mag_index].temperature);
-								_calibration[mag_index].ParametersSave();
-								_calibration_estimator_bias[mag_index].zero();
-
-								parameters_notify = true;
-							}
+							parameters_notify = true;
 						}
 					}
 				}

src/modules/sensors/voted_sensors_update.cpp

@@ -90,7 +90,7 @@ void VotedSensorsUpdate::parametersUpdate()
 		if (imu.get().timestamp > 0 && imu.get().accel_device_id > 0 && imu.get().gyro_device_id > 0) {
 
 			// find corresponding configured accel priority
-			int8_t accel_cal_index = calibration::FindCalibrationIndex("ACC", imu.get().accel_device_id);
+			int8_t accel_cal_index = calibration::FindCurrentCalibrationIndex("ACC", imu.get().accel_device_id);
 
 			if (accel_cal_index >= 0) {
 				// found matching CAL_ACCx_PRIO
@@ -113,7 +113,7 @@ void VotedSensorsUpdate::parametersUpdate()
 			}
 
 			// find corresponding configured gyro priority
-			int8_t gyro_cal_index = calibration::FindCalibrationIndex("GYRO", imu.get().gyro_device_id);
+			int8_t gyro_cal_index = calibration::FindCurrentCalibrationIndex("GYRO", imu.get().gyro_device_id);
 
 			if (gyro_cal_index >= 0) {
 				// found matching CAL_GYROx_PRIO