//----------------------------------------------------------
char* waypoint::GetLatLon(char* parseptr)
{
uint32_t latitd, longitd;
char latdir, longdir;
uint32_t degree, fraction;
latitd = parsedecimal(parseptr);
if (latitd != 0)
{
latitd *= 10000;
parseptr = strchr(parseptr, '.')+1;
latitd += parsedecimal(parseptr);
}
parseptr = strchr(parseptr, ',') + 1;
// read latitude N/S data
if (parseptr[0] != ',') {
latdir = parseptr[0];
}
// longitude
parseptr = strchr(parseptr, ',')+1;
longitd = parsedecimal(parseptr);
if (longitd != 0) {
longitd *= 10000;
parseptr = strchr(parseptr, '.')+1;
longitd += parsedecimal(parseptr);
}
parseptr = strchr(parseptr, ',')+1;
// read longitude E/W data
if (parseptr[0] != ',') {
longdir = parseptr[0];
}
// latitude is ddmmmmmm
degree = latitd/1000000;
fraction = (latitd%1000000)*100/60;
latitude = fraction + degree * 1000000;
if (latdir == 'S')
latitude = -latitude;
degree = longitd/1000000;
fraction = (longitd%1000000)*100/60;
longitude = fraction + degree * 1000000;
if (longdir == 'W')
longitude = -longitude;
Compute_mm();
/* Serial.print("\tLat: ");
Serial.print(latitd/1000000, DEC); Serial.print(".");
Serial.print((latitd%1000000)*100/60, DEC); Serial.print(',');
Serial.print("\tLong: ");
Serial.print(longitd/1000000, DEC); Serial.print(".");
Serial.print((longitd%1000000)*10/6, DEC); Serial.print('\n'); */
return parseptr;
}
Date getDateFromBuffer(char * buffer) {
Date date;
uint32_t tmp = parsedecimal(buffer);
date.date = tmp / 10000;
date.month = (tmp / 100) % 100;
date.year = tmp % 100;
return date;
}
Time getTimeFromBuffer(char * buffer) {
Time time;
// hhmmss time data
uint32_t tmp = parsedecimal(buffer);
time.hour = tmp / 10000;
time.minute = (tmp / 100) % 100;
time.second = tmp % 100;
return time;
}
int PA6C::getDate(){
char *parseptr; // a character pointer for parsing
parseptr = buffer+57;
uint32_t tmp = parsedecimal(parseptr);
//uint8_t date = tmp / 10000;
//month = (tmp / 100) % 100;
//year = tmp % 100;
return tmp;
}
float PA6C::getLongitude(){
char *parseptr; // a character pointer for parsing
parseptr = buffer+32;
uint32_t raw = parsedecimal(parseptr);
if (raw != 0) {
raw *= 10000;
parseptr = strchr(parseptr, '.')+1;
raw += parsedecimal(parseptr);
}
float new_val = (raw/1000000); //Grab the degrees from the raw number
new_val = new_val + ((((float)raw/1000000)-new_val)/0.6); //Find the minutes from the raw number
parseptr = strchr(parseptr, ',') + 1;
// read latitude N/S data
if (parseptr[0] != ',') {
if (parseptr[0] == 'W') new_val=new_val*-1; //Change the sign if it is South or West
}
return new_val;
}
int PA6C::getTime(){
// hhmmss time data
char *parseptr;
parseptr = buffer+7;
uint32_t tmp = parsedecimal(parseptr);
int hour = tmp/ 10000;
int minute = (tmp / 100) % 100;
int second = tmp % 100;
return tmp;
}
// Private Methods //////////////////////////////////////////////////////////////
void GPS_NMEA_Class::parse_nmea_gps(void)
{
byte NMEA_check;
long aux_deg;
long aux_min;
char *parseptr;
if (strncmp(buffer,"$GPGGA",6)==0){ // Check if sentence begins with $GPGGA
if (buffer[bufferidx-4]=='*'){ // Check for the "*" character
NMEA_check = parseHex(buffer[bufferidx-3])*16 + parseHex(buffer[bufferidx-2]); // Read the checksums characters
if (GPS_checksum == NMEA_check){ // Checksum validation
//Serial.println("buffer");
NewData = 1; // New GPS Data
parseptr = strchr(buffer, ',')+1;
//parseptr = strchr(parseptr, ',')+1;
Time = parsenumber(parseptr,2); // GPS UTC time hhmmss.ss
parseptr = strchr(parseptr, ',')+1;
//
aux_deg = parsedecimal(parseptr,2); // degrees
aux_min = parsenumber(parseptr+2,4); // minutes (sexagesimal) => Convert to decimal
Lattitude = aux_deg*10000000 + (aux_min*50)/3; // degrees + minutes/0.6 (*10000000) (0.6 = 3/5)
parseptr = strchr(parseptr, ',')+1;
//
if (*parseptr=='S')
Lattitude = -1*Lattitude; // South Lattitudes are negative
//
parseptr = strchr(parseptr, ',')+1;
// W Longitudes are Negative
aux_deg = parsedecimal(parseptr,3); // degrees
aux_min = parsenumber(parseptr+3,4); // minutes (sexagesimal)
Longitude = aux_deg*10000000 + (aux_min*50)/3; // degrees + minutes/0.6 (*10000000)
//Longitude = -1*Longitude; // This Assumes that we are in W longitudes...
parseptr = strchr(parseptr, ',')+1;
//
if (*parseptr=='W')
Longitude = -1*Longitude; // West Longitudes are negative
//
parseptr = strchr(parseptr, ',')+1;
Fix = parsedecimal(parseptr,1);
parseptr = strchr(parseptr, ',')+1;
NumSats = parsedecimal(parseptr,2);
parseptr = strchr(parseptr, ',')+1;
HDOP = parsenumber(parseptr,1); // HDOP * 10
parseptr = strchr(parseptr, ',')+1;
Altitude = parsenumber(parseptr,1)*100; // Altitude in decimeters*100 = milimeters
if (Fix < 1)
Quality = 0; // No FIX
else if(NumSats<5)
Quality = 1; // Bad (Num sats < 5)
else if(HDOP>30)
Quality = 2; // Poor (HDOP > 30)
else if(HDOP>25)
Quality = 3; // Medium (HDOP > 25)
else
Quality = 4; // Good (HDOP < 25)
}
else
{
if (PrintErrors)
Serial.println("GPSERR: Checksum error!!");
}
}
}
else if (strncmp(buffer,"$GPVTG",6)==0){ // Check if sentence begins with $GPVTG
//Serial.println(buffer);
if (buffer[bufferidx-4]=='*'){ // Check for the "*" character
NMEA_check = parseHex(buffer[bufferidx-3])*16 + parseHex(buffer[bufferidx-2]); // Read the checksums characters
if (GPS_checksum == NMEA_check){ // Checksum validation
parseptr = strchr(buffer, ',')+1;
Ground_Course = parsenumber(parseptr,2); // Ground course in degrees * 100
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
Ground_Speed = parsenumber(parseptr,2)*10/36; // Convert Km/h to m/s (*100)
//GPS_line = true;
}
else
{
if (PrintErrors)
Serial.println("GPSERR: Checksum error!!");
}
}
}
else
{
bufferidx = 0;
if (PrintErrors)
Serial.println("GPSERR: Bad sentence!!");
}
}
//----------------------------------------------------------
// Aquire a GPS $GPGGA signal and fill in the waypoint data.
// return 1 if valid, zero if not.
// wait up to max_wait milliseconds to get a valid signal.
bool waypoint::AcquireGPGGA(unsigned long max_wait_ms)
{
uint8_t satelites_used, hdop;
REAL HDOP, error_m, error_mm;
char FixIndicator = '0';
char *parseptr; // a character pointer for parsing
// value of millis() for GPS data
unsigned long AcquisitionTime_ms;
char* pTime;
unsigned long TimeOut = millis() + max_wait_ms;
sigmaE_mm = 10000;
sigmaN_mm = 10000;
// $GPGGA,161229.487,3723.2475,N,12158.3416,W,1,07,1.0,9.0,M,,,,0000*18
while (FixIndicator == '0')
{
if (!readline(3))
{ // nothing to read; how long have we waited?
if (millis() > TimeOut)
return false;
}
// Serial.println(buffer);
if (strncmp(buffer, "$GPGGA",6) == 0)
{
// Serial.println(buffer);
AcquisitionTime_ms = millis();
pTime =
parseptr = buffer+7;
parseptr = strchr(parseptr, ',') + 1;
// grab latitude & long data
parseptr = GetLatLon(parseptr);
parseptr = strchr(parseptr, ',') + 1;
FixIndicator = parseptr[0]; // A = data valid; V = data not valid
if (FixIndicator == '0')
continue;
parseptr += 2;
// satelites used
parseptr = strchr(parseptr, ',')+1;
satelites_used = parsedecimal(parseptr);
// HDOP
parseptr = strchr(parseptr, ',')+1;
hdop = parsedecimal(parseptr);
hdop *= 10;
parseptr = strchr(parseptr, '.')+1;
hdop += parsedecimal(parseptr);
// http://users.erols.com/dlwilson/gpshdop.htm uses the model
// error = sqrt( (3.04*HDOP)^2 + (3.57)^2)
// see http://en.wikipedia.org/wiki/Error_analysis_for_the_Global_Positioning_System
// get Horizontal Dillution of Precision (HDOP).
HDOP = (REAL) hdop / 10.;
error_m = sqrt((3.04*HDOP)*(3.04*HDOP) + 3.57*3.57);
error_mm = 1000 * error_m;
sigmaE_mm = sigmaN_mm = error_mm;
if (offset_ms == 0)
{
SetTime(pTime, "1205xx");
offset_ms = AcquisitionTime_ms;
}
time_ms = AcquisitionTime_ms;
}
}
if (FixIndicator == '0')
return false;
return true;
}
//----------------------------------------------------------
// Aquire a GPS GPRMC signal and fill in the waypoint data.
// return 1 if valid, zero if not.
// wait up to max_wait milliseconds to get a valid signal.
bool waypoint::AcquireGPRMC(unsigned long max_wait_ms)
{
uint8_t groundspeed, trackangle;
char status ='V'; // V = data invalid
char *parseptr; // a character pointer for parsing
// value of millis() for GPS data
unsigned long AcquisitionTime_ms;
char* pTime;
char* pDate;
unsigned long TimeOut = millis() + max_wait_ms;
// $GPRMC,161229.487,A,3723.2475,N,12158.3416,W,0.13,309.62,120598,,*10
sigmaE_mm = 10000;
sigmaN_mm = 10000;
// Serial.println("looking");
CosLatitude = cos(((double) LATITUDE_ORIGIN)/1000000. * TO_RADIANS);
while (status != 'A') // A = data valid
{
if (!readline(3))
{ // nothing to read; how long have we waited?
if (millis() > TimeOut)
{
Serial.println("Timed out");
return false;
}
}
// Serial.println("Read line");
Serial.println(buffer);
// check if $GPRMC (global positioning fixed data)
if (strncmp(buffer, "$GPRMC",6) == 0)
{
AcquisitionTime_ms = millis();
// hhmmss time data
pTime =
parseptr = buffer+7;
parseptr = strchr(parseptr, ',') + 1;
status = parseptr[0]; // A = data valid; V = data not valid
if (status != 'A')
continue;
parseptr += 2;
// grab latitude & long data
parseptr = GetLatLon(parseptr);
// groundspeed
parseptr = strchr(parseptr, ',')+1;
// Serial.println(parseptr);
groundspeed = parsedecimal(parseptr);
// track angle
parseptr = strchr(parseptr, ',')+1;
trackangle = parsedecimal(parseptr);
// date
parseptr = strchr(parseptr, ',')+1;
pDate = parseptr;
/* Serial.print("\tGroundspeed: ");
Serial.print(groundspeed, DEC); Serial.print(",");
Serial.print("\tHeading: ");
Serial.print(trackangle, DEC); Serial.print(",");
Serial.print("\tDate: ");
Serial.println(pDate); */
if (offset_ms == 0)
{
SetTime(pTime, pDate);
offset_ms = AcquisitionTime_ms;
}
time_ms = AcquisitionTime_ms;
}
}
if (status == 'A')
return true;
return false;
}
// Private Methods //////////////////////////////////////////////////////////////
void
AP_GPS_NMEA::parse_nmea_gps(void)
{
uint8_t NMEA_check;
long aux_deg;
long aux_min;
char *parseptr;
if (strncmp(buffer,"$GPGGA",6)==0){ // Check if sentence begins with $GPGGA
if (buffer[bufferidx-4]=='*'){ // Check for the "*" character
NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters
if (GPS_checksum == NMEA_check){ // Checksum validation
//Serial.println("buffer");
_setTime();
valid_read = true;
new_data = true; // New GPS Data
parseptr = strchr(buffer, ',')+1;
//parseptr = strchr(parseptr, ',')+1;
time = parsenumber(parseptr, 2); // GPS UTC time hhmmss.ss
parseptr = strchr(parseptr, ',')+1;
aux_deg = parsedecimal(parseptr, 2); // degrees
aux_min = parsenumber(parseptr + 2, 4); // minutes (sexagesimal) => Convert to decimal
latitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000) (0.6 = 3 / 5)
parseptr = strchr(parseptr, ',')+1;
if ( * parseptr == 'S')
latitude = -1 * latitude; // South latitudes are negative
parseptr = strchr(parseptr, ',')+1;
// W longitudes are Negative
aux_deg = parsedecimal(parseptr, 3); // degrees
aux_min = parsenumber(parseptr + 3, 4); // minutes (sexagesimal)
longitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000)
//longitude = -1*longitude; // This Assumes that we are in W longitudes...
parseptr = strchr(parseptr, ',')+1;
if ( * parseptr == 'W')
longitude = -1 * longitude; // West longitudes are negative
parseptr = strchr(parseptr, ',')+1;
fix = parsedecimal(parseptr, 1);
parseptr = strchr(parseptr, ',')+1;
num_sats = parsedecimal(parseptr, 2);
parseptr = strchr(parseptr, ',')+1;
HDOP = parsenumber(parseptr, 1); // HDOP * 10
parseptr = strchr(parseptr, ',')+1;
altitude = parsenumber(parseptr, 1) * 100; // altitude in decimeters * 100 = milimeters
if (fix < 1)
quality = 0; // No FIX
else if(num_sats < 5)
quality = 1; // Bad (Num sats < 5)
else if(HDOP > 30)
quality = 2; // Poor (HDOP > 30)
else if(HDOP > 25)
quality = 3; // Medium (HDOP > 25)
else
quality = 4; // Good (HDOP < 25)
} else {
_error("GPSERR: Checksum error!!\n");
}
}
} else if (strncmp(buffer,"$GPVTG",6)==0){ // Check if sentence begins with $GPVTG
//Serial.println(buffer);
if (buffer[bufferidx-4]=='*'){ // Check for the "*" character
NMEA_check = parseHex(buffer[bufferidx - 3]) * 16 + parseHex(buffer[bufferidx - 2]); // Read the checksums characters
if (GPS_checksum == NMEA_check){ // Checksum validation
_setTime();
valid_read = true;
new_data = true; // New GPS Data
parseptr = strchr(buffer, ',')+1;
ground_course = parsenumber(parseptr, 1) * 10; // Ground course in degrees * 100
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
parseptr = strchr(parseptr, ',')+1;
ground_speed = parsenumber(parseptr, 1) * 100 / 36; // Convert Km / h to m / s ( * 100)
//GPS_line = true;
} else {
_error("GPSERR: Checksum error!!\n");
}
}
} else {
bufferidx = 0;
_error("GPSERR: Bad sentence!!\n");
}
}
//$GPRMC,225446.000,A,4916.45,N,12311.12,W,000.5,054.7,191194,020.3,E*68\r\n
// 0 1 2 3 4 5 6 7
// 0123456789012345678901234567890123456789012345678901234567890123456789012
// 0 1 2 3 4 5 6 7 8 9 10 11 12
void parseGPSdata(char *gpsBuffer) {
time_t tNow;
tmElements_t tm;
uint8_t gpsCheck1, gpsCheck2; // checksums
// char gpsTime[10]; // time including fraction hhmmss.fff
char gpsFixStat; // fix status
// char gpsLat[7]; // ddmm.ff (with decimal point)
// char gpsLatH; // hemisphere
// char gpsLong[8]; // dddmm.ff (with decimal point)
// char gpsLongH; // hemisphere
// char gpsSpeed[5]; // speed over ground
// char gpsCourse[5]; // Course
// char gpsDate[6]; // Date
// char gpsMagV[5]; // Magnetic variation
// char gpsMagD; // Mag var E/W
// char gpsCKS[2]; // Checksum without asterisk
char *ptr;
uint32_t tmp;
if ( strncmp( gpsBuffer, "$GPRMC,", 7 ) == 0 ) {
//Serial.println("parseGPSData");
//Serial.println(gpsBuffer);
//beep(1000, 1);
//Calculate checksum from the received data
ptr = &gpsBuffer[1]; // start at the "G"
gpsCheck1 = 0; // init collector
/* Loop through entire string, XORing each character to the next */
while (*ptr != '*') // count all the bytes up to the asterisk
{
gpsCheck1 ^= *ptr;
ptr++;
if (ptr>(gpsBuffer+GPSBUFFERSIZE)) goto GPSerror1; // extra sanity check, can't hurt...
}
// now get the checksum from the string itself, which is in hex
gpsCheck2 = atoh(*(ptr+1)) * 16 + atoh(*(ptr+2));
if (gpsCheck1 == gpsCheck2) { // if checksums match, process the data
//beep(1000, 1);
ptr = strtok(gpsBuffer, ",*\r"); // parse $GPRMC
if (ptr == NULL) goto GPSerror1;
ptr = strtok(NULL, ",*\r"); // Time including fraction hhmmss.fff
if (ptr == NULL) goto GPSerror1;
if ((strlen(ptr) < 6) || (strlen(ptr) > 10)) goto GPSerror1; // check time length
// strncpy(gpsTime, ptr, 10); // copy time string hhmmss
tmp = parsedecimal(ptr); // parse integer portion
tm.Hour = tmp / 10000;
tm.Minute = (tmp / 100) % 100;
tm.Second = tmp % 100;
ptr = strtok(NULL, ",*\r"); // Status
if (ptr == NULL) goto GPSerror1;
gpsFixStat = ptr[0];
if (gpsFixStat == 'A') { // if data valid, parse time & date
gpsTimeout = 0; // reset gps timeout counter
ptr = strtok(NULL, ",*\r"); // Latitude including fraction
if (ptr == NULL) goto GPSerror1;
// strncpy(gpsLat, ptr, 7); // copy Latitude ddmm.ff
ptr = strtok(NULL, ",*\r"); // Latitude N/S
if (ptr == NULL) goto GPSerror1;
// gpsLatH = ptr[0];
ptr = strtok(NULL, ",*\r"); // Longitude including fraction hhmm.ff
if (ptr == NULL) goto GPSerror1;
// strncpy(gpsLong, ptr, 7);
ptr = strtok(NULL, ",*\r"); // Longitude Hemisphere
if (ptr == NULL) goto GPSerror1;
// gpsLongH = ptr[0];
ptr = strtok(NULL, ",*\r"); // Ground speed 000.5
if (ptr == NULL) goto GPSerror1;
// strncpy(gpsSpeed, ptr, 5);
ptr = strtok(NULL, ",*\r"); // Track angle (course) 054.7
if (ptr == NULL) goto GPSerror1;
// strncpy(gpsCourse, ptr, 5);
ptr = strtok(NULL, ",*\r"); // Date ddmmyy
if (ptr == NULL) goto GPSerror1;
// strncpy(gpsDate, ptr, 6);
if (strlen(ptr) != 6) goto GPSerror1; // check date length
tmp = parsedecimal(ptr);
tm.Day = tmp / 10000;
tm.Month = (tmp / 100) % 100;
tm.Year = tmp % 100;
ptr = strtok(NULL, "*\r"); // magnetic variation & dir
if (ptr == NULL) goto GPSerror1;
if (ptr == NULL) goto GPSerror1;
ptr = strtok(NULL, ",*\r"); // Checksum
if (ptr == NULL) goto GPSerror1;
// strncpy(gpsCKS, ptr, 2); // save checksum chars
tm.Year = y2kYearToTm(tm.Year); // convert yy year to (yyyy-1970) (add 30)
tNow = makeTime(&tm); // convert to time_t
if ((tGPSupdate>0) && (abs(tNow-tGPSupdate)>SECS_PER_DAY)) goto GPSerror2; // GPS time jumped more than 1 day
if ((tm.Second == 0) || ((tNow - tGPSupdate)>=60)) { // update RTC once/minute or if it's been 60 seconds
//beep(1000, 1); // debugging
g_gps_updating = true;
tGPSupdate = tNow; // remember time of this update
tNow = tNow + (long)(g_TZ_hour + g_DST_offset) * SECS_PER_HOUR; // add time zone hour offset & DST offset
if (g_TZ_hour < 0) // add or subtract time zone minute offset
tNow = tNow - (long)g_TZ_minute * SECS_PER_HOUR;
else
tNow = tNow + (long)g_TZ_minute * SECS_PER_HOUR;
setRTCTime(tNow); // set RTC from adjusted GPS time & date
}
else
g_gps_updating = false;
} // if fix status is A
} // if checksums match
else // checksums do not match
g_gps_cks_errors++; // increment error count
return;
GPSerror1:
g_gps_parse_errors++; // increment error count
goto GPSerror2a;
GPSerror2:
g_gps_time_errors++; // increment error count
GPSerror2a:
//beep(2093,1); // error signal - I'm leaving this in for now /wm
//flash_display(200); // flash display to show GPS error
strcpy(gpsBuffer, ""); // wipe GPS buffer
} // if "$GPRMC"
}
