feat: add automatic sensor calibration

1 files changed, 41 insertions, 0 deletions

diff --git a/src/sensor.hpp b/src/sensor.hpp
index 7ba01bf..3fbc544 100644
--- a/src/sensor.hpp
+++ b/src/sensor.hpp
@@ -19,6 +19,27 @@
 #define RAW_TO_DPS_FACTOR (1.0 / 131.0)
 #define RAW_TO_G_FACTOR (1.0 / 16384.0)
 
+// Automatic calibration precision
+#define ACCEL_CAL_X_MAX 0.002
+#define ACCEL_CAL_X_MIN -0.002
+
+#define ACCEL_CAL_Y_MAX 0.002
+#define ACCEL_CAL_Y_MIN -0.002
+
+#define ACCEL_CAL_Z_MAX 0.002
+#define ACCEL_CAL_Z_MIN -0.002
+
+#define GYRO_CAL_X_MAX 0.2
+#define GYRO_CAL_X_MIN -0.2
+
+#define GYRO_CAL_Y_MAX 0.02
+#define GYRO_CAL_Y_MIN -0.02
+
+#define GYRO_CAL_Z_MAX 0.08
+#define GYRO_CAL_Z_MIN -0.08
+
+#define CALIBRATION_TIMEOUT 120000 // Milliseconds
+
 class Sensor
 {
 public:
@@ -114,6 +135,11 @@ public:
 	float getAccelY();
 
 	/**
+	 * Returns the current Z axis acceleration in g:s (g-force).
+	 */
+	float getAccelZ();
+
+	/**
 	 * Returns the current X angle.
 	 */
 	float getAngleX();
@@ -139,6 +165,11 @@ public:
 	float getGyroY();
 
 	/**
+	 * Returns the current Z axis in degrees/s.
+	 */
+	float getGyroZ();
+
+	/**
 	 * Returns the current pitch angle.
 	 */
 	float getPitch();
@@ -174,6 +205,16 @@ public:
 	 */
 	int16_t getStatus();
 
+	/**
+	 * Automatically calibrates the sensor.
+	 *
+	 * @returns Whether or not the calibration succeeded.
+	 * 	Will return false if it times out.
+	 */
+	bool autoCalibrate();
+
+	bool is_calibrated;
+
 	// Accelerometer calibration values
 	float accel_cal_x;
 	float accel_cal_y;