I've done parsing of a NEMA message. In my case I wanted Lat, Lon, and Altitude. Perhaps my code can be used as a model.
#include <NewSoftSerial.h>
// Fault flags
extern const unsigned int GPS_fault;
extern unsigned int Faults;
NewSoftSerial GPS(2, -1);
// Parse GPS message
byte GPS_byteCount = 0;
unsigned char GPS_buff[100];
unsigned long GPS_time = 0;
String GPS_lat = "";
String GPS_long = "";
String GPS_altitude = "";
unsigned long GPS_lastgood = 0;
unsigned long GPS_lasttest = 0;
void GPS_setup(void)
{
GPS.begin(4800);
}
void GPS_loop(void)
{
// If any characters are available from the GPS, send them to the parser
while (GPS.available())
{
GPS_parse_char(GPS.read());
}
// Check for GPS status
if ((millis() - GPS_lastgood > 3000))
Faults |= GPS_fault;
}
void GPS_log(Print &dest)
{
// Log GPS data
dest.print(GPS_time, DEC);
dest.print(',');
dest.print(GPS_lat);
dest.print(',');
dest.print(GPS_long);
dest.print(',');
dest.print(GPS_altitude);
dest.print(',');
dest.print((millis() - GPS_lastgood)/1000);
}
int GPS_parse_char(byte inputChar)
{
switch (inputChar)
{
case '\r':
return GPS_check_buffer();
case '
:
GPS_byteCount = 0; // Start of message
default:
GPS_buff[GPS_byteCount++] = inputChar;
if (GPS_byteCount == 100)
{
GPS_byteCount = 0; // Start of message
return -3; // Buffer overrun
}
return 0; // Not a full message
}
}
int GPS_check_buffer()
{
GPS_buff[GPS_byteCount] = '\0';
unsigned char *buffp = GPS_buff;
// Check the message type
for (int i=0; i<7; i++)
{
if (buffp[i] != "$GPGGA,"[i])
return 0;
}
// Verify the checksum before doing any more
if (GPS_buff[GPS_byteCount-3] != '*') // Checksum marker
return -1; // Bad message format
byte checksum = 0;
for (int i=1; i<GPS_byteCount-3; i++) // Skip '
and '*'
checksum ^= GPS_buff[i];
// Test the result
checksum -= hexval(GPS_buff[GPS_byteCount-2]) << 4;
checksum -= hexval(GPS_buff[GPS_byteCount-1]);
if (checksum != 0) return -2; // Checksum failure
// parse the time field
buffp += 7; // Skip over $GPGGA,
GPS_time = 0;
while (hexval(*buffp) != -1) // for each digit
{
GPS_time *= 10;
GPS_time += hexval(*buffp);
buffp++;
}
if (*buffp++ != '.') // Should be a decimal point
return -1;
while (*buffp == '0') // Skip zeros
buffp++;
if (*buffp++ != ',') // Should be a comma
return -1;
// Parse latitude
GPS_lat = "";
for (int i=0; i<20; i++)
{
if (*buffp == ',')
break;
GPS_lat += *buffp++;
if (i == 1)
GPS_lat += ' '; // Separate degrees from minutes
}
buffp++; // skip the comma
if (*buffp == 'S')
GPS_lat = "-" + GPS_lat;
else
if (*buffp != 'N')
return -1;
buffp++; // Skip the N or S
if (*buffp++ != ',') // Should be a comma
return -1;
// Parse longitude
GPS_long = "";
for (int i=0; i<20; i++)
{
if (*buffp == ',')
break;
GPS_long += *buffp++;
if (i == 2)
GPS_long += ' '; // Separate degrees from minutes
}
buffp++; // skip the comma
if (*buffp == 'W')
GPS_long = "-" + GPS_long;
else
if (*buffp != 'E')
return -1;
buffp++; // Skip the E or W
if (*buffp++ != ',') // Should be a comma
return -1;
if (*buffp++ == '0')
{
Faults |= GPS_fault;
return -3; // No view of sattelites
}
if (*buffp++ != ',') // Should be a comma
return -1;
GPS_lastgood = millis();
while (*buffp++ != ',') ; // Skip number of satellites
while (*buffp++ != ',') ; // Skip horizontal dillution
GPS_altitude = "";
while (*buffp != ',')
GPS_altitude += *buffp++;
//Serial.println((char *)GPS_buff);
Faults &= ~GPS_fault;
return 1; // Good fix
}
int hexval(char digit)
{
if (digit >= '0' && digit <= '9')
return digit - '0';
if (digit >= 'A' && digit <= 'F')
return digit - 'A' + 10;
if (digit >= 'a' && digit <= 'f')
return digit - 'a' + 10;
return -1;
}
