 /*    }
     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.
-
-    //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;
+*/ 
+    // Send vehicle state to C3 and C4.   
 
     // 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();

  char* disable =   "$PSRF103,02,00,00,01*26\r\n";
  char* querryGGA = "$PSRF103,00,01,00,01*25";
  bool GPS_available = false;
  Serial3.begin(GPSRATE);
  digitalWrite(GPS_POWER, LOW);         // pull low to turn on!
  Serial.flush();
  Serial3.flush();
  delay(5000);
  // prints title with ending line break
  Serial.println(" GPS parser");
//  Serial.print("Acquiring GPS RMC...");
  checksum(protocol);
  Serial3.println(protocol);

    // ready to roll
    // Fuse all position estimates.
    // Send vehicle state to C3 and C4.
    
}
/*---------------------------------------------------------------------------------------*/ 

void setup() 
{ 
    pinMode(Rx0, INPUT);
    pinMode(Tx0, OUTPUT);
    pinMode(GPS_RX, INPUT);
