feat: add automatic sensor calibration

1 files changed, 29 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();