 /*    }
     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();

        digitalWrite(GPS_RED_LED, HIGH);
 //     Serial.println("error opening file");
    }
  
  // delay, but don't count time in loop
  while (time < endTime)
  {

      Serial.println("card initialized.\n");
      dataFile = SD.open(GPSfile, FILE_WRITE);
      // if the file is available, write date and time to it:
      if (dataFile)
      {
          dataFile.println(TimeHeader);
          dataFile.println(StartTime);
