feat: add automatic sensor calibration

3 files changed, 149 insertions, 6 deletions

diff --git a/src/gyronardo.cpp b/src/gyronardo.cpp
index 1a81850..025bb7d 100644
--- a/src/gyronardo.cpp
+++ b/src/gyronardo.cpp
@@ -4,6 +4,14 @@
 
 Sensor sensor(0x68, &Wire);
 
+/**
+ * Stops code execution.
+ */
+void stop()
+{
+	while (true) {}
+}
+
 void setup()
 {
 	Serial.begin(9600);
@@ -21,18 +29,33 @@ void setup()
 	sensor.setGyroSensitivity(1);  // 500 degrees/s
 
 	sensor.setThrottleEnabled(true);
-	Serial.println("start...");
 
-	// Calibration values
-	sensor.accel_cal_x = 0.198;
-	sensor.accel_cal_y = -0.018;
-	sensor.gyro_cal_x = 0.780;
-	sensor.gyro_cal_y = -1.495;
+	Serial.println("Automatically calibrating sensor...");
+	bool cal_status = sensor.autoCalibrate();
+
+	if (!cal_status)
+	{
+		Serial.print("Error: Automatic calibration timed out after ");
+		Serial.print(CALIBRATION_TIMEOUT);
+		Serial.println(" milliseconds");
+
+		stop();
+	}
+
+	Serial.println("Finished calibrating");
+
+	Serial.println("Starting...");
 }
 
 void loop()
 {
+	if (!sensor.is_calibrated)
+	{
+		return;
+	}
+
 	sensor.read();
+
 	float pitch = sensor.getPitch();
 	float roll = sensor.getRoll();
 
diff --git a/src/sensor.cpp b/src/sensor.cpp
index e3a2551..be57609 100644
--- a/src/sensor.cpp
+++ b/src/sensor.cpp
@@ -16,6 +16,8 @@ Sensor::Sensor(uint8_t address, TwoWire *wire)
 	gyro_cal_y = 0;
 	gyro_cal_z = 0;
 
+	is_calibrated = false;
+
 	_throttle_enabled = true;
 	_throttleTime = DEFAULT_THROTTLE_TIME;
 
@@ -284,6 +286,7 @@ uint8_t Sensor::getGyroSensitivity()
 
 float Sensor::getAccelX() { return _accel_raw_x; };
 float Sensor::getAccelY() { return _accel_raw_y; };
+float Sensor::getAccelZ() { return _accel_raw_z; };
 
 float Sensor::getAngleX() { return _accel_x; };
 float Sensor::getAngleY() { return _accel_y; };
@@ -292,6 +295,7 @@ float Sensor::getTemperature() { return _temperature; };
 
 float Sensor::getGyroX() { return _gyro_raw_x; };
 float Sensor::getGyroY() { return _gyro_raw_y; };
+float Sensor::getGyroZ() { return _gyro_raw_z; };
 
 float Sensor::getPitch() { return _pitch; };
 float Sensor::getRoll() { return _roll; };
@@ -352,3 +356,78 @@ int16_t Sensor::_readTwoBytes()
 
 	return response;
 }
+
+bool Sensor::autoCalibrate()
+{
+	bool done = false;
+
+	float accel_x, accel_y, accel_z;
+	float gyro_x, gyro_y, gyro_z;
+
+	auto cal_start_time = millis();
+
+	while (!done)
+	{
+		if (millis() - cal_start_time >= CALIBRATION_TIMEOUT)
+		{
+			return false;
+		}
+
+		delay(100);
+
+		accel_x = 0;
+		accel_y = 0;
+		accel_z = 0;
+
+		gyro_x = 0;
+		gyro_y = 0;
+		gyro_z = 0;
+
+		for (int i = 0; i < 20; i++)
+		{
+			read();
+
+			accel_x -= getAccelX();
+			accel_y -= getAccelY();
+			accel_z -= getAccelZ();
+
+			gyro_x -= getGyroX();
+			gyro_y -= getGyroY();
+			gyro_z -= getGyroZ();
+		}
+
+		const float adjust_val = 0.05;
+
+		accel_x *= adjust_val;
+		accel_y *= adjust_val;
+		accel_z *= adjust_val;
+
+		gyro_x *= adjust_val;
+		gyro_y *= adjust_val;
+		gyro_z *= adjust_val;
+
+		if (
+			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)
+		{
+			done = true;
+		}
+
+		// Adjust calibration values
+		accel_cal_x += accel_x;
+		accel_cal_y += accel_y;
+		accel_cal_z += accel_z;
+
+		gyro_cal_x += gyro_x;
+		gyro_cal_y += gyro_y;
+		gyro_cal_z += gyro_z;
+	}
+
+	is_calibrated = true;
+
+	return true;
+}
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;