2 files changed, 57 insertions, 66 deletions

diff --git a/src/sensor/calibration.cpp b/src/sensor/calibration.cpp
index 827094c..1538baa 100644
--- a/src/sensor/calibration.cpp
+++ b/src/sensor/calibration.cpp
@@ -6,13 +6,12 @@
 SensorCalibrator::SensorCalibrator(UniquePtr<Sensor> sensor, SerialStream sout)
 	: _sensor(sensor), _sout(sout)
 {
-	_resetValues();
 }
 
 bool SensorCalibrator::calibrate(unsigned int throttle_time)
 {
 	bool done = false;
-	Time start_time = time_now();
+	auto start_time = time_now();
 
 	while (!done)
 	{
@@ -23,85 +22,74 @@ bool SensorCalibrator::calibrate(unsigned int throttle_time)
 
 		delay(throttle_time);
 
-		_resetValues();
+		auto values = SensorCalibratorValues();
 
 		for (unsigned int i = 0U; i < SENSOR_READ_CNT; i++)
 		{
-			_updateValues();
+			_updateValues(values);
 		}
 
-		_adjustValues();
+		_adjustValues(values);
 
-		_sout << "Accel X: " << _accel_x << "	"
-			  << "Accel Y: " << _accel_y << "	"
-			  << "Accel Z: " << _accel_z << "	"
-			  << "Gyro X: " << _gyro_x << "	"
-			  << "Gyro Y: " << _gyro_y << "	"
-			  << "Gyro Z: " << _gyro_z << endl;
+		_sout << "Accel X: " << values.accel_x << "	"
+			  << "Accel Y: " << values.accel_y << "	"
+			  << "Accel Z: " << values.accel_z << "	"
+			  << "Gyro X: " << values.gyro_x << "	"
+			  << "Gyro Y: " << values.gyro_y << "	"
+			  << "Gyro Z: " << values.gyro_z << endl;
 
-		if (_isValuesInRange())
+		if (_isValuesInRange(values))
 		{
 			done = true;
 		}
 
-		_adjustCalibration();
+		_adjustCalibrationWithValues(values);
 	}
 
 	return true;
 }
 
-void SensorCalibrator::_resetValues()
-{
-	_accel_x = 0;
-	_accel_y = 0;
-	_accel_z = 0;
-
-	_gyro_x = 0;
-	_gyro_y = 0;
-	_gyro_z = 0;
-}
-
-void SensorCalibrator::_updateValues()
+void SensorCalibrator::_updateValues(SensorCalibratorValues &values)
 {
 	_sensor->read();
 
-	_accel_x -= _sensor->getAccelX();
-	_accel_y -= _sensor->getAccelY();
-	_accel_z -= _sensor->getAccelZ();
+	values.accel_x -= _sensor->getAccelX();
+	values.accel_y -= _sensor->getAccelY();
+	values.accel_z -= _sensor->getAccelZ();
 
-	_gyro_x -= _sensor->getGyroX();
-	_gyro_y -= _sensor->getGyroY();
-	_gyro_z -= _sensor->getGyroZ();
+	values.gyro_x -= _sensor->getGyroX();
+	values.gyro_y -= _sensor->getGyroY();
+	values.gyro_z -= _sensor->getGyroZ();
 }
 
-void SensorCalibrator::_adjustValues()
+void SensorCalibrator::_adjustCalibrationWithValues(const SensorCalibratorValues &values)
 {
-	_accel_x *= SENSOR_VAL_ADJUST;
-	_accel_y *= SENSOR_VAL_ADJUST;
-	_accel_z *= SENSOR_VAL_ADJUST;
+	_sensor->accel_cal_x += values.accel_x;
+	_sensor->accel_cal_y += values.accel_y;
+	_sensor->accel_cal_z += values.accel_z;
 
-	_gyro_x *= SENSOR_VAL_ADJUST;
-	_gyro_y *= SENSOR_VAL_ADJUST;
-	_gyro_z *= SENSOR_VAL_ADJUST;
+	_sensor->gyro_cal_x += values.gyro_x;
+	_sensor->gyro_cal_y += values.gyro_y;
+	_sensor->gyro_cal_z += values.gyro_z;
 }
 
-bool SensorCalibrator::_isValuesInRange() const
+void SensorCalibrator::_adjustValues(SensorCalibratorValues &values)
 {
-	return (_accel_x < ACCEL_CAL_X_MAX && _accel_x > ACCEL_CAL_X_MIN &&
-			_accel_y < ACCEL_CAL_Y_MAX && _accel_y > ACCEL_CAL_Y_MIN &&
-			_accel_z < ACCEL_CAL_Z_MAX && _accel_z > ACCEL_CAL_Z_MIN &&
-			_gyro_x < GYRO_CAL_X_MAX && _gyro_x > GYRO_CAL_X_MIN &&
-			_gyro_y < GYRO_CAL_Y_MAX && _gyro_y > GYRO_CAL_Y_MIN &&
-			_gyro_z < GYRO_CAL_Z_MAX && _gyro_z > GYRO_CAL_Z_MIN);
+	values.accel_x *= SENSOR_VAL_ADJUST;
+	values.accel_y *= SENSOR_VAL_ADJUST;
+	values.accel_z *= SENSOR_VAL_ADJUST;
+
+	values.gyro_x *= SENSOR_VAL_ADJUST;
+	values.gyro_y *= SENSOR_VAL_ADJUST;
+	values.gyro_z *= SENSOR_VAL_ADJUST;
 }
 
-void SensorCalibrator::_adjustCalibration()
+bool SensorCalibrator::_isValuesInRange(const SensorCalibratorValues &values)
 {
-	_sensor->accel_cal_x += _accel_x;
-	_sensor->accel_cal_y += _accel_y;
-	_sensor->accel_cal_z += _accel_z;
-
-	_sensor->gyro_cal_x += _gyro_x;
-	_sensor->gyro_cal_y += _gyro_y;
-	_sensor->gyro_cal_z += _gyro_z;
+	return (values.accel_x < ACCEL_CAL_X_MAX && values.accel_x > ACCEL_CAL_X_MIN &&
+			values.accel_y < ACCEL_CAL_Y_MAX && values.accel_y > ACCEL_CAL_Y_MIN &&
+			values.accel_z < ACCEL_CAL_Z_MAX && values.accel_z > ACCEL_CAL_Z_MIN &&
+			values.gyro_x < GYRO_CAL_X_MAX && values.gyro_x > GYRO_CAL_X_MIN &&
+			values.gyro_y < GYRO_CAL_Y_MAX && values.gyro_y > GYRO_CAL_Y_MIN &&
+			values.gyro_z < GYRO_CAL_Z_MAX && values.gyro_z > GYRO_CAL_Z_MIN);
 }
diff --git a/src/sensor/calibration.hpp b/src/sensor/calibration.hpp
index a808fe9..a2da58b 100644
--- a/src/sensor/calibration.hpp
+++ b/src/sensor/calibration.hpp
@@ -28,6 +28,18 @@ constexpr uint32_t CALIBRATION_TIMEOUT = 120000; // Milliseconds
 constexpr unsigned int SENSOR_READ_CNT = 20;
 constexpr float SENSOR_VAL_ADJUST = 0.05;
 
+class SensorCalibratorValues
+{
+public:
+	double accel_x = 0;
+	double accel_y = 0;
+	double accel_z = 0;
+
+	double gyro_x = 0;
+	double gyro_y = 0;
+	double gyro_z = 0;
+};
+
 /**
  * Sensor calibrator.
  */
@@ -52,22 +64,13 @@ public:
 	bool calibrate(unsigned int throttle_time);
 
 private:
-	void _resetValues();
-	void _updateValues();
-	void _adjustValues();
-	bool _isValuesInRange() const;
+	void _updateValues(SensorCalibratorValues &values);
+	void _adjustCalibrationWithValues(const SensorCalibratorValues &values);
 
-	void _adjustCalibration();
+	static void _adjustValues(SensorCalibratorValues &values);
+	static bool _isValuesInRange(const SensorCalibratorValues &values);
 
 	UniquePtr<Sensor> _sensor;
 
 	SerialStream _sout;
-
-	double _accel_x = 0;
-	double _accel_y = 0;
-	double _accel_z = 0;
-
-	double _gyro_x = 0;
-	double _gyro_y = 0;
-	double _gyro_z = 0;
 };