 /*    }
     if (landmarks availabe)
     {  // get the position based on bearing and angle to a known location.
-      ReadLandmarks(C4); 
+      ReadLandmarks(C4);
     }
     // Fuse all position estimates with a Kalman Filter */
     deltaT_ms = GPS_reading.time_ms - estimated_position.time_ms;
     estimated_position.fuse(GPS_reading, deltaT_ms);
     estimated_position.time_ms = GPS_reading.time_ms;
-/*  Serial.print("time, gps, dt_ms = "); 
+/*  Serial.print("time, gps, dt_ms = ");
     Serial.print(time, DEC); Serial.print(", ");
     Serial.print(GPS_reading.time_ms, DEC); Serial.print(", ");
     Serial.println(deltaT_ms, DEC);
-*/ 
-    // Send vehicle state to C3 and C4.   
+*/
+    // Send vehicle state to C3 and C4.
+
+    //Get a new sensor event from the magnitomitor
+    sensors_event_t event;
+    mag.getEvent(&event);
+
+    //Calculate the current heading (angle of the vector y,x)
+    float heading = (atan2(event.magnetic.y, event.magnetic.x) * 180) / M_PI;
+
+    //Normalize the heading
+    if (heading < 0)
+    {
+        heading = 360 + heading;
+    }
+    CurrentHeading = heading;
 
     // open the file. note that only one file can be open at a time,
     // so you have to close this one before opening another.
     dataFile = SD.open(GPSfile, FILE_WRITE);
-  
+
     // if the file is available, write to it:
-    if (GPS_available) 
+    if (GPS_available)
     {
         digitalWrite(GPS_GREEN_LED, HIGH);
         pGPS = GPS_reading.formPointString();

    pinMode(INU_RX, INPUT);
    pinMode(INU_TX, OUTPUT);
    pinMode(GPS_POWER, OUTPUT);
    Serial3.begin(GPSRATE); // GPS   
    Serial.begin(9600);
    digitalWrite(GPS_POWER, LOW);         // pull low to turn on!
    // make sure that the default chip select pin is set to

  Tx: $PSRF103,...
*/

/*---------------------------------------------------------------------------------------*/

#define MEGA
#include <Common.h>
#include <SPI.h>
#include <SD.h>
#include <IO.h>
#include <Matrix.h>
#include <Wire.h>
#include "Adafruit_Sensor.h"
#include "Adafruit_LSM303_U.h"

/* Assign a unique ID to this sensor at the same time */
Adafruit_LSM303_Mag mag = Adafruit_LSM303_Mag(1366123);

float CurrentHeading = -1;

// On the Ethernet Shield, CS is pin 4. Note that even if it's not
// used as the CS pin, the hardware CS pin (10 on most Arduino boards,
