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.