diff --git a/sample_main.cpp b/sample_main.cpp
index 68efa5f..47225ec 100644
--- a/sample_main.cpp
+++ b/sample_main.cpp
@@ -22,7 +22,7 @@ Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1();
 unsigned long now = 0, last = 0; //micros() timers for AHRS loop
 float deltat = 0;  //loop time in seconds
 
-#define SPEED 100 // ms between angle prints
+#define SPEED 50 // ms between angle prints
 unsigned long lastPrint = 0; // Keep track of print time
 // Vector to hold quaternion
 static float q[4] = {1.0, 0.0, 0.0, 0.0};
@@ -32,6 +32,11 @@ static float q[4] = {1.0, 0.0, 0.0, 0.0};
 // 2 - roll
 static float angles[3];
 
+float pitch_cal = 0;
+float yaw_cal = 0;
+float roll_cal = 0;
+
+
 void setupSensor()
 {
   // 1.) Set the accelerometer range
@@ -57,7 +62,7 @@ void setup()
   Serial.begin(115200);
 
   while (!Serial) {
-    delay(1); // will pause Zero, Leonardo, etc until serial console opens
+    delay(1);
   }
   
   Serial.println("LSM9DS1 data read demo");
@@ -70,13 +75,42 @@ void setup()
   }
   Serial.println("Found LSM9DS1 9DOF");
 
-  // helper to just set the default scaling we want, see above!
   setupSensor();
+
+  static float Gxyz[3], Axyz[3], Mxyz[3];
+  delay(100);
+  Serial.println("CALIBRATING HOLD STILL");
+  delay(500);
+  for (int i=0; i < 50; i++) {
+    lsm.read();  /* ask it to read in the data */ 
+
+    /* Get a new sensor event */ 
+    sensors_event_t a, m, g, temp;
+    lsm.getEvent(&a, &m, &g, &temp); 
+
+    get_scaled_IMU(Gxyz, Axyz, Mxyz, g.gyro.x, g.gyro.y, g.gyro.z, a.acceleration.x, a.acceleration.y, a.acceleration.z, m.magnetic.x, m.magnetic.y, m.magnetic.z);
+    now = micros();
+    deltat = (now - last) * 1.0e-6; //seconds since last update
+    last = now;
+    mahonyQuaternionUpdate(Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[0], Mxyz[1], Mxyz[2], deltat, q);
+    get_angles(angles, q);
+    delay(SPEED);
+    yaw_cal += (angles[0]);
+    pitch_cal += abs(angles[1]);
+    roll_cal += abs(angles[2]);
+  }
+  last = 0;
+  yaw_cal /= 50;
+  pitch_cal /= 50;
+  roll_cal /= 50;
+  Serial.println("CALIBRATION DONE!");
+  Serial.println(pitch_cal);
+  Serial.println(roll_cal);
+
 }
 
 void loop() 
 {
-   static char updated = 0;
    static int loop_counter = 0;
    static float Gxyz[3], Axyz[3], Mxyz[3]; // Gyro/Acc/Mag data
    lsm.read();  /* ask it to read in the data */ 
@@ -92,14 +126,14 @@ void loop()
    mahonyQuaternionUpdate(Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[0], Mxyz[1], Mxyz[2], deltat, q);
    get_angles(angles, q);
    delay(SPEED);
-   if (loop_counter == 5) {
+   if (loop_counter == 30) {
     Serial.print("Yaw: ");
-    Serial.println(angles[0]);
+    Serial.println(angles[0]-yaw_cal);
     Serial.print("Pitch: ");
-    Serial.println(angles[1]);
+    Serial.println(angles[1]-pitch_cal);
     Serial.print("Roll: ");
-    Serial.println(angles[2]);
+    Serial.println(angles[2]+roll_cal);
     loop_counter = 0;
    }
    
-}
+}
\ No newline at end of file