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@10:a6e1707fdec0, 2012-12-17 (annotated)

Committer:
tylerjw
Date:
Mon Dec 17 23:42:13 2012 +0000
Revision:
10:a6e1707fdec0
Parent:
9:9b2351e25a84 
publish

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 10:a6e1707fdec0 165 double calc_latitude = calc_dec_latitude();
tylerjw 10:a6e1707fdec0 166 double calc_longitude = calc_dec_longitude();
tylerjw 10:a6e1707fdec0 167
tylerjw 10:a6e1707fdec0 168 double dlat = abs(pointLat - calc_latitude) * d2r;
tylerjw 10:a6e1707fdec0 169 double dlong = abs(pontLong - calc_longitude) * d2r;
tylerjw 5:94daced1e61a 170 double y = sin(dlong) * cos(pointLat * d2r);
tylerjw 10:a6e1707fdec0 171 double x = cos(calc_latitude*d2r)*sin(pointLat*d2r) - sin(calc_latitude*d2r)*cos(pointLat*d2r)*cos(dlong);
tylerjw 5:94daced1e61a 172 return 360.0-(atan2(y,x)*r2d);
tylerjw 10:a6e1707fdec0 173
tylerjw 10:a6e1707fdec0 174 // (atan2(y,x*r2d) + 360.0) % 360.0 ??? http://www.movable-type.co.uk/scripts/latlong.html
tylerjw 5:94daced1e61a 175 }
tylerjw 5:94daced1e61a 176
tylerjw 9:9b2351e25a84 177 double NmeaParser::calc_dist_to_mi(float pointLat, float pontLong)
tylerjw 5:94daced1e61a 178 {
tylerjw 5:94daced1e61a 179 const double d2r = PI / 180.0;
tylerjw 9:9b2351e25a84 180 double dlat = pointLat - calc_dec_latitude();
tylerjw 9:9b2351e25a84 181 double dlong = pontLong - calc_dec_longitude();
tylerjw 9:9b2351e25a84 182 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 183 double c = 2.0 * asin(sqrt(abs(a)));
tylerjw 5:94daced1e61a 184 double d = 63.765 * c;
tylerjw 5:94daced1e61a 185
tylerjw 5:94daced1e61a 186 return d;
tylerjw 5:94daced1e61a 187 }
tylerjw 5:94daced1e61a 188
tylerjw 9:9b2351e25a84 189 double NmeaParser::calc_dist_to_km(float pointLat, float pontLong)
tylerjw 5:94daced1e61a 190 {
tylerjw 5:94daced1e61a 191 return calc_dist_to_mi(pointLat, pontLong)*1.609344;
tylerjw 5:94daced1e61a 192 }
tylerjw 5:94daced1e61a 193
tylerjw 9:9b2351e25a84 194 char *NmeaParser::my_token(char *source,char token)
tylerjw 7:01a8379370e4 195 {
tylerjw 7:01a8379370e4 196 char *start;
tylerjw 7:01a8379370e4 197 /* The source string is real only for the first call. Subsequent calls
tylerjw 7:01a8379370e4 198 are made with the source string pointer as NULL
tylerjw 7:01a8379370e4 199 */
tylerjw 7:01a8379370e4 200 if(source != NULL) {
tylerjw 7:01a8379370e4 201 /* If the string is empty return NULL */
tylerjw 7:01a8379370e4 202 if(strlen(source) == 0)
tylerjw 7:01a8379370e4 203 return NULL;
tylerjw 9:9b2351e25a84 204 strcpy(stat_string_,source);
tylerjw 7:01a8379370e4 205 /* Current is our 'current' position within the string */
tylerjw 9:9b2351e25a84 206 current_ = stat_string_;
tylerjw 7:01a8379370e4 207 }
tylerjw 9:9b2351e25a84 208 start = current_;
tylerjw 7:01a8379370e4 209
tylerjw 7:01a8379370e4 210 while (true) {
tylerjw 7:01a8379370e4 211 /* If we're at the end of the string, return NULL */
tylerjw 9:9b2351e25a84 212 if((*current_ == '\0') && (current_ == start))
tylerjw 7:01a8379370e4 213 return NULL;
tylerjw 7:01a8379370e4 214 /* If we're at the end now, but weren't when we started, we need
tylerjw 7:01a8379370e4 215 to return the pointer for the last field before the end of string
tylerjw 7:01a8379370e4 216 */
tylerjw 9:9b2351e25a84 217 if(*current_ == '\0')
tylerjw 7:01a8379370e4 218 return start;
tylerjw 7:01a8379370e4 219 /* If we've located our specified token (comma) in the string
tylerjw 7:01a8379370e4 220 load its location in the copy with an end of string marker
tylerjw 7:01a8379370e4 221 so that it can be handled correctly by the calling program.
tylerjw 7:01a8379370e4 222 */
tylerjw 9:9b2351e25a84 223 if(*current_ == token) {
tylerjw 9:9b2351e25a84 224 *current_ = '\0';
tylerjw 9:9b2351e25a84 225 current_++;
tylerjw 7:01a8379370e4 226 return start;
tylerjw 7:01a8379370e4 227 } else {
tylerjw 9:9b2351e25a84 228 current_++;
tylerjw 7:01a8379370e4 229 }
tylerjw 7:01a8379370e4 230 }
tylerjw 7:01a8379370e4 231 }
tylerjw 7:01a8379370e4 232
tylerjw 9:9b2351e25a84 233 int NmeaParser::parse(char *string)
tylerjw 7:01a8379370e4 234 {
tylerjw 7:01a8379370e4 235 int field_count;
tylerjw 7:01a8379370e4 236 field_count = 0;
tylerjw 7:01a8379370e4 237 /* NMEA 0183 fields are delimited by commas. The my_token function returns
tylerjw 7:01a8379370e4 238 pointers to the fields.
tylerjw 7:01a8379370e4 239 */
tylerjw 7:01a8379370e4 240 /* Get the first field pointer */
tylerjw 9:9b2351e25a84 241 field_[0] = my_token(string,',');
tylerjw 7:01a8379370e4 242 field_count++;
tylerjw 7:01a8379370e4 243
tylerjw 7:01a8379370e4 244 while (true) {
tylerjw 7:01a8379370e4 245 /* Contiue retrieving fields until there are no more (NULL) */
tylerjw 9:9b2351e25a84 246 field_[field_count] = my_token(NULL,',');
tylerjw 9:9b2351e25a84 247 if(field_[field_count] == NULL)
tylerjw 7:01a8379370e4 248 break;
tylerjw 7:01a8379370e4 249 field_count++;
tylerjw 7:01a8379370e4 250 }
tylerjw 7:01a8379370e4 251 /* If we got at least ONE field */
tylerjw 7:01a8379370e4 252 if(field_count) {
tylerjw 7:01a8379370e4 253 /* Check the first field for the valid NMEA 0183 headers */
tylerjw 9:9b2351e25a84 254 if(strcmp(field_[0],"$GPGGA") == 0) {
tylerjw 7:01a8379370e4 255 /* Retrieve the values from the remaining fields */
tylerjw 9:9b2351e25a84 256 utc_time_ = atof(field_[1]);
tylerjw 9:9b2351e25a84 257 latitude_ = atof(field_[2]);
tylerjw 9:9b2351e25a84 258 lat_reference_ = *(field_[3]);
tylerjw 9:9b2351e25a84 259 longitude_ = atof(field_[4]);
tylerjw 9:9b2351e25a84 260 long_reference_ = *(field_[5]);
tylerjw 9:9b2351e25a84 261 quality_ = atoi(field_[6]);
tylerjw 9:9b2351e25a84 262 satellite_count_ = atoi(field_[7]);
tylerjw 9:9b2351e25a84 263 hdop_ = atof(field_[8]);
tylerjw 9:9b2351e25a84 264 msl_altitude_ = atof(field_[9]);
tylerjw 9:9b2351e25a84 265 msl_altitude_unit_ = *(field_[10]);
tylerjw 8:59acef1c795b 266 return GGA;
tylerjw 7:01a8379370e4 267 }
tylerjw 9:9b2351e25a84 268 if(strcmp(field_[0],"$GPRMC") == 0) {
tylerjw 7:01a8379370e4 269 /* Retrieve the data from the remaining fields */
tylerjw 9:9b2351e25a84 270 utc_time_ = atof(field_[1]);
tylerjw 9:9b2351e25a84 271 latitude_ = atof(field_[3]);
tylerjw 9:9b2351e25a84 272 lat_reference_ = *(field_[4]);
tylerjw 9:9b2351e25a84 273 longitude_ = atof(field_[5]);
tylerjw 9:9b2351e25a84 274 long_reference_ = *(field_[6]);
tylerjw 9:9b2351e25a84 275 speed_ = atof(field_[7]);
tylerjw 9:9b2351e25a84 276 track_ = atof(field_[8]);
tylerjw 9:9b2351e25a84 277 date_ = atol(field_[9]);
tylerjw 8:59acef1c795b 278 return RMC;
tylerjw 7:01a8379370e4 279 }
tylerjw 7:01a8379370e4 280 }
tylerjw 8:59acef1c795b 281 return NOT_PARSED;
tylerjw 7:01a8379370e4 282 }