 		0x2 << 2 | // 1000 degrees per second
 		0x0);
 
+	calibrate_sensors();
+
 	ahrs.begin((float) SAMPLE_FREQUENCY);
+}
+
+int acc_x_offset;
+int acc_y_offset;
+int acc_z_offset;
+
+int gyro_x_offset;
+int gyro_y_offset;
+int gyro_z_offset;
+
+void calibrate_sensors()
+{
+	int samples_count = 0;
+
+	int64_t acc_x_sum = 0;
+	int64_t acc_y_sum = 0;
+	int64_t acc_z_sum = 0;
+
+	int64_t gyro_x_sum = 0;
+	int64_t gyro_y_sum = 0;
+	int64_t gyro_z_sum = 0;
+
+	// Collect sensor data for a second
+	while(micros() < MICROS_PER_SEC)
+	{
+		imu.read();
+
+		acc_x_sum += imu.a.x;
+		acc_y_sum += imu.a.y;
+		acc_z_sum += imu.a.z;
+
+		gyro_x_sum += imu.g.x;
+		gyro_y_sum += imu.g.y;
+		gyro_z_sum += imu.g.z;
+
+		++samples_count;
+	}
+
+	// Should I use floats here?
+	acc_x_offset = acc_x_sum / samples_count;
+	acc_y_offset = acc_y_sum / samples_count;
+	acc_z_offset = acc_z_sum / samples_count - gScale;
+
+	gyro_x_offset = gyro_x_sum / samples_count;
+	gyro_y_offset = gyro_y_sum / samples_count;
+	gyro_z_offset = gyro_z_sum / samples_count;
 }
 
 void read_sensors()
 {
 	imu.read();
+
+	imu.a.x -= acc_x_offset;
+	imu.a.y -= acc_y_offset;
+	imu.a.z -= acc_z_offset;
+
+	imu.g.x -= gyro_x_offset;
+	imu.g.y -= gyro_y_offset;
+	imu.g.z -= gyro_z_offset;
 }
 
 void update_ahrs()

	if(currentMicros - lastSampleMicros >= MICROS_PER_SEC / SAMPLE_FREQUENCY)
	{
		read_sensors();

		if(currentMicros < MICROS_PER_SEC)
		{
			// calibrate sensors
		}
		else update_ahrs();

		lastSampleMicros = currentMicros;
	}


char report[80];

int cyclesPerSecond = 0;

void print_info()
{
	float gyroX = imu.g.x / dpsScale;
