nmea gps library - without any serial
Dependents: HARP2 HARP3 20180621_FT813
Fork of GPS_parser by
NMEA GPS Serial Output parser.
Routine taken from NMEA Software Standard (NMEA 0183) http://www.winsystems.com/software/nmea.pdf
Only handles GGA and RMC Messages
GPS_parser.cpp
- Committer:
- tylerjw
- Date:
- 2012-12-13
- Revision:
- 7:01a8379370e4
- Parent:
- 6:4ed12067a314
- Child:
- 8:59acef1c795b
File content as of revision 7:01a8379370e4:
#include "GPS_parser.h" GPS_Parser::GPS_Parser() { nmea_longitude = 0.0; nmea_latitude = 0.0; utc_time = 0; ns = ' '; ew = ' '; lock = 0; satellites = 0; hdop = 0.0; msl_altitude = 0.0; msl_units = ' '; rmc_status = ' '; speed_k = 0.0; course_d = 0.0; date = 0; dec_longitude = 0.0; dec_latitude = 0.0; altitude_ft = 0.0; current = NULL; } float GPS_Parser::nmea_to_dec(float deg_coord, char nsew) { int degree = (int)(deg_coord/100); float minutes = deg_coord - degree*100; float dec_deg = minutes / 60; float decimal = degree + dec_deg; if (nsew == 'S' || nsew == 'W') { // return negative decimal *= -1; } return decimal; } // INTERNAL FUNCTINS //////////////////////////////////////////////////////////// float GPS_Parser::trunc(float v) { if (v < 0.0) { v*= -1.0; v = floor(v); v*=-1.0; } else { v = floor(v); } return v; } // GET FUNCTIONS ///////////////////////////////////////////////////////////////// float GPS_Parser::get_msl_altitude() { if (!lock) return 0.0; else return msl_altitude; } int GPS_Parser::get_satellites() { if (!lock) return 0; else return satellites; } float GPS_Parser::get_nmea_longitude() { if (!lock) return 0.0; else return nmea_longitude; } float GPS_Parser::get_dec_longitude() { dec_longitude = nmea_to_dec(nmea_longitude, ew); if (!lock) return 0.0; else return dec_longitude; } float GPS_Parser::get_nmea_latitude() { if (!lock) return 0.0; else return nmea_latitude; } float GPS_Parser::get_dec_latitude() { dec_latitude = nmea_to_dec(nmea_latitude, ns); if (!lock) return 0.0; else return dec_latitude; } float GPS_Parser::get_course_d() { if (!lock) return 0.0; else return course_d; } float GPS_Parser::get_speed_k() { if (!lock) return 0.0; else return speed_k; } float GPS_Parser::get_altitude_ft() { if (!lock) return 0.0; else return 3.280839895*msl_altitude; } // NAVIGATION FUNCTIONS //////////////////////////////////////////////////////////// float GPS_Parser::calc_course_to(float pointLat, float pontLong) { const double d2r = PI / 180.0; const double r2d = 180.0 / PI; double dlat = abs(pointLat - get_dec_latitude()) * d2r; double dlong = abs(pontLong - get_dec_longitude()) * d2r; double y = sin(dlong) * cos(pointLat * d2r); double x = cos(get_dec_latitude()*d2r)*sin(pointLat*d2r) - sin(get_dec_latitude()*d2r)*cos(pointLat*d2r)*cos(dlong); return 360.0-(atan2(y,x)*r2d); } /* var y = Math.sin(dLon) * Math.cos(lat2); var x = Math.cos(lat1)*Math.sin(lat2) - Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon); var brng = Math.atan2(y, x).toDeg(); */ /* The Haversine formula according to Dr. Math. http://mathforum.org/library/drmath/view/51879.html dlon = lon2 - lon1 dlat = lat2 - lat1 a = (sin(dlat/2))^2 + cos(lat1) * cos(lat2) * (sin(dlon/2))^2 c = 2 * atan2(sqrt(a), sqrt(1-a)) d = R * c Where * dlon is the change in longitude * dlat is the change in latitude * c is the great circle distance in Radians. * R is the radius of a spherical Earth. * The locations of the two points in spherical coordinates (longitude and latitude) are lon1,lat1 and lon2, lat2. */ double GPS_Parser::calc_dist_to_mi(float pointLat, float pontLong) { const double d2r = PI / 180.0; double dlat = pointLat - get_dec_latitude(); double dlong = pontLong - get_dec_longitude(); double a = pow(sin(dlat/2.0),2.0) + cos(get_dec_latitude()*d2r) * cos(pointLat*d2r) * pow(sin(dlong/2.0),2.0); double c = 2.0 * asin(sqrt(abs(a))); double d = 63.765 * c; return d; } double GPS_Parser::calc_dist_to_ft(float pointLat, float pontLong) { return calc_dist_to_mi(pointLat, pontLong)*5280.0; } double GPS_Parser::calc_dist_to_km(float pointLat, float pontLong) { return calc_dist_to_mi(pointLat, pontLong)*1.609344; } double GPS_Parser::calc_dist_to_m(float pointLat, float pontLong) { return calc_dist_to_mi(pointLat, pontLong)*1609.344; } char *GPS_Parser::my_token(char *source,char token) { char *start; /* The source string is real only for the first call. Subsequent calls are made with the source string pointer as NULL */ if(source != NULL) { /* If the string is empty return NULL */ if(strlen(source) == 0) return NULL; strcpy(stat_string,source); /* Current is our 'current' position within the string */ current = stat_string; } start = current; while (true) { /* If we're at the end of the string, return NULL */ if((*current == '\0') && (current == start)) return NULL; /* If we're at the end now, but weren't when we started, we need to return the pointer for the last field before the end of string */ if(*current == '\0') return start; /* If we've located our specified token (comma) in the string load its location in the copy with an end of string marker so that it can be handled correctly by the calling program. */ if(*current == token) { *current = '\0'; current++; return start; } else { current++; } } } int GPS_Parser::parse(char *string) { int field_count; field_count = 0; /* NMEA 0183 fields are delimited by commas. The my_token function returns pointers to the fields. */ /* Get the first field pointer */ field[0] = my_token(string,','); field_count++; while (true) { /* Contiue retrieving fields until there are no more (NULL) */ field[field_count] = my_token(NULL,','); if(field[field_count] == NULL) break; field_count++; } /* If we got at least ONE field */ if(field_count) { /* Check the first field for the valid NMEA 0183 headers */ if(strcmp(field[0],"$GPGGA") == 0) { /* Retrieve the values from the remaining fields */ utc_time = atof(field[1]); nmea_latitude = atof(field[2]); ns = *(field[3]); nmea_longitude = atof(field[4]); ew = *(field[5]); lock = atoi(field[6]); satellites = atoi(field[7]); hdop = atof(field[8]); msl_altitude = atof(field[9]); msl_units = *(field[10]); } if(strcmp(field[0],"$GPRMC") == 0) { /* Retrieve the data from the remaining fields */ utc_time = atof(field[1]); nmea_latitude = atof(field[3]); ns = *(field[4]); nmea_longitude = atof(field[5]); ew = *(field[6]); speed_k = atof(field[7]); course_d = atof(field[8]); date = atol(field[9]); } } return field_count; }