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/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