   * Reades the heading from HMC5883 and update compassHeagin in degrees
   */
   void updateHeading(){
-    /* Get a new sensor event */ 
-    sensors_event_t event; 
-    sensors_event_t accel_event;
-    accel.getEvent(&accel_event);
-    mag.getEvent(&event);
-    
-    if(!dof.magTiltCompensation(SENSOR_AXIS_Z, &event, &accel_event)){
-      return;
-    }
-    
-    // Hold the module so that Z is pointing 'up' and you can measure the heading with x&y
-    // Calculate heading when the magnetometer is level, then correct for signs of axis.
-    float heading = atan2(event.magnetic.y, event.magnetic.x);
-    
-    // Once you have your heading, you must then add your 'Declination Angle', which is the 'Error' of the magnetic field in your location.
-    // Find yours here: http://www.magnetic-declination.com/
-    // Mine is: -13* 2' W, which is ~13 Degrees, or (which we need) 0.22 radians
-    // If you cannot find your Declination, comment out these two lines, your compass will be slightly off.
-    float declinationAngle = 0.01;
-    heading += declinationAngle;
-    
-    // Correct for when signs are reversed.
-    if(heading < 0)
-      heading += 2*PI;
-      
-    // Check for wrap due to addition of declination.
-    if(heading > 2*PI)
-      heading -= 2*PI;
-     
-    // Convert radians to degrees for readability.
-    compassHeading = heading * 180/M_PI; 
+    
+    compass.read();
+    compassHeading = compass.heading();
+
     //Adjust heading
     compassHeading += mHeadingOffset;
     if(compassHeading >= 360){

#include <SoftwareSerial.h>
#include <TinyGPS.h>  
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_LSM303_U.h>
#include <Adafruit_L3GD20_U.h>
#include <Adafruit_9DOF.h>
#include <SabertoothSimplified.h>
#include <EasyTransfer.h>
#include <Kalman.h>

SoftwareSerial mySerial(2, 255);    //used for gps rx and tx pins in use

//Compass Stuff
LSM303 compass;
float compassHeading;
int mHeadingOffset = 0;
#define HEADING_ADJUST_PIN A2
