lib/sensor_calibration: handle calibration slot change on parameter update

src/lib/sensor_calibration/Accelerometer.cpp

@@ -150,12 +150,14 @@ void Accelerometer::ParametersUpdate()
 		return;
 	}
 
-	if (_calibration_index < 0) {
+	_calibration_index = FindCalibrationIndex(SensorString(), _device_id);
-		_calibration_index = FindCalibrationIndex(SensorString(), _device_id);
-	}
 
-	if (_calibration_index >= 0) {
+	ParametersLoad();
+}
 
+bool Accelerometer::ParametersLoad()
+{
+	if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
 		// CAL_ACCx_ROT
 		int32_t rotation_value = GetCalibrationParamInt32(SensorString(), "ROT", _calibration_index);
 
@@ -205,9 +207,10 @@ void Accelerometer::ParametersUpdate()
 		// CAL_ACCx_SCALE{X,Y,Z}
 		set_scale(GetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index));
 
-	} else {
+		return true;
-		Reset();
 	}
+
+	return false;
 }
 
 void Accelerometer::Reset()

src/lib/sensor_calibration/Accelerometer.hpp

@@ -91,6 +91,7 @@ public:
 		return (_rotation.I() * bias).edivide(_scale) + _thermal_offset + _offset;
 	}
 
+	bool ParametersLoad();
 	bool ParametersSave();
 	void ParametersUpdate();
 

src/lib/sensor_calibration/Gyroscope.cpp

@@ -135,12 +135,14 @@ void Gyroscope::ParametersUpdate()
 		return;
 	}
 
-	if (_calibration_index < 0) {
+	_calibration_index = FindCalibrationIndex(SensorString(), _device_id);
-		_calibration_index = FindCalibrationIndex(SensorString(), _device_id);
-	}
 
-	if (_calibration_index >= 0) {
+	ParametersLoad();
+}
 
+bool Gyroscope::ParametersLoad()
+{
+	if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
 		// CAL_GYROx_ROT
 		int32_t rotation_value = GetCalibrationParamInt32(SensorString(), "ROT", _calibration_index);
 
@@ -187,9 +189,10 @@ void Gyroscope::ParametersUpdate()
 		// CAL_GYROx_OFF{X,Y,Z}
 		set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index));
 
-	} else {
+		return true;
-		Reset();
 	}
+
+	return false;
 }
 
 void Gyroscope::Reset()

src/lib/sensor_calibration/Gyroscope.hpp

@@ -95,6 +95,7 @@ public:
 		return (_rotation.I() * bias) + _thermal_offset + _offset;
 	}
 
+	bool ParametersLoad();
 	bool ParametersSave();
 	void ParametersUpdate();
 

src/lib/sensor_calibration/Magnetometer.cpp

@@ -135,12 +135,14 @@ void Magnetometer::ParametersUpdate()
 		return;
 	}
 
-	if (_calibration_index < 0) {
+	_calibration_index = FindCalibrationIndex(SensorString(), _device_id);
-		_calibration_index = FindCalibrationIndex(SensorString(), _device_id);
-	}
 
-	if (_calibration_index >= 0) {
+	ParametersLoad();
+}
 
+bool Magnetometer::ParametersLoad()
+{
+	if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) {
 		// CAL_MAGx_ROT
 		int32_t rotation_value = GetCalibrationParamInt32(SensorString(), "ROT", _calibration_index);
 
@@ -196,9 +198,10 @@ void Magnetometer::ParametersUpdate()
 		// CAL_MAGx_COMP{X,Y,Z}
 		_power_compensation = GetCalibrationParamsVector3f(SensorString(), "COMP", _calibration_index);
 
-	} else {
+		return true;
-		Reset();
 	}
+
+	return false;
 }
 
 void Magnetometer::Reset()

src/lib/sensor_calibration/Magnetometer.hpp

@@ -93,6 +93,7 @@ public:
 		return _scale.I() * _rotation.I() * bias + _offset;
 	}
 
+	bool ParametersLoad();
 	bool ParametersSave();
 	void ParametersUpdate();
 

src/modules/sensors/sensors.cpp

@@ -340,26 +340,32 @@ int Sensors::parameters_update()
 	// ensure calibration slots are active for the number of sensors currently available
 	// this to done to eliminate differences in the active set of parameters before and after sensor calibration
 	for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) {
+
+		// sensor_accel
 		uORB::SubscriptionData<sensor_accel_s> sensor_accel_sub{ORB_ID(sensor_accel), i};
-		uORB::SubscriptionData<sensor_gyro_s> sensor_gyro_sub{ORB_ID(sensor_gyro), i};
-		uORB::SubscriptionData<sensor_mag_s> sensor_mag_sub{ORB_ID(sensor_mag), i};
 
 		if (sensor_accel_sub.get().device_id != 0) {
-			calibration::Accelerometer cal{sensor_accel_sub.get().device_id};
+			calibration::Accelerometer cal;
 			cal.set_calibration_index(i);
-			cal.ParametersUpdate();
+			cal.ParametersLoad();
 		}
 
+		// sensor_gyro
+		uORB::SubscriptionData<sensor_gyro_s> sensor_gyro_sub{ORB_ID(sensor_gyro), i};
+
 		if (sensor_gyro_sub.get().device_id != 0) {
-			calibration::Gyroscope cal{sensor_gyro_sub.get().device_id};
+			calibration::Gyroscope cal;
 			cal.set_calibration_index(i);
-			cal.ParametersUpdate();
+			cal.ParametersLoad();
 		}
 
+		// sensor_mag
+		uORB::SubscriptionData<sensor_mag_s> sensor_mag_sub{ORB_ID(sensor_mag), i};
+
 		if (sensor_mag_sub.get().device_id != 0) {
-			calibration::Magnetometer cal{sensor_mag_sub.get().device_id};
+			calibration::Magnetometer cal;
 			cal.set_calibration_index(i);
-			cal.ParametersUpdate();
+			cal.ParametersLoad();
 		}
 	}