Tyler Weaver / nmea_parser

nmea gps library - without any serial

Dependents:   HARP2 HARP3 20180621_FT813

Fork of GPS_parser by Tyler Weaver

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@7:01a8379370e4, 2012-12-13 (annotated)

Committer:
tylerjw
Date:
Thu Dec 13 04:57:10 2012 +0000
Revision:
7:01a8379370e4
Parent:
6:4ed12067a314
Child:
8:59acef1c795b
tokenizer replaces sscanf :)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tylerjw 5:94daced1e61a 1 #include "GPS_parser.h"
tylerjw 5:94daced1e61a 2
tylerjw 5:94daced1e61a 3 GPS_Parser::GPS_Parser()
tylerjw 5:94daced1e61a 4 {
tylerjw 5:94daced1e61a 5 nmea_longitude = 0.0;
tylerjw 5:94daced1e61a 6 nmea_latitude = 0.0;
tylerjw 5:94daced1e61a 7 utc_time = 0;
tylerjw 5:94daced1e61a 8 ns = ' ';
tylerjw 5:94daced1e61a 9 ew = ' ';
tylerjw 5:94daced1e61a 10 lock = 0;
tylerjw 7:01a8379370e4 11 satellites = 0;
tylerjw 5:94daced1e61a 12 hdop = 0.0;
tylerjw 5:94daced1e61a 13 msl_altitude = 0.0;
tylerjw 5:94daced1e61a 14 msl_units = ' ';
tylerjw 5:94daced1e61a 15
tylerjw 5:94daced1e61a 16 rmc_status = ' ';
tylerjw 5:94daced1e61a 17 speed_k = 0.0;
tylerjw 5:94daced1e61a 18 course_d = 0.0;
tylerjw 5:94daced1e61a 19 date = 0;
tylerjw 5:94daced1e61a 20
tylerjw 5:94daced1e61a 21 dec_longitude = 0.0;
tylerjw 5:94daced1e61a 22 dec_latitude = 0.0;
tylerjw 5:94daced1e61a 23
tylerjw 7:01a8379370e4 24 altitude_ft = 0.0;
tylerjw 5:94daced1e61a 25
tylerjw 7:01a8379370e4 26 current = NULL;
tylerjw 5:94daced1e61a 27 }
tylerjw 5:94daced1e61a 28
tylerjw 5:94daced1e61a 29 float GPS_Parser::nmea_to_dec(float deg_coord, char nsew)
tylerjw 5:94daced1e61a 30 {
tylerjw 5:94daced1e61a 31 int degree = (int)(deg_coord/100);
tylerjw 5:94daced1e61a 32 float minutes = deg_coord - degree*100;
tylerjw 5:94daced1e61a 33 float dec_deg = minutes / 60;
tylerjw 5:94daced1e61a 34 float decimal = degree + dec_deg;
tylerjw 5:94daced1e61a 35 if (nsew == 'S' || nsew == 'W') { // return negative
tylerjw 5:94daced1e61a 36 decimal *= -1;
tylerjw 5:94daced1e61a 37 }
tylerjw 5:94daced1e61a 38 return decimal;
tylerjw 5:94daced1e61a 39 }
tylerjw 5:94daced1e61a 40
tylerjw 5:94daced1e61a 41
tylerjw 5:94daced1e61a 42 // INTERNAL FUNCTINS ////////////////////////////////////////////////////////////
tylerjw 5:94daced1e61a 43 float GPS_Parser::trunc(float v)
tylerjw 5:94daced1e61a 44 {
tylerjw 5:94daced1e61a 45 if (v < 0.0) {
tylerjw 5:94daced1e61a 46 v*= -1.0;
tylerjw 5:94daced1e61a 47 v = floor(v);
tylerjw 5:94daced1e61a 48 v*=-1.0;
tylerjw 5:94daced1e61a 49 } else {
tylerjw 5:94daced1e61a 50 v = floor(v);
tylerjw 5:94daced1e61a 51 }
tylerjw 5:94daced1e61a 52 return v;
tylerjw 5:94daced1e61a 53 }
tylerjw 5:94daced1e61a 54
tylerjw 5:94daced1e61a 55 // GET FUNCTIONS /////////////////////////////////////////////////////////////////
tylerjw 6:4ed12067a314 56
tylerjw 5:94daced1e61a 57 float GPS_Parser::get_msl_altitude()
tylerjw 5:94daced1e61a 58 {
tylerjw 5:94daced1e61a 59 if (!lock)
tylerjw 5:94daced1e61a 60 return 0.0;
tylerjw 5:94daced1e61a 61 else
tylerjw 5:94daced1e61a 62 return msl_altitude;
tylerjw 5:94daced1e61a 63 }
tylerjw 5:94daced1e61a 64
tylerjw 7:01a8379370e4 65 int GPS_Parser::get_satellites()
tylerjw 5:94daced1e61a 66 {
tylerjw 5:94daced1e61a 67 if (!lock)
tylerjw 5:94daced1e61a 68 return 0;
tylerjw 5:94daced1e61a 69 else
tylerjw 7:01a8379370e4 70 return satellites;
tylerjw 5:94daced1e61a 71 }
tylerjw 5:94daced1e61a 72
tylerjw 5:94daced1e61a 73 float GPS_Parser::get_nmea_longitude()
tylerjw 5:94daced1e61a 74 {
tylerjw 5:94daced1e61a 75 if (!lock)
tylerjw 5:94daced1e61a 76 return 0.0;
tylerjw 5:94daced1e61a 77 else
tylerjw 5:94daced1e61a 78 return nmea_longitude;
tylerjw 5:94daced1e61a 79 }
tylerjw 5:94daced1e61a 80
tylerjw 5:94daced1e61a 81 float GPS_Parser::get_dec_longitude()
tylerjw 5:94daced1e61a 82 {
tylerjw 5:94daced1e61a 83 dec_longitude = nmea_to_dec(nmea_longitude, ew);
tylerjw 5:94daced1e61a 84 if (!lock)
tylerjw 5:94daced1e61a 85 return 0.0;
tylerjw 5:94daced1e61a 86 else
tylerjw 5:94daced1e61a 87 return dec_longitude;
tylerjw 5:94daced1e61a 88 }
tylerjw 5:94daced1e61a 89
tylerjw 5:94daced1e61a 90 float GPS_Parser::get_nmea_latitude()
tylerjw 5:94daced1e61a 91 {
tylerjw 5:94daced1e61a 92 if (!lock)
tylerjw 5:94daced1e61a 93 return 0.0;
tylerjw 5:94daced1e61a 94 else
tylerjw 5:94daced1e61a 95 return nmea_latitude;
tylerjw 5:94daced1e61a 96 }
tylerjw 5:94daced1e61a 97
tylerjw 5:94daced1e61a 98 float GPS_Parser::get_dec_latitude()
tylerjw 5:94daced1e61a 99 {
tylerjw 5:94daced1e61a 100 dec_latitude = nmea_to_dec(nmea_latitude, ns);
tylerjw 5:94daced1e61a 101 if (!lock)
tylerjw 5:94daced1e61a 102 return 0.0;
tylerjw 5:94daced1e61a 103 else
tylerjw 5:94daced1e61a 104 return dec_latitude;
tylerjw 5:94daced1e61a 105 }
tylerjw 5:94daced1e61a 106
tylerjw 7:01a8379370e4 107 float GPS_Parser::get_course_d()
tylerjw 5:94daced1e61a 108 {
tylerjw 5:94daced1e61a 109 if (!lock)
tylerjw 5:94daced1e61a 110 return 0.0;
tylerjw 5:94daced1e61a 111 else
tylerjw 7:01a8379370e4 112 return course_d;
tylerjw 5:94daced1e61a 113 }
tylerjw 5:94daced1e61a 114
tylerjw 5:94daced1e61a 115 float GPS_Parser::get_speed_k()
tylerjw 5:94daced1e61a 116 {
tylerjw 5:94daced1e61a 117 if (!lock)
tylerjw 5:94daced1e61a 118 return 0.0;
tylerjw 5:94daced1e61a 119 else
tylerjw 5:94daced1e61a 120 return speed_k;
tylerjw 5:94daced1e61a 121 }
tylerjw 5:94daced1e61a 122
tylerjw 5:94daced1e61a 123 float GPS_Parser::get_altitude_ft()
tylerjw 5:94daced1e61a 124 {
tylerjw 5:94daced1e61a 125 if (!lock)
tylerjw 5:94daced1e61a 126 return 0.0;
tylerjw 5:94daced1e61a 127 else
tylerjw 5:94daced1e61a 128 return 3.280839895*msl_altitude;
tylerjw 5:94daced1e61a 129 }
tylerjw 5:94daced1e61a 130
tylerjw 5:94daced1e61a 131 // NAVIGATION FUNCTIONS ////////////////////////////////////////////////////////////
tylerjw 5:94daced1e61a 132 float GPS_Parser::calc_course_to(float pointLat, float pontLong)
tylerjw 5:94daced1e61a 133 {
tylerjw 5:94daced1e61a 134 const double d2r = PI / 180.0;
tylerjw 5:94daced1e61a 135 const double r2d = 180.0 / PI;
tylerjw 5:94daced1e61a 136 double dlat = abs(pointLat - get_dec_latitude()) * d2r;
tylerjw 5:94daced1e61a 137 double dlong = abs(pontLong - get_dec_longitude()) * d2r;
tylerjw 5:94daced1e61a 138 double y = sin(dlong) * cos(pointLat * d2r);
tylerjw 5:94daced1e61a 139 double x = cos(get_dec_latitude()*d2r)*sin(pointLat*d2r) - sin(get_dec_latitude()*d2r)*cos(pointLat*d2r)*cos(dlong);
tylerjw 5:94daced1e61a 140 return 360.0-(atan2(y,x)*r2d);
tylerjw 5:94daced1e61a 141 }
tylerjw 5:94daced1e61a 142
tylerjw 5:94daced1e61a 143 /*
tylerjw 5:94daced1e61a 144 var y = Math.sin(dLon) * Math.cos(lat2);
tylerjw 5:94daced1e61a 145 var x = Math.cos(lat1)*Math.sin(lat2) -
tylerjw 5:94daced1e61a 146 Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
tylerjw 5:94daced1e61a 147 var brng = Math.atan2(y, x).toDeg();
tylerjw 5:94daced1e61a 148 */
tylerjw 5:94daced1e61a 149
tylerjw 5:94daced1e61a 150 /*
tylerjw 5:94daced1e61a 151 The Haversine formula according to Dr. Math.
tylerjw 5:94daced1e61a 152 http://mathforum.org/library/drmath/view/51879.html
tylerjw 5:94daced1e61a 153
tylerjw 5:94daced1e61a 154 dlon = lon2 - lon1
tylerjw 5:94daced1e61a 155 dlat = lat2 - lat1
tylerjw 5:94daced1e61a 156 a = (sin(dlat/2))^2 + cos(lat1) * cos(lat2) * (sin(dlon/2))^2
tylerjw 5:94daced1e61a 157 c = 2 * atan2(sqrt(a), sqrt(1-a))
tylerjw 5:94daced1e61a 158 d = R * c
tylerjw 5:94daced1e61a 159
tylerjw 5:94daced1e61a 160 Where
tylerjw 5:94daced1e61a 161 * dlon is the change in longitude
tylerjw 5:94daced1e61a 162 * dlat is the change in latitude
tylerjw 5:94daced1e61a 163 * c is the great circle distance in Radians.
tylerjw 5:94daced1e61a 164 * R is the radius of a spherical Earth.
tylerjw 5:94daced1e61a 165 * The locations of the two points in
tylerjw 5:94daced1e61a 166 spherical coordinates (longitude and
tylerjw 5:94daced1e61a 167 latitude) are lon1,lat1 and lon2, lat2.
tylerjw 5:94daced1e61a 168 */
tylerjw 5:94daced1e61a 169 double GPS_Parser::calc_dist_to_mi(float pointLat, float pontLong)
tylerjw 5:94daced1e61a 170 {
tylerjw 5:94daced1e61a 171 const double d2r = PI / 180.0;
tylerjw 5:94daced1e61a 172 double dlat = pointLat - get_dec_latitude();
tylerjw 5:94daced1e61a 173 double dlong = pontLong - get_dec_longitude();
tylerjw 5:94daced1e61a 174 double a = pow(sin(dlat/2.0),2.0) + cos(get_dec_latitude()*d2r) * cos(pointLat*d2r) * pow(sin(dlong/2.0),2.0);
tylerjw 5:94daced1e61a 175 double c = 2.0 * asin(sqrt(abs(a)));
tylerjw 5:94daced1e61a 176 double d = 63.765 * c;
tylerjw 5:94daced1e61a 177
tylerjw 5:94daced1e61a 178 return d;
tylerjw 5:94daced1e61a 179 }
tylerjw 5:94daced1e61a 180
tylerjw 5:94daced1e61a 181 double GPS_Parser::calc_dist_to_ft(float pointLat, float pontLong)
tylerjw 5:94daced1e61a 182 {
tylerjw 5:94daced1e61a 183 return calc_dist_to_mi(pointLat, pontLong)*5280.0;
tylerjw 5:94daced1e61a 184 }
tylerjw 5:94daced1e61a 185
tylerjw 5:94daced1e61a 186 double GPS_Parser::calc_dist_to_km(float pointLat, float pontLong)
tylerjw 5:94daced1e61a 187 {
tylerjw 5:94daced1e61a 188 return calc_dist_to_mi(pointLat, pontLong)*1.609344;
tylerjw 5:94daced1e61a 189 }
tylerjw 5:94daced1e61a 190
tylerjw 5:94daced1e61a 191 double GPS_Parser::calc_dist_to_m(float pointLat, float pontLong)
tylerjw 5:94daced1e61a 192 {
tylerjw 5:94daced1e61a 193 return calc_dist_to_mi(pointLat, pontLong)*1609.344;
tylerjw 5:94daced1e61a 194 }
tylerjw 5:94daced1e61a 195
tylerjw 7:01a8379370e4 196 char *GPS_Parser::my_token(char *source,char token)
tylerjw 7:01a8379370e4 197 {
tylerjw 7:01a8379370e4 198 char *start;
tylerjw 7:01a8379370e4 199 /* The source string is real only for the first call. Subsequent calls
tylerjw 7:01a8379370e4 200 are made with the source string pointer as NULL
tylerjw 7:01a8379370e4 201 */
tylerjw 7:01a8379370e4 202 if(source != NULL) {
tylerjw 7:01a8379370e4 203 /* If the string is empty return NULL */
tylerjw 7:01a8379370e4 204 if(strlen(source) == 0)
tylerjw 7:01a8379370e4 205 return NULL;
tylerjw 7:01a8379370e4 206 strcpy(stat_string,source);
tylerjw 7:01a8379370e4 207 /* Current is our 'current' position within the string */
tylerjw 7:01a8379370e4 208 current = stat_string;
tylerjw 7:01a8379370e4 209 }
tylerjw 7:01a8379370e4 210 start = current;
tylerjw 7:01a8379370e4 211
tylerjw 7:01a8379370e4 212 while (true) {
tylerjw 7:01a8379370e4 213 /* If we're at the end of the string, return NULL */
tylerjw 7:01a8379370e4 214 if((*current == '\0') && (current == start))
tylerjw 7:01a8379370e4 215 return NULL;
tylerjw 7:01a8379370e4 216 /* If we're at the end now, but weren't when we started, we need
tylerjw 7:01a8379370e4 217 to return the pointer for the last field before the end of string
tylerjw 7:01a8379370e4 218 */
tylerjw 7:01a8379370e4 219 if(*current == '\0')
tylerjw 7:01a8379370e4 220 return start;
tylerjw 7:01a8379370e4 221 /* If we've located our specified token (comma) in the string
tylerjw 7:01a8379370e4 222 load its location in the copy with an end of string marker
tylerjw 7:01a8379370e4 223 so that it can be handled correctly by the calling program.
tylerjw 7:01a8379370e4 224 */
tylerjw 7:01a8379370e4 225 if(*current == token) {
tylerjw 7:01a8379370e4 226 *current = '\0';
tylerjw 7:01a8379370e4 227 current++;
tylerjw 7:01a8379370e4 228 return start;
tylerjw 7:01a8379370e4 229 } else {
tylerjw 7:01a8379370e4 230 current++;
tylerjw 7:01a8379370e4 231 }
tylerjw 7:01a8379370e4 232 }
tylerjw 7:01a8379370e4 233 }
tylerjw 7:01a8379370e4 234
tylerjw 7:01a8379370e4 235 int GPS_Parser::parse(char *string)
tylerjw 7:01a8379370e4 236 {
tylerjw 7:01a8379370e4 237 int field_count;
tylerjw 7:01a8379370e4 238 field_count = 0;
tylerjw 7:01a8379370e4 239 /* NMEA 0183 fields are delimited by commas. The my_token function returns
tylerjw 7:01a8379370e4 240 pointers to the fields.
tylerjw 7:01a8379370e4 241 */
tylerjw 7:01a8379370e4 242 /* Get the first field pointer */
tylerjw 7:01a8379370e4 243 field[0] = my_token(string,',');
tylerjw 7:01a8379370e4 244 field_count++;
tylerjw 7:01a8379370e4 245
tylerjw 7:01a8379370e4 246 while (true) {
tylerjw 7:01a8379370e4 247 /* Contiue retrieving fields until there are no more (NULL) */
tylerjw 7:01a8379370e4 248 field[field_count] = my_token(NULL,',');
tylerjw 7:01a8379370e4 249 if(field[field_count] == NULL)
tylerjw 7:01a8379370e4 250 break;
tylerjw 7:01a8379370e4 251 field_count++;
tylerjw 7:01a8379370e4 252 }
tylerjw 7:01a8379370e4 253 /* If we got at least ONE field */
tylerjw 7:01a8379370e4 254 if(field_count) {
tylerjw 7:01a8379370e4 255 /* Check the first field for the valid NMEA 0183 headers */
tylerjw 7:01a8379370e4 256 if(strcmp(field[0],"$GPGGA") == 0) {
tylerjw 7:01a8379370e4 257 /* Retrieve the values from the remaining fields */
tylerjw 7:01a8379370e4 258 utc_time = atof(field[1]);
tylerjw 7:01a8379370e4 259 nmea_latitude = atof(field[2]);
tylerjw 7:01a8379370e4 260 ns = *(field[3]);
tylerjw 7:01a8379370e4 261 nmea_longitude = atof(field[4]);
tylerjw 7:01a8379370e4 262 ew = *(field[5]);
tylerjw 7:01a8379370e4 263 lock = atoi(field[6]);
tylerjw 7:01a8379370e4 264 satellites = atoi(field[7]);
tylerjw 7:01a8379370e4 265 hdop = atof(field[8]);
tylerjw 7:01a8379370e4 266 msl_altitude = atof(field[9]);
tylerjw 7:01a8379370e4 267 msl_units = *(field[10]);
tylerjw 7:01a8379370e4 268 }
tylerjw 7:01a8379370e4 269 if(strcmp(field[0],"$GPRMC") == 0) {
tylerjw 7:01a8379370e4 270 /* Retrieve the data from the remaining fields */
tylerjw 7:01a8379370e4 271 utc_time = atof(field[1]);
tylerjw 7:01a8379370e4 272 nmea_latitude = atof(field[3]);
tylerjw 7:01a8379370e4 273 ns = *(field[4]);
tylerjw 7:01a8379370e4 274 nmea_longitude = atof(field[5]);
tylerjw 7:01a8379370e4 275 ew = *(field[6]);
tylerjw 7:01a8379370e4 276 speed_k = atof(field[7]);
tylerjw 7:01a8379370e4 277 course_d = atof(field[8]);
tylerjw 7:01a8379370e4 278 date = atol(field[9]);
tylerjw 7:01a8379370e4 279 }
tylerjw 7:01a8379370e4 280 }
tylerjw 7:01a8379370e4 281 return field_count;
tylerjw 7:01a8379370e4 282 }