1 files changed, 107 insertions, 0 deletions
diff --git a/src/sensor/calibration.cpp b/src/sensor/calibration.cpp
new file mode 100644
index 0000000..40bb52b
--- /dev/null
+++ b/src/sensor/calibration.cpp
@@ -0,0 +1,107 @@
+#include "calibration.hpp"
+
+#include "std/time.hpp"
+#include "utils.hpp"
+
+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();
+
+	while (!done)
+	{
+		if (time_now().diff(start_time).millisecs() >= CALIBRATION_TIMEOUT)
+		{
+			return false;
+		}
+
+		delay(throttle_time);
+
+		_resetValues();
+
+		for (unsigned int i = 0U; i < SENSOR_READ_CNT; i++)
+		{
+			_updateValues();
+		}
+
+		_adjustValues();
+
+		_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;
+
+		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() const
+{
+	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;
+}