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 {
-		Reset();
+		return true;
 	}
+
+	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 {
-		Reset();
+		return true;
 	}
+
+	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 {
-		Reset();
+		return true;
 	}
+
+	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();