1 files changed, 79 insertions, 0 deletions
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;
+}