Michael Shimniok / Mbed OS DataBus2018

2018 revision to classic DataBus AVC code.

Dependencies:   LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell

GPS/NMEA.cpp@44:0d72a8a1288a, 2019-01-07 (annotated)

Committer:
shimniok
Date:
Mon Jan 07 16:47:33 2019 +0000
Revision:
44:0d72a8a1288a
Rewrote TinyGPS -> NMEA, GPS::read() now returns struct

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 44:0d72a8a1288a 1 /** NMEA - a small NMEA-parsing library based on TinyGPS
shimniok 44:0d72a8a1288a 2 * @Author Michael Shimniok
shimniok 44:0d72a8a1288a 3 * www.bot-thoughts.com
shimniok 44:0d72a8a1288a 4
shimniok 44:0d72a8a1288a 5 * TinyGPS - a small GPS library for Arduino providing basic NMEA parsing
shimniok 44:0d72a8a1288a 6 * Copyright (C) 2008-9 Mikal Hart
shimniok 44:0d72a8a1288a 7 * All rights reserved.
shimniok 44:0d72a8a1288a 8 *
shimniok 44:0d72a8a1288a 9 * This library is free software; you can redistribute it and/or
shimniok 44:0d72a8a1288a 10 * modify it under the terms of the GNU Lesser General Public
shimniok 44:0d72a8a1288a 11 * License as published by the Free Software Foundation; either
shimniok 44:0d72a8a1288a 12 * version 2.1 of the License, or (at your option) any later version.
shimniok 44:0d72a8a1288a 13 *
shimniok 44:0d72a8a1288a 14 * This library is distributed in the hope that it will be useful,
shimniok 44:0d72a8a1288a 15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
shimniok 44:0d72a8a1288a 16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
shimniok 44:0d72a8a1288a 17 * Lesser General Public License for more details.
shimniok 44:0d72a8a1288a 18 *
shimniok 44:0d72a8a1288a 19 * You should have received a copy of the GNU Lesser General Public
shimniok 44:0d72a8a1288a 20 * License along with this library; if not, write to the Free Software
shimniok 44:0d72a8a1288a 21 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
shimniok 44:0d72a8a1288a 22 */
shimniok 44:0d72a8a1288a 23
shimniok 44:0d72a8a1288a 24 #include "NMEA.h"
shimniok 44:0d72a8a1288a 25 #include <cstdlib>
shimniok 44:0d72a8a1288a 26 #include <stdio.h>
shimniok 44:0d72a8a1288a 27 #include <string.h>
shimniok 44:0d72a8a1288a 28
shimniok 44:0d72a8a1288a 29 #define _GPRMC_TERM "GPRMC"
shimniok 44:0d72a8a1288a 30 #define _GPGGA_TERM "GPGGA"
shimniok 44:0d72a8a1288a 31
shimniok 44:0d72a8a1288a 32 NMEA::NMEA()
shimniok 44:0d72a8a1288a 33 : _new_time(GPS_INVALID_TIME)
shimniok 44:0d72a8a1288a 34 , _new_date(GPS_INVALID_DATE)
shimniok 44:0d72a8a1288a 35 , _new_latitude(GPS_INVALID_ANGLE)
shimniok 44:0d72a8a1288a 36 , _new_longitude(GPS_INVALID_ANGLE)
shimniok 44:0d72a8a1288a 37 , _new_altitude(GPS_INVALID_ALTITUDE)
shimniok 44:0d72a8a1288a 38 , _new_speed(GPS_INVALID_SPEED)
shimniok 44:0d72a8a1288a 39 , _new_course(GPS_INVALID_ANGLE)
shimniok 44:0d72a8a1288a 40 , _new_hdop(0)
shimniok 44:0d72a8a1288a 41 , _new_sat_count(0)
shimniok 44:0d72a8a1288a 42 , _last_time_fix(GPS_INVALID_FIX_TIME)
shimniok 44:0d72a8a1288a 43 , _new_time_fix(GPS_INVALID_FIX_TIME)
shimniok 44:0d72a8a1288a 44 , _last_position_fix(GPS_INVALID_FIX_TIME)
shimniok 44:0d72a8a1288a 45 , _new_position_fix(GPS_INVALID_FIX_TIME)
shimniok 44:0d72a8a1288a 46 , _parity(0)
shimniok 44:0d72a8a1288a 47 , _is_checksum_term(false)
shimniok 44:0d72a8a1288a 48 , _sentence_type(_GPS_SENTENCE_OTHER)
shimniok 44:0d72a8a1288a 49 , _term_number(0)
shimniok 44:0d72a8a1288a 50 , _term_offset(0)
shimniok 44:0d72a8a1288a 51 , _gps_data_good(false)
shimniok 44:0d72a8a1288a 52 , _rmc_ready(false)
shimniok 44:0d72a8a1288a 53 , _gga_ready(false)
shimniok 44:0d72a8a1288a 54 {
shimniok 44:0d72a8a1288a 55 _term[0] = '\0';
shimniok 44:0d72a8a1288a 56 }
shimniok 44:0d72a8a1288a 57
shimniok 44:0d72a8a1288a 58 /////////////////////////////////////////////////////////////////////////////////
shimniok 44:0d72a8a1288a 59 // Public member functions
shimniok 44:0d72a8a1288a 60 //
shimniok 44:0d72a8a1288a 61
shimniok 44:0d72a8a1288a 62 // Parse NMEA once character at a time
shimniok 44:0d72a8a1288a 63 // @return 1 if GGA and RMC sentences are valid and ready
shimniok 44:0d72a8a1288a 64 int NMEA::parse(char c)
shimniok 44:0d72a8a1288a 65 {
shimniok 44:0d72a8a1288a 66 int valid_sentence = 0;
shimniok 44:0d72a8a1288a 67
shimniok 44:0d72a8a1288a 68 switch(c) {
shimniok 44:0d72a8a1288a 69 case ',': // term terminators
shimniok 44:0d72a8a1288a 70 _parity ^= c;
shimniok 44:0d72a8a1288a 71 // no break
shimniok 44:0d72a8a1288a 72 case '\r':
shimniok 44:0d72a8a1288a 73 case '\n':
shimniok 44:0d72a8a1288a 74 case '*':
shimniok 44:0d72a8a1288a 75 if (_term_offset < sizeof(_term)) {
shimniok 44:0d72a8a1288a 76 _term[_term_offset] = 0;
shimniok 44:0d72a8a1288a 77 valid_sentence = term_complete();
shimniok 44:0d72a8a1288a 78 }
shimniok 44:0d72a8a1288a 79 ++_term_number;
shimniok 44:0d72a8a1288a 80 _term_offset = 0;
shimniok 44:0d72a8a1288a 81 _is_checksum_term = c == '*';
shimniok 44:0d72a8a1288a 82 #ifdef __MBED__
shimniok 44:0d72a8a1288a 83 if (_callback) _callback();
shimniok 44:0d72a8a1288a 84 #endif
shimniok 44:0d72a8a1288a 85 break;
shimniok 44:0d72a8a1288a 86
shimniok 44:0d72a8a1288a 87 case '$': // sentence begin
shimniok 44:0d72a8a1288a 88 _term_number = _term_offset = 0;
shimniok 44:0d72a8a1288a 89 _parity = 0;
shimniok 44:0d72a8a1288a 90 _sentence_type = _GPS_SENTENCE_OTHER;
shimniok 44:0d72a8a1288a 91 _is_checksum_term = false;
shimniok 44:0d72a8a1288a 92 _gps_data_good = false;
shimniok 44:0d72a8a1288a 93 break;
shimniok 44:0d72a8a1288a 94
shimniok 44:0d72a8a1288a 95 default:
shimniok 44:0d72a8a1288a 96 // ordinary characters
shimniok 44:0d72a8a1288a 97 if (_term_offset < sizeof(_term) - 1)
shimniok 44:0d72a8a1288a 98 _term[_term_offset++] = c;
shimniok 44:0d72a8a1288a 99 if (!_is_checksum_term)
shimniok 44:0d72a8a1288a 100 _parity ^= c;
shimniok 44:0d72a8a1288a 101 }
shimniok 44:0d72a8a1288a 102
shimniok 44:0d72a8a1288a 103 return valid_sentence & ready();
shimniok 44:0d72a8a1288a 104 }
shimniok 44:0d72a8a1288a 105
shimniok 44:0d72a8a1288a 106
shimniok 44:0d72a8a1288a 107 /////////////////////////////////////////////////////////////////////////////////
shimniok 44:0d72a8a1288a 108 // Private member functions
shimniok 44:0d72a8a1288a 109
shimniok 44:0d72a8a1288a 110 // Convert hex char representation to int
shimniok 44:0d72a8a1288a 111 int NMEA::from_hex(char a)
shimniok 44:0d72a8a1288a 112 {
shimniok 44:0d72a8a1288a 113 if (a >= 'A' && a <= 'F')
shimniok 44:0d72a8a1288a 114 return a - 'A' + 10;
shimniok 44:0d72a8a1288a 115 else if (a >= 'a' && a <= 'f')
shimniok 44:0d72a8a1288a 116 return a - 'a' + 10;
shimniok 44:0d72a8a1288a 117 else
shimniok 44:0d72a8a1288a 118 return a - '0';
shimniok 44:0d72a8a1288a 119 }
shimniok 44:0d72a8a1288a 120
shimniok 44:0d72a8a1288a 121
shimniok 44:0d72a8a1288a 122 // Convert string to integer
shimniok 44:0d72a8a1288a 123 int NMEA::parse_int()
shimniok 44:0d72a8a1288a 124 {
shimniok 44:0d72a8a1288a 125 char *p = _term;
shimniok 44:0d72a8a1288a 126 bool isneg = *p == '-';
shimniok 44:0d72a8a1288a 127 if (isneg) ++p;
shimniok 44:0d72a8a1288a 128 int ret = atoi(p);
shimniok 44:0d72a8a1288a 129 return isneg ? -ret : ret;
shimniok 44:0d72a8a1288a 130 }
shimniok 44:0d72a8a1288a 131
shimniok 44:0d72a8a1288a 132
shimniok 44:0d72a8a1288a 133 // Convert string to double
shimniok 44:0d72a8a1288a 134 double NMEA::parse_decimal()
shimniok 44:0d72a8a1288a 135 {
shimniok 44:0d72a8a1288a 136 char *p = _term;
shimniok 44:0d72a8a1288a 137 double ret = atof(p);
shimniok 44:0d72a8a1288a 138 return ret;
shimniok 44:0d72a8a1288a 139 }
shimniok 44:0d72a8a1288a 140
shimniok 44:0d72a8a1288a 141
shimniok 44:0d72a8a1288a 142 // Convert NMEA lat/lon degrees/minutes to double degrees
shimniok 44:0d72a8a1288a 143 double NMEA::parse_degrees()
shimniok 44:0d72a8a1288a 144 {
shimniok 44:0d72a8a1288a 145 double result;
shimniok 44:0d72a8a1288a 146 int16_t degrees = atoi(_term) / 100;
shimniok 44:0d72a8a1288a 147 char *minutes = strchr(_term, '.') - 2;
shimniok 44:0d72a8a1288a 148 //printf("term=<%s> %d %f\n", _term, degrees, atof(minutes));
shimniok 44:0d72a8a1288a 149 result = degrees + atof(minutes) / 60.0;
shimniok 44:0d72a8a1288a 150 return result;
shimniok 44:0d72a8a1288a 151 }
shimniok 44:0d72a8a1288a 152
shimniok 44:0d72a8a1288a 153
shimniok 44:0d72a8a1288a 154 // Processes a just-completed term
shimniok 44:0d72a8a1288a 155 // @return true if new sentence has just passed checksum test and is validated
shimniok 44:0d72a8a1288a 156 bool NMEA::term_complete()
shimniok 44:0d72a8a1288a 157 {
shimniok 44:0d72a8a1288a 158 if (_is_checksum_term) {
shimniok 44:0d72a8a1288a 159 byte checksum = 16 * from_hex(_term[0]) + from_hex(_term[1]);
shimniok 44:0d72a8a1288a 160 if (checksum == _parity) {
shimniok 44:0d72a8a1288a 161 if (_gps_data_good) {
shimniok 44:0d72a8a1288a 162 _last_time_fix = _new_time_fix;
shimniok 44:0d72a8a1288a 163 _last_position_fix = _new_position_fix;
shimniok 44:0d72a8a1288a 164 switch(_sentence_type) {
shimniok 44:0d72a8a1288a 165 case _GPS_SENTENCE_GPRMC:
shimniok 44:0d72a8a1288a 166 latest.date = _new_date;
shimniok 44:0d72a8a1288a 167 latest.month = _new_month;
shimniok 44:0d72a8a1288a 168 latest.year = _new_year;
shimniok 44:0d72a8a1288a 169 latest.hour = _new_hour;
shimniok 44:0d72a8a1288a 170 latest.minute = _new_minute;
shimniok 44:0d72a8a1288a 171 latest.second = _new_second;
shimniok 44:0d72a8a1288a 172 latest.lat = _new_latitude;
shimniok 44:0d72a8a1288a 173 latest.lon = _new_longitude;
shimniok 44:0d72a8a1288a 174 latest.speed = _new_speed;
shimniok 44:0d72a8a1288a 175 latest.course = _new_course;
shimniok 44:0d72a8a1288a 176 printf("--- RMC_READY\n");
shimniok 44:0d72a8a1288a 177 _rmc_ready = true;
shimniok 44:0d72a8a1288a 178 break;
shimniok 44:0d72a8a1288a 179 case _GPS_SENTENCE_GPGGA:
shimniok 44:0d72a8a1288a 180 _altitude = _new_altitude;
shimniok 44:0d72a8a1288a 181 latest.lat = _new_latitude;
shimniok 44:0d72a8a1288a 182 latest.lon = _new_longitude;
shimniok 44:0d72a8a1288a 183 latest.hdop = _new_hdop;
shimniok 44:0d72a8a1288a 184 latest.svcount = _new_sat_count;
shimniok 44:0d72a8a1288a 185 printf("--- GGA_READY\n");
shimniok 44:0d72a8a1288a 186 _gga_ready = true;
shimniok 44:0d72a8a1288a 187 break;
shimniok 44:0d72a8a1288a 188 }//switch _sentence_type
shimniok 44:0d72a8a1288a 189 return true;
shimniok 44:0d72a8a1288a 190 // } else {
shimniok 44:0d72a8a1288a 191 // printf("bad data\n");
shimniok 44:0d72a8a1288a 192 }//if _gps_data_good
shimniok 44:0d72a8a1288a 193 } else {
shimniok 44:0d72a8a1288a 194 printf("bad checksum 0x%x expected 0x%x\n", checksum, _parity);
shimniok 44:0d72a8a1288a 195 }//if checksum
shimniok 44:0d72a8a1288a 196
shimniok 44:0d72a8a1288a 197 return false;
shimniok 44:0d72a8a1288a 198 }//if _is_checksum_term
shimniok 44:0d72a8a1288a 199
shimniok 44:0d72a8a1288a 200 // the first term determines the sentence type
shimniok 44:0d72a8a1288a 201 if (_term_number == 0) {
shimniok 44:0d72a8a1288a 202 if (!strcmp(_term, _GPRMC_TERM))
shimniok 44:0d72a8a1288a 203 _sentence_type = _GPS_SENTENCE_GPRMC;
shimniok 44:0d72a8a1288a 204 else if (!strcmp(_term, _GPGGA_TERM))
shimniok 44:0d72a8a1288a 205 _sentence_type = _GPS_SENTENCE_GPGGA;
shimniok 44:0d72a8a1288a 206 else
shimniok 44:0d72a8a1288a 207 _sentence_type = _GPS_SENTENCE_OTHER;
shimniok 44:0d72a8a1288a 208 return false;
shimniok 44:0d72a8a1288a 209 }
shimniok 44:0d72a8a1288a 210
shimniok 44:0d72a8a1288a 211 if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) {
shimniok 44:0d72a8a1288a 212 printf("\tterm:<%s>\n", _term);
shimniok 44:0d72a8a1288a 213 switch (_sentence_type | _term_number) {
shimniok 44:0d72a8a1288a 214 case _GPS_SENTENCE_GPRMC|1: // Time in both sentences
shimniok 44:0d72a8a1288a 215 //case _GPS_SENTENCE_GPGGA|1:
shimniok 44:0d72a8a1288a 216 {
shimniok 44:0d72a8a1288a 217 char *s = _term+4;
shimniok 44:0d72a8a1288a 218 _new_second = atoi(s);
shimniok 44:0d72a8a1288a 219 *s = 0;
shimniok 44:0d72a8a1288a 220 s -= 2;
shimniok 44:0d72a8a1288a 221 _new_minute = atoi(s);
shimniok 44:0d72a8a1288a 222 *s = 0;
shimniok 44:0d72a8a1288a 223 s -= 2;
shimniok 44:0d72a8a1288a 224 _new_hour = atoi(s);
shimniok 44:0d72a8a1288a 225 }
shimniok 44:0d72a8a1288a 226 break;
shimniok 44:0d72a8a1288a 227 case _GPS_SENTENCE_GPRMC|2: // GPRMC validity
shimniok 44:0d72a8a1288a 228 _gps_data_good = _term[0] == 'A';
shimniok 44:0d72a8a1288a 229 break;
shimniok 44:0d72a8a1288a 230 case _GPS_SENTENCE_GPRMC|3: // Latitude
shimniok 44:0d72a8a1288a 231 case _GPS_SENTENCE_GPGGA|2:
shimniok 44:0d72a8a1288a 232 _new_latitude = parse_degrees();
shimniok 44:0d72a8a1288a 233 break;
shimniok 44:0d72a8a1288a 234 case _GPS_SENTENCE_GPRMC|4: // N/S
shimniok 44:0d72a8a1288a 235 case _GPS_SENTENCE_GPGGA|3:
shimniok 44:0d72a8a1288a 236 if (_term[0] == 'S')
shimniok 44:0d72a8a1288a 237 _new_latitude = -_new_latitude;
shimniok 44:0d72a8a1288a 238 break;
shimniok 44:0d72a8a1288a 239 case _GPS_SENTENCE_GPRMC|5: // Longitude
shimniok 44:0d72a8a1288a 240 case _GPS_SENTENCE_GPGGA|4:
shimniok 44:0d72a8a1288a 241 _new_longitude = parse_degrees();
shimniok 44:0d72a8a1288a 242 break;
shimniok 44:0d72a8a1288a 243 case _GPS_SENTENCE_GPRMC|6: // E/W
shimniok 44:0d72a8a1288a 244 case _GPS_SENTENCE_GPGGA|5:
shimniok 44:0d72a8a1288a 245 if (_term[0] == 'W')
shimniok 44:0d72a8a1288a 246 _new_longitude = -_new_longitude;
shimniok 44:0d72a8a1288a 247 break;
shimniok 44:0d72a8a1288a 248 case _GPS_SENTENCE_GPRMC|7: // Speed (GPRMC)
shimniok 44:0d72a8a1288a 249 _new_speed = parse_decimal();
shimniok 44:0d72a8a1288a 250 break;
shimniok 44:0d72a8a1288a 251 case _GPS_SENTENCE_GPRMC|8: // Course (GPRMC)
shimniok 44:0d72a8a1288a 252 _new_course = parse_decimal();
shimniok 44:0d72a8a1288a 253 break;
shimniok 44:0d72a8a1288a 254 case _GPS_SENTENCE_GPRMC|9: // Date (GPRMC)
shimniok 44:0d72a8a1288a 255 {
shimniok 44:0d72a8a1288a 256 char *s = _term+4;
shimniok 44:0d72a8a1288a 257 _new_year = atoi(s);
shimniok 44:0d72a8a1288a 258 *s = 0;
shimniok 44:0d72a8a1288a 259 s -= 2;
shimniok 44:0d72a8a1288a 260 _new_month = atoi(s);
shimniok 44:0d72a8a1288a 261 *s = 0;
shimniok 44:0d72a8a1288a 262 s -= 2;
shimniok 44:0d72a8a1288a 263 _new_date = atoi(s);
shimniok 44:0d72a8a1288a 264 //printf("%02d:%02d:%02d %02d/%02d/%02d\n", _new_hour, _new_minute, _new_second, _new_month, _new_date, _new_year);
shimniok 44:0d72a8a1288a 265 }
shimniok 44:0d72a8a1288a 266 break;
shimniok 44:0d72a8a1288a 267 case _GPS_SENTENCE_GPGGA|6: // Fix data (GPGGA)
shimniok 44:0d72a8a1288a 268 _gps_data_good = _term[0] > '0';
shimniok 44:0d72a8a1288a 269 break;
shimniok 44:0d72a8a1288a 270 case _GPS_SENTENCE_GPGGA|7: // Number of satelites tracked (GPGGA)
shimniok 44:0d72a8a1288a 271 _new_sat_count = parse_int();
shimniok 44:0d72a8a1288a 272 break;
shimniok 44:0d72a8a1288a 273 case _GPS_SENTENCE_GPGGA|8: // Horizontal Dilution of Position (GPGGA)
shimniok 44:0d72a8a1288a 274 _new_hdop = parse_decimal();
shimniok 44:0d72a8a1288a 275 break;
shimniok 44:0d72a8a1288a 276 case _GPS_SENTENCE_GPGGA|9: // Altitude (GPGGA)
shimniok 44:0d72a8a1288a 277 _new_altitude = parse_decimal();
shimniok 44:0d72a8a1288a 278 break;
shimniok 44:0d72a8a1288a 279 default :
shimniok 44:0d72a8a1288a 280 break;
shimniok 44:0d72a8a1288a 281 } /* switch */
shimniok 44:0d72a8a1288a 282 }//if
shimniok 44:0d72a8a1288a 283
shimniok 44:0d72a8a1288a 284 return false;
shimniok 44:0d72a8a1288a 285 }