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;
}