1 files changed, 70 insertions, 17 deletions

diff --git a/src/gyronardo.cpp b/src/gyronardo.cpp
index 025bb7d..f1e968e 100644
--- a/src/gyronardo.cpp
+++ b/src/gyronardo.cpp
@@ -1,8 +1,15 @@
 #include "Arduino.h"
 #include "Wire.h"
+#include "calibration.hpp"
 #include "sensor.hpp"
 
-Sensor sensor(0x68, &Wire);
+#define LINE_CLEAR_LENGTH 30
+
+#define SENSOR_THROTTLE_TIME 50 // milliseconds
+
+Sensor sensor(0x68, &Wire, SENSOR_THROTTLE_TIME);
+
+char line_clear[LINE_CLEAR_LENGTH + 1];
 
 /**
  * Stops code execution.
@@ -21,47 +28,93 @@ void setup()
 
 	while (!sensor.begin())
 	{
-		Serial.print(millis());
-		Serial.println("Error: Could not connect to the sensor. Retrying after 2000 milliseconds...");
+		Serial.println("Error: Could not connect to the sensor. Retrying after 2000 "
+					   "milliseconds...");
 		delay(2000);
 	}
-	sensor.setAccelSensitivity(2); // 8g
-	sensor.setGyroSensitivity(1);  // 500 degrees/s
 
-	sensor.setThrottleEnabled(true);
+	if (!sensor.setAccelSensitivity(2)) // 8g
+	{
+		Serial.print(
+			"Error: Failed to set the sensor's accelerometer sensitivity. Status: ");
+		Serial.println(static_cast<int>(sensor.getStatus()));
+		stop();
+	}
 
-	Serial.println("Automatically calibrating sensor...");
-	bool cal_status = sensor.autoCalibrate();
+	if (!sensor.setGyroSensitivity(1)) // 500 degrees/s
+	{
+		Serial.print("Error: Failed to set the sensor's gyro sensitivity. Status: ");
+		Serial.println(static_cast<int>(sensor.getStatus()));
+		stop();
+	}
 
-	if (!cal_status)
+	Serial.println("Calibrating sensor...");
+	SensorCalibrator sensor_calibrator(&sensor);
+
+	if (!sensor_calibrator.calibrate(SENSOR_THROTTLE_TIME))
 	{
-		Serial.print("Error: Automatic calibration timed out after ");
+		Serial.print("Error: Sensor calibration timed out after ");
 		Serial.print(CALIBRATION_TIMEOUT);
 		Serial.println(" milliseconds");
 
 		stop();
 	}
+	Serial.println("Finished calibrating sensor");
+
+	Serial.println("Calibration values:");
 
-	Serial.println("Finished calibrating");
+	Serial.print("Accelerometer X: ");
+	Serial.println(sensor.accel_cal_x);
 
-	Serial.println("Starting...");
+	Serial.print("Accelerometer Y: ");
+	Serial.println(sensor.accel_cal_y);
+
+	Serial.print("Accelerometer Z: ");
+	Serial.println(sensor.accel_cal_z);
+
+	Serial.print("Gyro X: ");
+	Serial.println(sensor.gyro_cal_x);
+
+	Serial.print("Gyro Y: ");
+	Serial.println(sensor.gyro_cal_y);
+
+	Serial.print("Gyro Z: ");
+	Serial.println(sensor.gyro_cal_z);
+
+	Serial.println("\nStarting...");
+
+	size_t prev_len = strlen(line_clear);
+	memset(line_clear + prev_len, ' ', LINE_CLEAR_LENGTH - prev_len);
 }
 
 void loop()
 {
-	if (!sensor.is_calibrated)
+	delay(SENSOR_THROTTLE_TIME);
+
+	if (!sensor.read())
 	{
-		return;
-	}
+		SensorStatus status = sensor.getStatus();
+
+		if (status == SensorStatus::THROTTLED)
+		{
+			Serial.println("Warning: The sensor was read too frequently and throttled");
+			return;
+		}
 
-	sensor.read();
+		Serial.print("Error: Failed to read sensor. Status: ");
+		Serial.println(static_cast<int>(status));
+		stop();
+	}
 
 	float pitch = sensor.getPitch();
 	float roll = sensor.getRoll();
 
+	Serial.print(line_clear);
+	Serial.print("\r");
 	Serial.print("Pitch: ");
 	Serial.print(pitch, 3);
 	Serial.print(" Roll: ");
 	Serial.print(roll, 3);
-	Serial.println();
+	Serial.print("\r");
+	Serial.flush();
 }