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
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?
User | Revision | Line number | New 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 | } |