The GPS module I've chosen to try is based on a MediaTek MT3329 chip. This has been paired with a patch antenna by a Taiwan company called GlobalTop, and placed on a custom breakout board by 4D Systems, based in Australia. The wonderful thing about this unit is it can give updates at 10Hz, or 10 times a second! In comparison, most hand-held GPS units only update once a second. The higher bandwidth is needed for this project.

Although the spec sheet states that it can be configured to run at 10Hz and a baud rate of 115200, the unit from 4D Systems can NOT be configured "as-is". I did not discover that until my chip arrived. After speaking with them, I was told that the existing firmware doesn't allow me to update the baud rate. But, they offered help and even some "tools" to allow me to update the firmware as needed.

Success! I managed to make the updates without much problems and was finally able to set the chip to 10Hz updates and 115200 baud. The little configuration utility that they supply is very handy and allows you to make these changes on-the-fly. You can even pick and choose which NMEA sentences you want and how often you want them.

The primary sentence I'll be using is $GPRMC. Below is a sample of this sentence and a table showing its format:

$GPRMC,064951.000,A,2307.1256,N,12016.4438,E,0.03,165.48,260406,,,A*55

Now, some code: Below is the code thus far for reading the GPS and parsing some of the variables in the GPS sentence. It is very important to note that the ONLY sentence I'm using is $GPRMC. I have my GPS unit configured to only output this particular sentence, so I am only concerned with this one at the moment. The function ParseGPS() shown below is specifically tailored to the $GPRMC sentence. Of course, this can easily be modified for any sentence you're using.

/////////////////////// GPS Functions ////////////////////


void ReadGPS()     /////// Read GPS data  /////////
{

index=0;
  while(complete==0)
  {
   while(Serial.available() > 0)
   {
    aChar = Serial.read();


    if(aChar == '\n')
    {
       // End of record detected. Time to parse
           length=index;
       index = 0;
           complete=1;
    }
    else
    {
       GPSData[index] = aChar;
       index++;
    }
   }
} // end while complete=0
   
} /////// END read GPS


void ParseGPS()     ////// Parse GPS data //////////
{
  comma_position=0;
  Lat1=0; Lon1=0; Course=0;
  for(index=0; index<length; index++)
  {
    
     if(GPSData[index]==',')
     {
       comma_position++;
       index++;
     }
     
    switch(comma_position)
      {
        case 2: // Valid Data
          ValidData=GPSData[index];
        break;
        
        case 3: // Lat1
              if(GPSData[index]!='.')
              Lat1=Lat1*10+(GPSData[index]-'0');
        break;
        
        case 4: // N/S Indicator
          NS_Indicator=GPSData[index];
        break;
        
        case 5: // Lon1
              if(GPSData[index]!='.')
              Lon1=Lon1*10+(GPSData[index]-'0');
        break;
        
        case 6: // E/W Indicator
          EW_Indicator=GPSData[index];
        break;
        
        case 8: // Course (course)
              if(GPSData[index]!='.')
              Course=Course*10+(GPSData[index]-'0');
        break;
      } // switch

  } // for loop
  
  
} // End parse GPS

How it works: In the function ReadGPS(), serial data is read until the end of the line is reached and stored in the array GPSData[]. The function ParseGPS() will then take this comma delimited sentence and break each 'part' of the sentence into its respective variables, but without any decimal points. A switch command is used to do this. The image below shows the numerical assignment to each 'part':

In my code example above, I'm seperating out the variables: valid data, latitude, longitude, N/S indicator, E/W indicator and course. Some of these require extra processing. For example, after ParseGPS() is run, latitude will look like this:

23071256

Lastly, I'll need to also make use of the N/S indicator to determine if we're in a north or south latitude. If southern latitude, I'll need to make the final radian value negative. Here is the code for both latitude and longitude:

  /////////////////////////////////////////////
  /////  Now, finish parsing  /////////////////
  /////////////////////////////////////////////
  
  // Latitude  -  Seperate Lat1 into Deg & Mins, convert to decimal deg, then to radians
  TempLong = Lat1/1000000;                     // Degrees
  TempFloat = (Lat1-TempLong*1000000)/10000;   // Minutes             - TempLong & TempFloat are just temporary variables
  Lat1 = TempLong + TempFloat/60;              // Decimal degrees       one of them is float, the other Long
  Lat1 = Lat1 * Deg2Rad;                       // Radians
  if(NS_Indicator=='S') Lat1=Lat1*-1;           //make negative if South latitude


  // Longitude
  TempLong = Lon1/1000000;                     // Degrees
  TempFloat = (Lon1-TempLong*1000000)/10000;   // Minutes
  Lon1 = TempLong + TempFloat/60;              // Decimal degrees
  Lon1 = Lon1 * Deg2Rad;                       // Radians
  if(EW_Indicator=='W') Lon1=Lon1*-1;           // make negative if West longitude

Bearing: One of the more important functions for an INS is calculating bearing from one point to another. This is fairly easy to do and the formulas are readily found online. Here is one such website that gives several formulas for distance, bearing and other useful calculations. These calculations all use radians, so care is needed to ensure proper conversions are done. I did have some initial concerns about doing these calculations on my Arduino due to it's limited floating-point math capabilities. But I was relieved to find it handled the following bearing calc just fine! For this project, accuracy to tenths of a degree are acceptable, and Arduino has no problem delivering. The code:

void Calc_Bearing() // Bearing  -  calculate bearing from point to point
{
  dLon = Lon2-Lon1;
  Bearing = atan2(sin(dLon)*cos(Lat2),cos(Lat1)*sin(Lat2)-sin(Lat1)*cos(Lat2)*cos(dLon));
  Bearing=Bearing*57.29578;
  if(Bearing<0) Bearing+=360;  
}

Distance: By using the Haversine formula, distance can be calculated between two points. This code will calculate distance in miles. For kilometers, use an Earth radius of 6371km instead of 3959mi. Code:

void Calc_Distance() // Distance - calculate distance between two points
{
  dLat = Lat2 - Lat1; // dLon is already calculated above
  float a = sin(dLat/2) * sin(dLat/2) + sin(dLon/2) * sin(dLon/2) * cos(Lat1) * cos(Lat2); 
  float c = 2 * atan2(sqrt(a), sqrt(1-a)); 
  Distance = 3959 * c; // 3959 is average Earth radius in miles
}

And to make life easy, here is a complete sketch to perform all the above:

   //////////////////////////////////////////////////
  //      GPS parsing and calculations		  //
 //  Brought to you by: www.NuclearProjects.com  //
//////////////////////////////////////////////////

#define Deg2Rad 0.0174532925 // 0.0174532925 rads = 1 deg

//---- GPS related variables ----------------------
char aChar;
char GPSData[80];

int index = 0;
int length, complete, comma_position=0;
int ValidData, EW_Indicator, NS_Indicator;

float TempFloat, Lat1, Lat2, Lon1, Lon2, Course;
long TempLong;

float dLon = 0;   // difference from Lon1 and Lon2
float dLat = 0;   // difference from Lat1 and Lat2
float Bearing, Distance;




void setup()
{
  Serial.begin(115200);	      // Set this to whatever your GPS module supports
  delay(300);                 // Give things time to "warm-up"

  // Test lat/lon in radians (these simulate a waypoint)
  Lat2 = 0.835899;
  Lon2 = -2.14473;
}

void loop()
{

  //---------- Parse GPS ------------------
  //  Read GPS when data is available
    if(Serial.available() > 0)   ReadGPS();
    if(complete==1)
      {
        ParseGPS();
        Calc_Bearing();
        Calc_Distance();
    	Print_Results();
      }
  //---------------------------------------

  delay(10);
} // End loop


//////////////////   FUNCTIONS   ///////////////////////

void ReadGPS()     /////// Read GPS data  /////////
{

index=0;
  while(complete==0)
  {
   while(Serial.available() > 0)
   {
    aChar = Serial.read();


    if(aChar == '\n')
    {
       // End of record detected. Time to parse
           length=index;
       index = 0;
           complete=1;
    }
    else
    {
       GPSData[index] = aChar;
       index++;
    }
   }
} // end while complete=0
   
} /////// END read GPS


void ParseGPS()     ////// Parse GPS data //////////
{
  comma_position=0;
  Lat1=0; Lon1=0; Course=0;
  for(index=0; index<length; index++)
  {
    
     if(GPSData[index]==',')
     {
       comma_position++;
       index++;
     }
     
    switch(comma_position)
      {
        case 2: // Valid Data
          ValidData=GPSData[index];
        break;
        
        case 3: // Lat1
              if(GPSData[index]!='.')
              Lat1=Lat1*10+(GPSData[index]-'0');
        break;
        
        case 4: // Valid Data
          NS_Indicator=GPSData[index];
        break;
        
        case 5: // Lon1
              if(GPSData[index]!='.')
              Lon1=Lon1*10+(GPSData[index]-'0');
        break;
        
        case 6: // Valid Data
          EW_Indicator=GPSData[index];
        break;
        
        case 8: // Course (course)
              if(GPSData[index]!='.')
              Course=Course*10+(GPSData[index]-'0');
        break;
      } // switch

  } // for loop
  
  
  /////////////////////////////////////////////
  /////  Now, finish parsing  /////////////////
  /////////////////////////////////////////////
  
  // Latitude  -  Seperate Lat1 into Deg & Mins, convert to decimal deg, then to radians
  TempLong = Lat1/1000000;                     // Degrees
  TempFloat = (Lat1-TempLong*1000000)/10000;   // Minutes
  Lat1 = TempLong + TempFloat/60;              // Decimal degrees
  Lat1 = Lat1 * Deg2Rad;                       // Radians
  if(NS_Indicator=='S') Lat1=Lat1*-1;          // make negative if South latitude
  
  // Longitude
  TempLong = Lon1/1000000;                     // Degrees
  TempFloat = (Lon1-TempLong*1000000)/10000;   // Minutes
  Lon1 = TempLong + TempFloat/60;              // Decimal degrees
  Lon1 = Lon1 * Deg2Rad;                       // Radians
  if(EW_Indicator=='W') Lon1=Lon1*-1;          // make negative if West longitude
  
  // Course
  Course = Course/100;
  
  
  complete=0;  // reset variable
} // End parse GPS



void Calc_Bearing() // Bearing  -  calculate bearing from point to point
{
  dLon = Lon2-Lon1;
  Bearing = atan2(sin(dLon)*cos(Lat2),cos(Lat1)*sin(Lat2)-sin(Lat1)*cos(Lat2)*cos(dLon));
  Bearing=Bearing*57.29578;
  if(Bearing<0) Bearing+=360;  
}

void Calc_Distance() // Distance - calculate distance between two points
{
  dLat = Lat2 - Lat1; // dLon is already calculated above
  float a = sin(dLat/2) * sin(dLat/2) + sin(dLon/2) * sin(dLon/2) * cos(Lat1) * cos(Lat2); 
  float c = 2 * atan2(sqrt(a), sqrt(1-a)); 
  Distance = 3959 * c; // 3959 is average Earth radius in miles
}


void Print_Results()
{
  Serial.print("Lat: ");
  Serial.print(Lat1,6);   
  Serial.print(NS_Indicator,BYTE);     
  Serial.print("   ");

  Serial.print("Lon: ");
  Serial.print(Lon1,6); 
  Serial.print(EW_Indicator,BYTE);      
  Serial.print("   ");

  Serial.print("Bearing: ");
  Serial.print(Bearing,4);    
  Serial.print("   ");
  
  Serial.print("Distance: ");
  Serial.println(Distance,4);    

}