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

nmea_parser.cpp@9:9b2351e25a84, 2012-12-17 (annotated)

Committer:
tylerjw
Date:
Mon Dec 17 22:11:24 2012 +0000
Revision:
9:9b2351e25a84
Parent:
GPS_parser.cpp@8:59acef1c795b
Child:
10:a6e1707fdec0
naming conventions and documentation update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tylerjw 9:9b2351e25a84 1 /*
tylerjw 9:9b2351e25a84 2 * @file nmea_parser.cpp
tylerjw 9:9b2351e25a84 3 * @author Tyler Weaver
tylerjw 9:9b2351e25a84 4 *
tylerjw 9:9b2351e25a84 5 * @section LICENSE
tylerjw 9:9b2351e25a84 6 *
tylerjw 9:9b2351e25a84 7 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
tylerjw 9:9b2351e25a84 8 * and associated documentation files (the "Software"), to deal in the Software without restriction,
tylerjw 9:9b2351e25a84 9 * including without limitation the rights to use, copy, modify, merge, publish, distribute,
tylerjw 9:9b2351e25a84 10 * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
tylerjw 9:9b2351e25a84 11 * furnished to do so, subject to the following conditions:
tylerjw 9:9b2351e25a84 12 *
tylerjw 9:9b2351e25a84 13 * The above copyright notice and this permission notice shall be included in all copies or
tylerjw 9:9b2351e25a84 14 * substantial portions of the Software.
tylerjw 9:9b2351e25a84 15 *
tylerjw 9:9b2351e25a84 16 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
tylerjw 9:9b2351e25a84 17 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
tylerjw 9:9b2351e25a84 18 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
tylerjw 9:9b2351e25a84 19 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
tylerjw 9:9b2351e25a84 20 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
tylerjw 9:9b2351e25a84 21 *
tylerjw 9:9b2351e25a84 22 * @section DESCRIPTION
tylerjw 9:9b2351e25a84 23 *
tylerjw 9:9b2351e25a84 24 * NMEA GPS Serial Output parser.
tylerjw 9:9b2351e25a84 25 * Routine taken from NMEA Software Standard (NMEA 0183)
tylerjw 9:9b2351e25a84 26 * http://www.winsystems.com/software/nmea.pdf
tylerjw 9:9b2351e25a84 27 *
tylerjw 9:9b2351e25a84 28 * Only handles GGA and RMC Messages
tylerjw 9:9b2351e25a84 29 */
tylerjw 9:9b2351e25a84 30
tylerjw 9:9b2351e25a84 31 #include "nmea_parser.h"
tylerjw 5:94daced1e61a 32
tylerjw 9:9b2351e25a84 33 NmeaParser::NmeaParser()
tylerjw 5:94daced1e61a 34 {
tylerjw 9:9b2351e25a84 35 longitude_ = 0.0;
tylerjw 9:9b2351e25a84 36 latitude_ = 0.0;
tylerjw 9:9b2351e25a84 37 utc_time_ = 0;
tylerjw 9:9b2351e25a84 38 lat_reference_ = '\0';
tylerjw 9:9b2351e25a84 39 long_reference_ = '\0';
tylerjw 9:9b2351e25a84 40 quality_ = 0;
tylerjw 9:9b2351e25a84 41 satellite_count_ = 0;
tylerjw 9:9b2351e25a84 42 hdop_ = 0.0;
tylerjw 9:9b2351e25a84 43 msl_altitude_ = 0.0;
tylerjw 9:9b2351e25a84 44 msl_altitude_unit_ = '\0';
tylerjw 5:94daced1e61a 45
tylerjw 9:9b2351e25a84 46 rmc_status_ = '\0';
tylerjw 9:9b2351e25a84 47 speed_ = 0.0;
tylerjw 9:9b2351e25a84 48 track_ = 0.0;
tylerjw 9:9b2351e25a84 49 date_ = 0;
tylerjw 5:94daced1e61a 50
tylerjw 9:9b2351e25a84 51 dec_longitude_ = 0.0;
tylerjw 9:9b2351e25a84 52 dec_latitude_ = 0.0;
tylerjw 5:94daced1e61a 53
tylerjw 9:9b2351e25a84 54 current_ = NULL;
tylerjw 5:94daced1e61a 55 }
tylerjw 5:94daced1e61a 56
tylerjw 9:9b2351e25a84 57 float NmeaParser::nmea_to_dec(float deg_coord, char nsew)
tylerjw 5:94daced1e61a 58 {
tylerjw 9:9b2351e25a84 59 int degree = static_cast<int>(deg_coord/100);
tylerjw 5:94daced1e61a 60 float minutes = deg_coord - degree*100;
tylerjw 5:94daced1e61a 61 float dec_deg = minutes / 60;
tylerjw 5:94daced1e61a 62 float decimal = degree + dec_deg;
tylerjw 5:94daced1e61a 63 if (nsew == 'S' || nsew == 'W') { // return negative
tylerjw 5:94daced1e61a 64 decimal *= -1;
tylerjw 5:94daced1e61a 65 }
tylerjw 5:94daced1e61a 66 return decimal;
tylerjw 5:94daced1e61a 67 }
tylerjw 5:94daced1e61a 68
tylerjw 5:94daced1e61a 69 // GET FUNCTIONS /////////////////////////////////////////////////////////////////
tylerjw 6:4ed12067a314 70
tylerjw 9:9b2351e25a84 71 float NmeaParser::msl_altitude()
tylerjw 5:94daced1e61a 72 {
tylerjw 9:9b2351e25a84 73 if (!quality_)
tylerjw 5:94daced1e61a 74 return 0.0;
tylerjw 5:94daced1e61a 75 else
tylerjw 9:9b2351e25a84 76 return msl_altitude_;
tylerjw 9:9b2351e25a84 77 }
tylerjw 9:9b2351e25a84 78
tylerjw 9:9b2351e25a84 79 int NmeaParser::satellite_count()
tylerjw 9:9b2351e25a84 80 {
tylerjw 9:9b2351e25a84 81 if (!quality_)
tylerjw 9:9b2351e25a84 82 return 0;
tylerjw 9:9b2351e25a84 83 else
tylerjw 9:9b2351e25a84 84 return satellite_count_;
tylerjw 5:94daced1e61a 85 }
tylerjw 5:94daced1e61a 86
tylerjw 9:9b2351e25a84 87 float NmeaParser::longitude()
tylerjw 5:94daced1e61a 88 {
tylerjw 9:9b2351e25a84 89 if (!quality_)
tylerjw 9:9b2351e25a84 90 return 0.0;
tylerjw 5:94daced1e61a 91 else
tylerjw 9:9b2351e25a84 92 return longitude_;
tylerjw 5:94daced1e61a 93 }
tylerjw 5:94daced1e61a 94
tylerjw 9:9b2351e25a84 95 float NmeaParser::calc_dec_longitude()
tylerjw 5:94daced1e61a 96 {
tylerjw 9:9b2351e25a84 97 dec_longitude_ = nmea_to_dec(longitude_, long_reference_);
tylerjw 9:9b2351e25a84 98 if (!quality_)
tylerjw 5:94daced1e61a 99 return 0.0;
tylerjw 5:94daced1e61a 100 else
tylerjw 9:9b2351e25a84 101 return dec_longitude_;
tylerjw 9:9b2351e25a84 102 }
tylerjw 9:9b2351e25a84 103
tylerjw 9:9b2351e25a84 104 float NmeaParser::latitude()
tylerjw 9:9b2351e25a84 105 {
tylerjw 9:9b2351e25a84 106 if (!quality_)
tylerjw 9:9b2351e25a84 107 return 0.0;
tylerjw 9:9b2351e25a84 108 else
tylerjw 9:9b2351e25a84 109 return latitude_;
tylerjw 5:94daced1e61a 110 }
tylerjw 5:94daced1e61a 111
tylerjw 9:9b2351e25a84 112 float NmeaParser::calc_dec_latitude()
tylerjw 5:94daced1e61a 113 {
tylerjw 9:9b2351e25a84 114 dec_latitude_ = nmea_to_dec(latitude_, lat_reference_);
tylerjw 9:9b2351e25a84 115 if (!quality_)
tylerjw 5:94daced1e61a 116 return 0.0;
tylerjw 5:94daced1e61a 117 else
tylerjw 9:9b2351e25a84 118 return dec_latitude_;
tylerjw 5:94daced1e61a 119 }
tylerjw 5:94daced1e61a 120
tylerjw 9:9b2351e25a84 121 float NmeaParser::track()
tylerjw 5:94daced1e61a 122 {
tylerjw 9:9b2351e25a84 123 if (!quality_)
tylerjw 5:94daced1e61a 124 return 0.0;
tylerjw 5:94daced1e61a 125 else
tylerjw 9:9b2351e25a84 126 return track_;
tylerjw 5:94daced1e61a 127 }
tylerjw 5:94daced1e61a 128
tylerjw 9:9b2351e25a84 129 float NmeaParser::speed()
tylerjw 5:94daced1e61a 130 {
tylerjw 9:9b2351e25a84 131 if (!quality_)
tylerjw 5:94daced1e61a 132 return 0.0;
tylerjw 5:94daced1e61a 133 else
tylerjw 9:9b2351e25a84 134 return speed_;
tylerjw 5:94daced1e61a 135 }
tylerjw 5:94daced1e61a 136
tylerjw 9:9b2351e25a84 137 float NmeaParser::calc_altitude_ft()
tylerjw 5:94daced1e61a 138 {
tylerjw 9:9b2351e25a84 139 if (!quality_)
tylerjw 5:94daced1e61a 140 return 0.0;
tylerjw 5:94daced1e61a 141 else
tylerjw 9:9b2351e25a84 142 return 3.280839895*msl_altitude_;
tylerjw 5:94daced1e61a 143 }
tylerjw 5:94daced1e61a 144
tylerjw 9:9b2351e25a84 145 int NmeaParser::quality()
tylerjw 5:94daced1e61a 146 {
tylerjw 9:9b2351e25a84 147 return quality_;
tylerjw 5:94daced1e61a 148 }
tylerjw 5:94daced1e61a 149
tylerjw 9:9b2351e25a84 150 int NmeaParser::date()
tylerjw 5:94daced1e61a 151 {
tylerjw 9:9b2351e25a84 152 return date_;
tylerjw 9:9b2351e25a84 153 }
tylerjw 9:9b2351e25a84 154
tylerjw 9:9b2351e25a84 155 float NmeaParser::utc_time()
tylerjw 9:9b2351e25a84 156 {
tylerjw 9:9b2351e25a84 157 return utc_time_;
tylerjw 5:94daced1e61a 158 }
tylerjw 5:94daced1e61a 159
tylerjw 5:94daced1e61a 160 // NAVIGATION FUNCTIONS ////////////////////////////////////////////////////////////
tylerjw 9:9b2351e25a84 161 float NmeaParser::calc_initial_bearing(float pointLat, float pontLong)
tylerjw 5:94daced1e61a 162 {
tylerjw 5:94daced1e61a 163 const double d2r = PI / 180.0;
tylerjw 5:94daced1e61a 164 const double r2d = 180.0 / PI;
tylerjw 9:9b2351e25a84 165 double dlat = abs(pointLat - calc_dec_latitude()) * d2r;
tylerjw 9:9b2351e25a84 166 double dlong = abs(pontLong - calc_dec_longitude()) * d2r;
tylerjw 5:94daced1e61a 167 double y = sin(dlong) * cos(pointLat * d2r);
tylerjw 9:9b2351e25a84 168 double x = cos(calc_dec_latitude()*d2r)*sin(pointLat*d2r) - sin(calc_dec_latitude()*d2r)*cos(pointLat*d2r)*cos(dlong);
tylerjw 5:94daced1e61a 169 return 360.0-(atan2(y,x)*r2d);
tylerjw 5:94daced1e61a 170 }
tylerjw 5:94daced1e61a 171
tylerjw 5:94daced1e61a 172 /*
tylerjw 5:94daced1e61a 173 var y = Math.sin(dLon) * Math.cos(lat2);
tylerjw 5:94daced1e61a 174 var x = Math.cos(lat1)*Math.sin(lat2) -
tylerjw 5:94daced1e61a 175 Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
tylerjw 5:94daced1e61a 176 var brng = Math.atan2(y, x).toDeg();
tylerjw 5:94daced1e61a 177 */
tylerjw 5:94daced1e61a 178
tylerjw 5:94daced1e61a 179 /*
tylerjw 5:94daced1e61a 180 The Haversine formula according to Dr. Math.
tylerjw 5:94daced1e61a 181 http://mathforum.org/library/drmath/view/51879.html
tylerjw 5:94daced1e61a 182
tylerjw 5:94daced1e61a 183 dlon = lon2 - lon1
tylerjw 5:94daced1e61a 184 dlat = lat2 - lat1
tylerjw 5:94daced1e61a 185 a = (sin(dlat/2))^2 + cos(lat1) * cos(lat2) * (sin(dlon/2))^2
tylerjw 5:94daced1e61a 186 c = 2 * atan2(sqrt(a), sqrt(1-a))
tylerjw 5:94daced1e61a 187 d = R * c
tylerjw 5:94daced1e61a 188
tylerjw 5:94daced1e61a 189 Where
tylerjw 5:94daced1e61a 190 * dlon is the change in longitude
tylerjw 5:94daced1e61a 191 * dlat is the change in latitude
tylerjw 5:94daced1e61a 192 * c is the great circle distance in Radians.
tylerjw 5:94daced1e61a 193 * R is the radius of a spherical Earth.
tylerjw 5:94daced1e61a 194 * The locations of the two points in
tylerjw 5:94daced1e61a 195 spherical coordinates (longitude and
tylerjw 5:94daced1e61a 196 latitude) are lon1,lat1 and lon2, lat2.
tylerjw 5:94daced1e61a 197 */
tylerjw 9:9b2351e25a84 198
tylerjw 9:9b2351e25a84 199 double NmeaParser::calc_dist_to_mi(float pointLat, float pontLong)
tylerjw 5:94daced1e61a 200 {
tylerjw 5:94daced1e61a 201 const double d2r = PI / 180.0;
tylerjw 9:9b2351e25a84 202 double dlat = pointLat - calc_dec_latitude();
tylerjw 9:9b2351e25a84 203 double dlong = pontLong - calc_dec_longitude();
tylerjw 9:9b2351e25a84 204 double a = pow(sin(dlat/2.0),2.0) + cos(calc_dec_latitude()*d2r) * cos(pointLat*d2r) * pow(sin(dlong/2.0),2.0);
tylerjw 5:94daced1e61a 205 double c = 2.0 * asin(sqrt(abs(a)));
tylerjw 5:94daced1e61a 206 double d = 63.765 * c;
tylerjw 5:94daced1e61a 207
tylerjw 5:94daced1e61a 208 return d;
tylerjw 5:94daced1e61a 209 }
tylerjw 5:94daced1e61a 210
tylerjw 9:9b2351e25a84 211 double NmeaParser::calc_dist_to_km(float pointLat, float pontLong)
tylerjw 5:94daced1e61a 212 {
tylerjw 5:94daced1e61a 213 return calc_dist_to_mi(pointLat, pontLong)*1.609344;
tylerjw 5:94daced1e61a 214 }
tylerjw 5:94daced1e61a 215
tylerjw 9:9b2351e25a84 216 char *NmeaParser::my_token(char *source,char token)
tylerjw 7:01a8379370e4 217 {
tylerjw 7:01a8379370e4 218 char *start;
tylerjw 7:01a8379370e4 219 /* The source string is real only for the first call. Subsequent calls
tylerjw 7:01a8379370e4 220 are made with the source string pointer as NULL
tylerjw 7:01a8379370e4 221 */
tylerjw 7:01a8379370e4 222 if(source != NULL) {
tylerjw 7:01a8379370e4 223 /* If the string is empty return NULL */
tylerjw 7:01a8379370e4 224 if(strlen(source) == 0)
tylerjw 7:01a8379370e4 225 return NULL;
tylerjw 9:9b2351e25a84 226 strcpy(stat_string_,source);
tylerjw 7:01a8379370e4 227 /* Current is our 'current' position within the string */
tylerjw 9:9b2351e25a84 228 current_ = stat_string_;
tylerjw 7:01a8379370e4 229 }
tylerjw 9:9b2351e25a84 230 start = current_;
tylerjw 7:01a8379370e4 231
tylerjw 7:01a8379370e4 232 while (true) {
tylerjw 7:01a8379370e4 233 /* If we're at the end of the string, return NULL */
tylerjw 9:9b2351e25a84 234 if((*current_ == '\0') && (current_ == start))
tylerjw 7:01a8379370e4 235 return NULL;
tylerjw 7:01a8379370e4 236 /* If we're at the end now, but weren't when we started, we need
tylerjw 7:01a8379370e4 237 to return the pointer for the last field before the end of string
tylerjw 7:01a8379370e4 238 */
tylerjw 9:9b2351e25a84 239 if(*current_ == '\0')
tylerjw 7:01a8379370e4 240 return start;
tylerjw 7:01a8379370e4 241 /* If we've located our specified token (comma) in the string
tylerjw 7:01a8379370e4 242 load its location in the copy with an end of string marker
tylerjw 7:01a8379370e4 243 so that it can be handled correctly by the calling program.
tylerjw 7:01a8379370e4 244 */
tylerjw 9:9b2351e25a84 245 if(*current_ == token) {
tylerjw 9:9b2351e25a84 246 *current_ = '\0';
tylerjw 9:9b2351e25a84 247 current_++;
tylerjw 7:01a8379370e4 248 return start;
tylerjw 7:01a8379370e4 249 } else {
tylerjw 9:9b2351e25a84 250 current_++;
tylerjw 7:01a8379370e4 251 }
tylerjw 7:01a8379370e4 252 }
tylerjw 7:01a8379370e4 253 }
tylerjw 7:01a8379370e4 254
tylerjw 9:9b2351e25a84 255 int NmeaParser::parse(char *string)
tylerjw 7:01a8379370e4 256 {
tylerjw 7:01a8379370e4 257 int field_count;
tylerjw 7:01a8379370e4 258 field_count = 0;
tylerjw 7:01a8379370e4 259 /* NMEA 0183 fields are delimited by commas. The my_token function returns
tylerjw 7:01a8379370e4 260 pointers to the fields.
tylerjw 7:01a8379370e4 261 */
tylerjw 7:01a8379370e4 262 /* Get the first field pointer */
tylerjw 9:9b2351e25a84 263 field_[0] = my_token(string,',');
tylerjw 7:01a8379370e4 264 field_count++;
tylerjw 7:01a8379370e4 265
tylerjw 7:01a8379370e4 266 while (true) {
tylerjw 7:01a8379370e4 267 /* Contiue retrieving fields until there are no more (NULL) */
tylerjw 9:9b2351e25a84 268 field_[field_count] = my_token(NULL,',');
tylerjw 9:9b2351e25a84 269 if(field_[field_count] == NULL)
tylerjw 7:01a8379370e4 270 break;
tylerjw 7:01a8379370e4 271 field_count++;
tylerjw 7:01a8379370e4 272 }
tylerjw 7:01a8379370e4 273 /* If we got at least ONE field */
tylerjw 7:01a8379370e4 274 if(field_count) {
tylerjw 7:01a8379370e4 275 /* Check the first field for the valid NMEA 0183 headers */
tylerjw 9:9b2351e25a84 276 if(strcmp(field_[0],"$GPGGA") == 0) {
tylerjw 7:01a8379370e4 277 /* Retrieve the values from the remaining fields */
tylerjw 9:9b2351e25a84 278 utc_time_ = atof(field_[1]);
tylerjw 9:9b2351e25a84 279 latitude_ = atof(field_[2]);
tylerjw 9:9b2351e25a84 280 lat_reference_ = *(field_[3]);
tylerjw 9:9b2351e25a84 281 longitude_ = atof(field_[4]);
tylerjw 9:9b2351e25a84 282 long_reference_ = *(field_[5]);
tylerjw 9:9b2351e25a84 283 quality_ = atoi(field_[6]);
tylerjw 9:9b2351e25a84 284 satellite_count_ = atoi(field_[7]);
tylerjw 9:9b2351e25a84 285 hdop_ = atof(field_[8]);
tylerjw 9:9b2351e25a84 286 msl_altitude_ = atof(field_[9]);
tylerjw 9:9b2351e25a84 287 msl_altitude_unit_ = *(field_[10]);
tylerjw 8:59acef1c795b 288 return GGA;
tylerjw 7:01a8379370e4 289 }
tylerjw 9:9b2351e25a84 290 if(strcmp(field_[0],"$GPRMC") == 0) {
tylerjw 7:01a8379370e4 291 /* Retrieve the data from the remaining fields */
tylerjw 9:9b2351e25a84 292 utc_time_ = atof(field_[1]);
tylerjw 9:9b2351e25a84 293 latitude_ = atof(field_[3]);
tylerjw 9:9b2351e25a84 294 lat_reference_ = *(field_[4]);
tylerjw 9:9b2351e25a84 295 longitude_ = atof(field_[5]);
tylerjw 9:9b2351e25a84 296 long_reference_ = *(field_[6]);
tylerjw 9:9b2351e25a84 297 speed_ = atof(field_[7]);
tylerjw 9:9b2351e25a84 298 track_ = atof(field_[8]);
tylerjw 9:9b2351e25a84 299 date_ = atol(field_[9]);
tylerjw 8:59acef1c795b 300 return RMC;
tylerjw 7:01a8379370e4 301 }
tylerjw 7:01a8379370e4 302 }
tylerjw 8:59acef1c795b 303 return NOT_PARSED;
tylerjw 7:01a8379370e4 304 }