2018 revision to classic DataBus AVC code.
Dependencies: LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell
Diff: GPS/NMEA.cpp
- Revision:
- 44:0d72a8a1288a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/GPS/NMEA.cpp Mon Jan 07 16:47:33 2019 +0000 @@ -0,0 +1,285 @@ +/** NMEA - a small NMEA-parsing library based on TinyGPS + * @Author Michael Shimniok + * www.bot-thoughts.com + + * TinyGPS - a small GPS library for Arduino providing basic NMEA parsing + * Copyright (C) 2008-9 Mikal Hart + * All rights reserved. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + */ + +#include "NMEA.h" +#include <cstdlib> +#include <stdio.h> +#include <string.h> + +#define _GPRMC_TERM "GPRMC" +#define _GPGGA_TERM "GPGGA" + +NMEA::NMEA() +: _new_time(GPS_INVALID_TIME) +, _new_date(GPS_INVALID_DATE) +, _new_latitude(GPS_INVALID_ANGLE) +, _new_longitude(GPS_INVALID_ANGLE) +, _new_altitude(GPS_INVALID_ALTITUDE) +, _new_speed(GPS_INVALID_SPEED) +, _new_course(GPS_INVALID_ANGLE) +, _new_hdop(0) +, _new_sat_count(0) +, _last_time_fix(GPS_INVALID_FIX_TIME) +, _new_time_fix(GPS_INVALID_FIX_TIME) +, _last_position_fix(GPS_INVALID_FIX_TIME) +, _new_position_fix(GPS_INVALID_FIX_TIME) +, _parity(0) +, _is_checksum_term(false) +, _sentence_type(_GPS_SENTENCE_OTHER) +, _term_number(0) +, _term_offset(0) +, _gps_data_good(false) +, _rmc_ready(false) +, _gga_ready(false) +{ + _term[0] = '\0'; +} + +///////////////////////////////////////////////////////////////////////////////// +// Public member functions +// + +// Parse NMEA once character at a time +// @return 1 if GGA and RMC sentences are valid and ready +int NMEA::parse(char c) +{ + int valid_sentence = 0; + + switch(c) { + case ',': // term terminators + _parity ^= c; + // no break + case '\r': + case '\n': + case '*': + if (_term_offset < sizeof(_term)) { + _term[_term_offset] = 0; + valid_sentence = term_complete(); + } + ++_term_number; + _term_offset = 0; + _is_checksum_term = c == '*'; +#ifdef __MBED__ + if (_callback) _callback(); +#endif + break; + + case '$': // sentence begin + _term_number = _term_offset = 0; + _parity = 0; + _sentence_type = _GPS_SENTENCE_OTHER; + _is_checksum_term = false; + _gps_data_good = false; + break; + + default: + // ordinary characters + if (_term_offset < sizeof(_term) - 1) + _term[_term_offset++] = c; + if (!_is_checksum_term) + _parity ^= c; + } + + return valid_sentence & ready(); +} + + +///////////////////////////////////////////////////////////////////////////////// +// Private member functions + +// Convert hex char representation to int +int NMEA::from_hex(char a) +{ + if (a >= 'A' && a <= 'F') + return a - 'A' + 10; + else if (a >= 'a' && a <= 'f') + return a - 'a' + 10; + else + return a - '0'; +} + + +// Convert string to integer +int NMEA::parse_int() +{ + char *p = _term; + bool isneg = *p == '-'; + if (isneg) ++p; + int ret = atoi(p); + return isneg ? -ret : ret; +} + + +// Convert string to double +double NMEA::parse_decimal() +{ + char *p = _term; + double ret = atof(p); + return ret; +} + + +// Convert NMEA lat/lon degrees/minutes to double degrees +double NMEA::parse_degrees() +{ + double result; + int16_t degrees = atoi(_term) / 100; + char *minutes = strchr(_term, '.') - 2; + //printf("term=<%s> %d %f\n", _term, degrees, atof(minutes)); + result = degrees + atof(minutes) / 60.0; + return result; +} + + +// Processes a just-completed term +// @return true if new sentence has just passed checksum test and is validated +bool NMEA::term_complete() +{ + if (_is_checksum_term) { + byte checksum = 16 * from_hex(_term[0]) + from_hex(_term[1]); + if (checksum == _parity) { + if (_gps_data_good) { + _last_time_fix = _new_time_fix; + _last_position_fix = _new_position_fix; + switch(_sentence_type) { + case _GPS_SENTENCE_GPRMC: + latest.date = _new_date; + latest.month = _new_month; + latest.year = _new_year; + latest.hour = _new_hour; + latest.minute = _new_minute; + latest.second = _new_second; + latest.lat = _new_latitude; + latest.lon = _new_longitude; + latest.speed = _new_speed; + latest.course = _new_course; + printf("--- RMC_READY\n"); + _rmc_ready = true; + break; + case _GPS_SENTENCE_GPGGA: + _altitude = _new_altitude; + latest.lat = _new_latitude; + latest.lon = _new_longitude; + latest.hdop = _new_hdop; + latest.svcount = _new_sat_count; + printf("--- GGA_READY\n"); + _gga_ready = true; + break; + }//switch _sentence_type + return true; +// } else { +// printf("bad data\n"); + }//if _gps_data_good + } else { + printf("bad checksum 0x%x expected 0x%x\n", checksum, _parity); + }//if checksum + + return false; + }//if _is_checksum_term + + // the first term determines the sentence type + if (_term_number == 0) { + if (!strcmp(_term, _GPRMC_TERM)) + _sentence_type = _GPS_SENTENCE_GPRMC; + else if (!strcmp(_term, _GPGGA_TERM)) + _sentence_type = _GPS_SENTENCE_GPGGA; + else + _sentence_type = _GPS_SENTENCE_OTHER; + return false; + } + + if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) { + printf("\tterm:<%s>\n", _term); + switch (_sentence_type | _term_number) { + case _GPS_SENTENCE_GPRMC|1: // Time in both sentences + //case _GPS_SENTENCE_GPGGA|1: + { + char *s = _term+4; + _new_second = atoi(s); + *s = 0; + s -= 2; + _new_minute = atoi(s); + *s = 0; + s -= 2; + _new_hour = atoi(s); + } + break; + case _GPS_SENTENCE_GPRMC|2: // GPRMC validity + _gps_data_good = _term[0] == 'A'; + break; + case _GPS_SENTENCE_GPRMC|3: // Latitude + case _GPS_SENTENCE_GPGGA|2: + _new_latitude = parse_degrees(); + break; + case _GPS_SENTENCE_GPRMC|4: // N/S + case _GPS_SENTENCE_GPGGA|3: + if (_term[0] == 'S') + _new_latitude = -_new_latitude; + break; + case _GPS_SENTENCE_GPRMC|5: // Longitude + case _GPS_SENTENCE_GPGGA|4: + _new_longitude = parse_degrees(); + break; + case _GPS_SENTENCE_GPRMC|6: // E/W + case _GPS_SENTENCE_GPGGA|5: + if (_term[0] == 'W') + _new_longitude = -_new_longitude; + break; + case _GPS_SENTENCE_GPRMC|7: // Speed (GPRMC) + _new_speed = parse_decimal(); + break; + case _GPS_SENTENCE_GPRMC|8: // Course (GPRMC) + _new_course = parse_decimal(); + break; + case _GPS_SENTENCE_GPRMC|9: // Date (GPRMC) + { + char *s = _term+4; + _new_year = atoi(s); + *s = 0; + s -= 2; + _new_month = atoi(s); + *s = 0; + s -= 2; + _new_date = atoi(s); + //printf("%02d:%02d:%02d %02d/%02d/%02d\n", _new_hour, _new_minute, _new_second, _new_month, _new_date, _new_year); + } + break; + case _GPS_SENTENCE_GPGGA|6: // Fix data (GPGGA) + _gps_data_good = _term[0] > '0'; + break; + case _GPS_SENTENCE_GPGGA|7: // Number of satelites tracked (GPGGA) + _new_sat_count = parse_int(); + break; + case _GPS_SENTENCE_GPGGA|8: // Horizontal Dilution of Position (GPGGA) + _new_hdop = parse_decimal(); + break; + case _GPS_SENTENCE_GPGGA|9: // Altitude (GPGGA) + _new_altitude = parse_decimal(); + break; + default : + break; + } /* switch */ + }//if + + return false; +}