1 files changed, 73 insertions, 0 deletions
diff --git a/src/sensor/calibration.hpp b/src/sensor/calibration.hpp
new file mode 100644
index 0000000..03e2e9c
--- /dev/null
+++ b/src/sensor/calibration.hpp
@@ -0,0 +1,73 @@
+#pragma once
+
+#include "sensor/sensor.hpp"
+#include "serial.hpp"
+#include "std/memory.hpp"
+
+// Calibration precision
+constexpr float ACCEL_CAL_X_MAX = 0.006;
+constexpr float ACCEL_CAL_X_MIN = -0.006;
+
+constexpr float ACCEL_CAL_Y_MAX = 0.006;
+constexpr float ACCEL_CAL_Y_MIN = -0.006;
+
+constexpr float ACCEL_CAL_Z_MAX = 0.006;
+constexpr float ACCEL_CAL_Z_MIN = -0.006;
+
+constexpr float GYRO_CAL_X_MAX = 0.06;
+constexpr float GYRO_CAL_X_MIN = -0.06;
+
+constexpr float GYRO_CAL_Y_MAX = 0.06;
+constexpr float GYRO_CAL_Y_MIN = -0.06;
+
+constexpr float GYRO_CAL_Z_MAX = 0.06;
+constexpr float GYRO_CAL_Z_MIN = -0.06;
+
+constexpr uint32_t CALIBRATION_TIMEOUT = 120000; // Milliseconds
+
+constexpr unsigned int SENSOR_READ_CNT = 20;
+constexpr float SENSOR_VAL_ADJUST = 0.05;
+
+/**
+ * Sensor calibrator.
+ */
+class SensorCalibrator
+{
+public:
+	/**
+	 * Sensor calibrator.
+	 *
+	 * @param sensor A sensor to calibrate
+	 * @param sout A Serial output stream
+	 */
+	SensorCalibrator(UniquePtr<Sensor> &sensor, SerialStream sout);
+
+	/**
+	 * 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() const;
+
+	void _adjustCalibration();
+
+	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;
+};