2018 revision to classic DataBus AVC code.
Dependencies: LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell
GPS/NMEA.cpp
- Committer:
- shimniok
- Date:
- 2019-01-07
- Revision:
- 44:0d72a8a1288a
File content as of revision 44:0d72a8a1288a:
/** 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; }