1 files changed, 71 insertions, 0 deletions
diff --git a/src/calibration.hpp b/src/calibration.hpp
new file mode 100644
index 0000000..79b3758
--- /dev/null
+++ b/src/calibration.hpp
@@ -0,0 +1,71 @@
+#ifndef CALIBRATION_HPP
+#define CALIBRATION_HPP
+
+#include "sensor.hpp"
+
+// Calibration precision
+#define ACCEL_CAL_X_MAX 0.006
+#define ACCEL_CAL_X_MIN -0.006
+
+#define ACCEL_CAL_Y_MAX 0.006
+#define ACCEL_CAL_Y_MIN -0.006
+
+#define ACCEL_CAL_Z_MAX 0.006
+#define ACCEL_CAL_Z_MIN -0.006
+
+#define GYRO_CAL_X_MAX 0.06
+#define GYRO_CAL_X_MIN -0.06
+
+#define GYRO_CAL_Y_MAX 0.06
+#define GYRO_CAL_Y_MIN -0.06
+
+#define GYRO_CAL_Z_MAX 0.06
+#define GYRO_CAL_Z_MIN -0.06
+
+#define CALIBRATION_TIMEOUT 120000 // Milliseconds
+
+#define SENSOR_READ_CNT 20
+#define SENSOR_VAL_ADJUST 0.05
+
+/**
+ * Sensor calibrator.
+ */
+class SensorCalibrator
+{
+public:
+	/**
+	 * Sensor calibrator.
+	 *
+	 * @param sensor A sensor to calibrate
+	 */
+	SensorCalibrator(Sensor *sensor);
+
+	/**
+	 * Calibrates the sensor.
+	 *
+	 * @param throttle_time The sensor's throttle time
+	 * @returns Whether or not the calibration succeeded. Will return false on
+	 * timeout.
+	 */
+	bool calibrate(unsigned int throttle_time);
+
+private:
+	void _resetValues();
+	void _updateValues();
+	void _adjustValues();
+	bool _isValuesInRange();
+
+	void _adjustCalibration();
+
+	Sensor *_sensor;
+
+	float _accel_x;
+	float _accel_y;
+	float _accel_z;
+
+	float _gyro_x;
+	float _gyro_y;
+	float _gyro_z;
+};
+
+#endif