1 files changed, 110 insertions, 0 deletions
diff --git a/src/calibration.cpp b/src/calibration.cpp
new file mode 100644
index 0000000..f76d8f8
--- /dev/null
+++ b/src/calibration.cpp
@@ -0,0 +1,110 @@
+#include "calibration.hpp"
+#include "time_utils.hpp"
+
+SensorCalibrator::SensorCalibrator(Sensor *sensor)
+{
+	_sensor = sensor;
+	_resetValues();
+}
+
+bool SensorCalibrator::calibrate(unsigned int throttle_time)
+{
+	bool done = false;
+	Time start_time = time_now();
+
+	while (!done)
+	{
+		if (time_now().diff(start_time).millisecs() >= CALIBRATION_TIMEOUT)
+			return false;
+
+		delay(throttle_time);
+
+		_resetValues();
+
+		for (int i = 0; i < SENSOR_READ_CNT; i++)
+		{
+			_updateValues();
+		}
+
+		_adjustValues();
+
+		Serial.print("Accel X: ");
+		Serial.print(_accel_x);
+		Serial.print("	Accel Y: ");
+		Serial.print(_accel_y);
+		Serial.print("	Accel Z: ");
+		Serial.print(_accel_z);
+
+		Serial.print("	Gyro X: ");
+		Serial.print(_gyro_x);
+		Serial.print("	Gyro Y: ");
+		Serial.print(_gyro_y);
+		Serial.print("	Gyro Z: ");
+		Serial.println(_gyro_z);
+
+		if (_isValuesInRange())
+		{
+			done = true;
+		}
+
+		_adjustCalibration();
+	}
+
+	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()
+{
+	_sensor->read();
+
+	_accel_x -= _sensor->getAccelX();
+	_accel_y -= _sensor->getAccelY();
+	_accel_z -= _sensor->getAccelZ();
+
+	_gyro_x -= _sensor->getGyroX();
+	_gyro_y -= _sensor->getGyroY();
+	_gyro_z -= _sensor->getGyroZ();
+}
+
+void SensorCalibrator::_adjustValues()
+{
+	_accel_x *= SENSOR_VAL_ADJUST;
+	_accel_y *= SENSOR_VAL_ADJUST;
+	_accel_z *= SENSOR_VAL_ADJUST;
+
+	_gyro_x *= SENSOR_VAL_ADJUST;
+	_gyro_y *= SENSOR_VAL_ADJUST;
+	_gyro_z *= SENSOR_VAL_ADJUST;
+}
+
+bool SensorCalibrator::_isValuesInRange()
+{
+	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);
+}
+
+void SensorCalibrator::_adjustCalibration()
+{
+	_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;
+}