refactor: reorganize files & improve classes

4 files changed, 206 insertions, 29 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;
+}
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;
+};
diff --git a/src/sensor/sensor.cpp b/src/sensor/sensor.cpp
index a3b9a5c..5ca5dc3 100644
--- a/src/sensor/sensor.cpp
+++ b/src/sensor/sensor.cpp
@@ -1,20 +1,18 @@
 #include "sensor.hpp"
 
-#include "registers.hpp"
-#include "utils/serial.hpp"
-#include "utils/time.hpp"
+#include "sensor/registers.hpp"
 
 Sensor::Sensor(uint8_t address, TwoWire wire, SerialStream sout,
 			   unsigned int throttle_time)
 	: _wire(wire),
+	  _sout(sout),
 	  _address(address),
 	  _throttle_enabled(true),
 	  _throttle_time(throttle_time),
 	  _last_time(0),
 	  _status(SensorStatus::OK),
 	  _accel_to_g_force(RAW_TO_G_FACTOR),
-	  _ang_rate_to_dps(RAW_TO_DPS_FACTOR),
-	  _sout(sout)
+	  _ang_rate_to_dps(RAW_TO_DPS_FACTOR)
 {
 }
 
@@ -241,52 +239,52 @@ bool Sensor::setGyroSensitivity(uint8_t sensitivity)
 	return true;
 }
 
-double Sensor::getAccelX()
+double Sensor::getAccelX() const
 {
 	return _accel_raw_x;
 }
 
-double Sensor::getAccelY()
+double Sensor::getAccelY() const
 {
 	return _accel_raw_y;
 }
 
-double Sensor::getAccelZ()
+double Sensor::getAccelZ() const
 {
 	return _accel_raw_z;
 }
 
-double Sensor::getAngleX()
+double Sensor::getAngleX() const
 {
 	return _accel_x;
 }
 
-double Sensor::getAngleY()
+double Sensor::getAngleY() const
 {
 	return _accel_y;
 }
 
-double Sensor::getGyroX()
+double Sensor::getGyroX() const
 {
 	return _gyro_raw_x;
 }
 
-double Sensor::getGyroY()
+double Sensor::getGyroY() const
 {
 	return _gyro_raw_y;
 }
 
-double Sensor::getGyroZ()
+double Sensor::getGyroZ() const
 {
 	return _gyro_raw_z;
 }
 
-double Sensor::getPitch()
+double Sensor::getPitch() const
 {
 	return _pitch;
 }
 
-double Sensor::getRoll()
+double Sensor::getRoll() const
 {
 	return _roll;
 }
diff --git a/src/sensor/sensor.hpp b/src/sensor/sensor.hpp
index 3e56627..7b76ae2 100644
--- a/src/sensor/sensor.hpp
+++ b/src/sensor/sensor.hpp
@@ -1,7 +1,7 @@
 #pragma once
 
-#include "utils/serial.hpp"
-#include "utils/time.hpp"
+#include "serial.hpp"
+#include "std/time.hpp"
 
 #include <Arduino.h>
 #include <Wire.h>
@@ -77,52 +77,52 @@ public:
 	/**
 	 * Returns the current X axis acceleration in g:s (g-force).
 	 */
-	double getAccelX();
+	double getAccelX() const;
 
 	/**
 	 * Returns the current Y axis acceleration in g:s (g-force).
 	 */
-	double getAccelY();
+	double getAccelY() const;
 
 	/**
 	 * Returns the current Z axis acceleration in g:s (g-force).
 	 */
-	double getAccelZ();
+	double getAccelZ() const;
 
 	/**
 	 * Returns the current X angle.
 	 */
-	double getAngleX();
+	double getAngleX() const;
 
 	/**
 	 * Returns the current Y angle.
 	 */
-	double getAngleY();
+	double getAngleY() const;
 
 	/**
 	 * Returns the current X axis in degrees/s.
 	 */
-	double getGyroX();
+	double getGyroX() const;
 
 	/**
 	 * Returns the current Y axis in degrees/s.
 	 */
-	double getGyroY();
+	double getGyroY() const;
 
 	/**
 	 * Returns the current Z axis in degrees/s.
 	 */
-	double getGyroZ();
+	double getGyroZ() const;
 
 	/**
 	 * Returns the current pitch angle.
 	 */
-	double getPitch();
+	double getPitch() const;
 
 	/**
 	 * Returns the current roll angle.
 	 */
-	double getRoll();
+	double getRoll() const;
 
 	/**
 	 * Sets the value of a key in the sensor's register.
@@ -157,6 +157,7 @@ public:
 
 private:
 	TwoWire _wire;
+	SerialStream _sout;
 
 	uint8_t _address;
 
@@ -193,6 +194,4 @@ private:
 	double _yaw = 0;
 
 	int16_t _readHighLow();
-
-	SerialStream _sout;
 };