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
Revision 9:9b2351e25a84, committed 2012-12-17
- Comitter:
- tylerjw
- Date:
- Mon Dec 17 22:11:24 2012 +0000
- Parent:
- 8:59acef1c795b
- Child:
- 10:a6e1707fdec0
- Commit message:
- naming conventions and documentation update
Changed in this revision
--- a/GPS_parser.cpp Thu Dec 13 05:52:43 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,284 +0,0 @@ -#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]); - return GGA; - } - 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 RMC; - } - } - return NOT_PARSED; -} \ No newline at end of file
--- a/GPS_parser.h Thu Dec 13 05:52:43 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,87 +0,0 @@ -#include "mbed.h" - -#ifndef MBED_GPS_PARSER_H -#define MBED_GPS_PARSER_H - -#define NO_LOCK 1 -#define NOT_PARSED 2 -#define GGA 3 -#define GLL 4 -#define RMC 5 -#define VTG 6 - -#define PI (3.141592653589793) - -/** A GPS_parser interface for reading from a Globalsat EM-406 GPS Module */ -class GPS_Parser -{ -public: - - GPS_Parser(); - - /** Parse the incoming GPS data, returning whether there is a lock - * - * @param line the nmea string to parse, uses tokenizer vs sscanf - * @return 1 if there was a lock when the sample was taken (and therefore .longitude and .latitude are valid), else 0 - */ - int parse(char *); - int get_lock() { - return lock; - } - int get_date() { - return date; - } - float get_time() { - return utc_time; - } - float get_nmea_longitude(); - float get_nmea_latitude(); - float get_dec_longitude(); - float get_dec_latitude(); - float get_msl_altitude(); - float get_course_d(); - float get_speed_k(); - int get_satellites(); - float get_altitude_ft(); - - // navigational functions - float calc_course_to(float, float); - double calc_dist_to_mi(float, float); - double calc_dist_to_ft(float, float); - double calc_dist_to_km(float, float); - double calc_dist_to_m(float, float); - -private: - float nmea_to_dec(float, char); - float trunc(float v); - char *my_token(char *,char); - - char stat_string[128]; // used in my_token - char *current; - - char *field[50]; // used by parse nmea - - // calculated values - volatile float dec_longitude; - volatile float dec_latitude; - volatile float altitude_ft; - - // GGA - Global Positioning System Fixed Data - volatile float nmea_longitude; - volatile float nmea_latitude; - volatile float utc_time; - volatile char ns, ew; - volatile int lock; - volatile int satellites; - volatile float hdop; - volatile float msl_altitude; - volatile char msl_units; - - // RMC - Recommended Minimmum Specific GNS Data - volatile char rmc_status; - volatile float speed_k; - volatile float course_d; - volatile int date; -}; - -#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/nmea_parser.cpp Mon Dec 17 22:11:24 2012 +0000 @@ -0,0 +1,304 @@ +/* + * @file nmea_parser.cpp + * @author Tyler Weaver + * + * @section LICENSE + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of this software + * and associated documentation files (the "Software"), to deal in the Software without restriction, + * including without limitation the rights to use, copy, modify, merge, publish, distribute, + * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all copies or + * substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING + * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, + * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * + * @section DESCRIPTION + * + * 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 + */ + +#include "nmea_parser.h" + +NmeaParser::NmeaParser() +{ + longitude_ = 0.0; + latitude_ = 0.0; + utc_time_ = 0; + lat_reference_ = '\0'; + long_reference_ = '\0'; + quality_ = 0; + satellite_count_ = 0; + hdop_ = 0.0; + msl_altitude_ = 0.0; + msl_altitude_unit_ = '\0'; + + rmc_status_ = '\0'; + speed_ = 0.0; + track_ = 0.0; + date_ = 0; + + dec_longitude_ = 0.0; + dec_latitude_ = 0.0; + + current_ = NULL; +} + +float NmeaParser::nmea_to_dec(float deg_coord, char nsew) +{ + int degree = static_cast<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; +} + +// GET FUNCTIONS ///////////////////////////////////////////////////////////////// + +float NmeaParser::msl_altitude() +{ + if (!quality_) + return 0.0; + else + return msl_altitude_; +} + +int NmeaParser::satellite_count() +{ + if (!quality_) + return 0; + else + return satellite_count_; +} + +float NmeaParser::longitude() +{ + if (!quality_) + return 0.0; + else + return longitude_; +} + +float NmeaParser::calc_dec_longitude() +{ + dec_longitude_ = nmea_to_dec(longitude_, long_reference_); + if (!quality_) + return 0.0; + else + return dec_longitude_; +} + +float NmeaParser::latitude() +{ + if (!quality_) + return 0.0; + else + return latitude_; +} + +float NmeaParser::calc_dec_latitude() +{ + dec_latitude_ = nmea_to_dec(latitude_, lat_reference_); + if (!quality_) + return 0.0; + else + return dec_latitude_; +} + +float NmeaParser::track() +{ + if (!quality_) + return 0.0; + else + return track_; +} + +float NmeaParser::speed() +{ + if (!quality_) + return 0.0; + else + return speed_; +} + +float NmeaParser::calc_altitude_ft() +{ + if (!quality_) + return 0.0; + else + return 3.280839895*msl_altitude_; +} + +int NmeaParser::quality() +{ + return quality_; +} + +int NmeaParser::date() +{ + return date_; +} + +float NmeaParser::utc_time() +{ + return utc_time_; +} + +// NAVIGATION FUNCTIONS //////////////////////////////////////////////////////////// +float NmeaParser::calc_initial_bearing(float pointLat, float pontLong) +{ + const double d2r = PI / 180.0; + const double r2d = 180.0 / PI; + double dlat = abs(pointLat - calc_dec_latitude()) * d2r; + double dlong = abs(pontLong - calc_dec_longitude()) * d2r; + double y = sin(dlong) * cos(pointLat * d2r); + double x = cos(calc_dec_latitude()*d2r)*sin(pointLat*d2r) - sin(calc_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 NmeaParser::calc_dist_to_mi(float pointLat, float pontLong) +{ + const double d2r = PI / 180.0; + double dlat = pointLat - calc_dec_latitude(); + double dlong = pontLong - calc_dec_longitude(); + double a = pow(sin(dlat/2.0),2.0) + cos(calc_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 NmeaParser::calc_dist_to_km(float pointLat, float pontLong) +{ + return calc_dist_to_mi(pointLat, pontLong)*1.609344; +} + +char *NmeaParser::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 NmeaParser::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]); + latitude_ = atof(field_[2]); + lat_reference_ = *(field_[3]); + longitude_ = atof(field_[4]); + long_reference_ = *(field_[5]); + quality_ = atoi(field_[6]); + satellite_count_ = atoi(field_[7]); + hdop_ = atof(field_[8]); + msl_altitude_ = atof(field_[9]); + msl_altitude_unit_ = *(field_[10]); + return GGA; + } + if(strcmp(field_[0],"$GPRMC") == 0) { + /* Retrieve the data from the remaining fields */ + utc_time_ = atof(field_[1]); + latitude_ = atof(field_[3]); + lat_reference_ = *(field_[4]); + longitude_ = atof(field_[5]); + long_reference_ = *(field_[6]); + speed_ = atof(field_[7]); + track_ = atof(field_[8]); + date_ = atol(field_[9]); + return RMC; + } + } + return NOT_PARSED; +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/nmea_parser.h Mon Dec 17 22:11:24 2012 +0000 @@ -0,0 +1,176 @@ +/* + * @file nmea_parser.h + * @author Tyler Weaver + * + * @section LICENSE + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of this software + * and associated documentation files (the "Software"), to deal in the Software without restriction, + * including without limitation the rights to use, copy, modify, merge, publish, distribute, + * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all copies or + * substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING + * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, + * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * + * @section DESCRIPTION + * + * 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 + */ + +#include "mbed.h" + +#ifndef MBED_NMEA_PARSER_H +#define MBED_NMEA_PARSER_H + +#define NO_LOCK 1 +#define NOT_PARSED 2 +#define GGA 3 +#define GLL 4 +#define RMC 5 +#define VTG 6 + +#define PI (3.141592653589793) + +/** A NmeaParser interface for parsing NMEA gps serial output */ +class NmeaParser +{ +public: + + /** + * Default Constructor + * Initalizes variables + */ + NmeaParser(); + + /** Parse the incoming GPS data, returning whether there is a lock + * + * Routine from NMEA Software Standard (NMEA 0183) + * + * @param line the nmea string to parse, uses tokenizer vs sscanf + * @return 1 if there was a lock when the sample was taken (and therefore .longitude and .latitude are valid), else 0 + */ + int parse(char *); + + /** + * @returns quality of signal (0 = No GPS, 1 = GPS, 2 = DGPS) + */ + int quality(); + /** + * From RMC message + * @returns date in ddmmyy format + */ + int date(); + /** + * @returns time in utc format hhmmss.ss format + */ + float utc_time(); + /** + * @returns Longitude in NMEA format dddmm.mm + */ + float longitude(); + /** + * @returns Latitude in NMEA format dddmm.mm + */ + float latitude(); + /** + * @returns Altitude Mean Sea Level (MSL) in Meters + */ + float msl_altitude(); + /** + * @returns track (heading) made good in degrees true. + */ + float track(); + /** + * @returns speed over ground in knots + */ + float speed(); + /** + * @returns number of satellites in use + */ + int satellite_count(); + + /** + * @returns longitude in decimal form (calculated) + */ + float calc_dec_longitude(); + /** + * @returns longitude in decimal form (calculated) + */ + float calc_dec_latitude(); + /** + * @returns altitude MSL in feet (calculated) + */ + float calc_altitude_ft(); + /** + * Initial bearing is the angle to fly at for the shortest flight between two points on a sphere. + * Calculations from: http://www.movable-type.co.uk/scripts/latlong.html + * + * Uses current position and calculates to the inputed point. + * + * @param latitude of target point + * @param longitude of target point + * @returns initial bearing for flying to given point + */ + float calc_initial_bearing(float, float); + /** + * Uses the shortest distance across a sphere (haversine formula) to calculate distance + * + * Uses current position and calculates to the inputed point. + * + * @param latitude of target point + * @param longitude of target point + * @returns distance in miles to given point + */ + double calc_dist_to_mi(float, float); + /** + * Uses the calc_dist_to_mi then converts to kilometers. + * + * @param latitude of target point + * @param longitude of target point + * @returns distance in kilometers to given point + */ + double calc_dist_to_km(float, float); +private: + float nmea_to_dec(float, char); + char *my_token(char *,char); + + char stat_string_[128]; // used in my_token + char *current_; + + char *field_[50]; // used by parse nmea + + // calculated values + volatile float dec_longitude_; + volatile float dec_latitude_; + + // GGA - Global Positioning System Fixed Data + volatile float utc_time_; + volatile float latitude_; + volatile char lat_reference_; + volatile float longitude_; + volatile char long_reference_; + volatile int quality_; + volatile int satellite_count_; + volatile float hdop_; + volatile float msl_altitude_; + volatile char msl_altitude_unit_; + + // RMC - Recommended Minimmum Specific GNS Data + volatile char rmc_status_; + volatile float speed_; + volatile float track_; + volatile int date_; +}; + +#endif