Statistics
| Branch: | Revision:

root / quad1 / AeroQuad / TinyGPS.cpp @ 9240aaa3

History | View | Annotate | Download (6.39 KB)

1
/*
2
  TinyGPS - a small GPS library for Arduino providing basic NMEA parsing
3
  Copyright (C) 2008-9 Mikal Hart
4
  All rights reserved.
5

6
  This library is free software; you can redistribute it and/or
7
  modify it under the terms of the GNU Lesser General Public
8
  License as published by the Free Software Foundation; either
9
  version 2.1 of the License, or (at your option) any later version.
10

11
  This library is distributed in the hope that it will be useful,
12
  but WITHOUT ANY WARRANTY; without even the implied warranty of
13
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
14
  Lesser General Public License for more details.
15

16
  You should have received a copy of the GNU Lesser General Public
17
  License along with this library; if not, write to the Free Software
18
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
19
*/
20

    
21
#include "WProgram.h"
22
#include "TinyGPS.h"
23

    
24
#define _GPRMC_TERM   "GPRMC"
25
#define _GPGGA_TERM   "GPGGA"
26

    
27
TinyGPS::TinyGPS()
28
:  _time(GPS_INVALID_TIME)
29
,  _date(GPS_INVALID_DATE)
30
,  _latitude(GPS_INVALID_ANGLE)
31
,  _longitude(GPS_INVALID_ANGLE)
32
,  _altitude(GPS_INVALID_ALTITUDE)
33
,  _speed(GPS_INVALID_SPEED)
34
,  _course(GPS_INVALID_ANGLE)
35
,  _last_time_fix(GPS_INVALID_FIX_TIME)
36
,  _last_position_fix(GPS_INVALID_FIX_TIME)
37
,  _parity(0)
38
,  _is_checksum_term(false)
39
,  _sentence_type(_GPS_SENTENCE_OTHER)
40
,  _term_number(0)
41
,  _term_offset(0)
42
,  _gps_data_good(false)
43
#ifndef _GPS_NO_STATS
44
,  _encoded_characters(0)
45
,  _good_sentences(0)
46
,  _failed_checksum(0)
47
#endif
48
{
49
  _term[0] = '\0';
50
}
51

    
52
//
53
// public methods
54
//
55

    
56
bool TinyGPS::encode(char c)
57
{
58
  bool valid_sentence = false;
59

    
60
  ++_encoded_characters;
61
  switch(c)
62
  {
63
  case ',': // term terminators
64
    _parity ^= c;
65
  case '\r':
66
  case '\n':
67
  case '*':
68
    if (_term_offset < sizeof(_term))
69
    {
70
      _term[_term_offset] = 0;
71
      valid_sentence = term_complete();
72
    }
73
    ++_term_number;
74
    _term_offset = 0;
75
    _is_checksum_term = c == '*';
76
    return valid_sentence;
77

    
78
  case '$': // sentence begin
79
    _term_number = _term_offset = 0;
80
    _parity = 0;
81
    _sentence_type = _GPS_SENTENCE_OTHER;
82
    _is_checksum_term = false;
83
    _gps_data_good = false;
84
    return valid_sentence;
85
  }
86

    
87
  // ordinary characters
88
  if (_term_offset < sizeof(_term) - 1)
89
    _term[_term_offset++] = c;
90
  if (!_is_checksum_term)
91
    _parity ^= c;
92

    
93
  return valid_sentence;
94
}
95

    
96
#ifndef _GPS_NO_STATS
97
void TinyGPS::stats(unsigned long *chars, unsigned short *sentences, unsigned short *failed_cs)
98
{
99
  if (chars) *chars = _encoded_characters;
100
  if (sentences) *sentences = _good_sentences;
101
  if (failed_cs) *failed_cs = _failed_checksum;
102
}
103
#endif
104

    
105
//
106
// internal utilities
107
//
108
int TinyGPS::from_hex(char a) 
109
{
110
  if (a >= 'A' && a <= 'F')
111
    return a - 'A' + 10;
112
  else if (a >= 'a' && a <= 'f')
113
    return a - 'a' + 10;
114
  else
115
    return a - '0';
116
}
117

    
118
unsigned long TinyGPS::parse_decimal()
119
{
120
  char *p = _term;
121
  bool isneg = *p == '-';
122
  if (isneg) ++p;
123
  unsigned long ret = 100UL * gpsatol(p);
124
  while (gpsisdigit(*p)) ++p;
125
  if (*p == '.')
126
  {
127
    if (gpsisdigit(p[1]))
128
    {
129
      ret += 10 * (p[1] - '0');
130
      if (gpsisdigit(p[2]))
131
        ret += p[2] - '0';
132
    }
133
  }
134
  return isneg ? -ret : ret;
135
}
136

    
137
unsigned long TinyGPS::parse_degrees()
138
{
139
  char *p;
140
  unsigned long left = gpsatol(_term);
141
  unsigned long tenk_minutes = (left % 100UL) * 10000UL;
142
  for (p=_term; gpsisdigit(*p); ++p);
143
  if (*p == '.')
144
  {
145
    unsigned long mult = 1000;
146
    while (gpsisdigit(*++p))
147
    {
148
      tenk_minutes += mult * (*p - '0');
149
      mult /= 10;
150
    }
151
  }
152
  return (left / 100) * 100000 + tenk_minutes / 6;
153
}
154

    
155
// Processes a just-completed term
156
// Returns true if new sentence has just passed checksum test and is validated
157
bool TinyGPS::term_complete()
158
{
159
  if (_is_checksum_term)
160
  {
161
    byte checksum = 16 * from_hex(_term[0]) + from_hex(_term[1]);
162
    if (checksum == _parity)
163
    {
164
      if (_gps_data_good)
165
      {
166
#ifndef _GPS_NO_STATS
167
        ++_good_sentences;
168
#endif
169
        _last_time_fix = _new_time_fix;
170
        _last_position_fix = _new_position_fix;
171

    
172
        switch(_sentence_type)
173
        {
174
        case _GPS_SENTENCE_GPRMC:
175
          _time      = _new_time;
176
          _date      = _new_date;
177
          _latitude  = _new_latitude;
178
          _longitude = _new_longitude;
179
          _speed     = _new_speed;
180
          _course    = _new_course;
181
          break;
182
        case _GPS_SENTENCE_GPGGA:
183
          _altitude  = _new_altitude;
184
          _time      = _new_time;
185
          _latitude  = _new_latitude;
186
          _longitude = _new_longitude;
187
          break;
188
        }
189

    
190
        return true;
191
      }
192
    }
193

    
194
#ifndef _GPS_NO_STATS
195
    else
196
      ++_failed_checksum;
197
#endif
198
    return false;
199
  }
200

    
201
  // the first term determines the sentence type
202
  if (_term_number == 0)
203
  {
204
    if (!gpsstrcmp(_term, _GPRMC_TERM))
205
      _sentence_type = _GPS_SENTENCE_GPRMC;
206
    else if (!gpsstrcmp(_term, _GPGGA_TERM))
207
      _sentence_type = _GPS_SENTENCE_GPGGA;
208
    else
209
      _sentence_type = _GPS_SENTENCE_OTHER;
210
    return false;
211
  }
212

    
213
  if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0])
214
  switch((_sentence_type == _GPS_SENTENCE_GPGGA ? 200 : 100) + _term_number)
215
  {
216
    case 101: // Time in both sentences
217
    case 201:
218
      _new_time = parse_decimal();
219
      _new_time_fix = millis();
220
      break;
221
    case 102: // GPRMC validity
222
      _gps_data_good = _term[0] == 'A';
223
      break;
224
    case 103: // Latitude
225
    case 202:
226
      _new_latitude = parse_degrees();
227
      _new_position_fix = millis();
228
      break;
229
    case 104: // N/S
230
    case 203:
231
      if (_term[0] == 'S')
232
        _new_latitude = -_new_latitude;
233
      break;
234
    case 105: // Longitude
235
    case 204:
236
      _new_longitude = parse_degrees();
237
      break;
238
    case 106: // E/W
239
    case 205:
240
      if (_term[0] == 'W')
241
        _new_longitude = -_new_longitude;
242
      break;
243
    case 107: // Speed (GPRMC)
244
      _new_speed = parse_decimal();
245
      break;
246
    case 108: // Course (GPRMC)
247
      _new_course = parse_decimal();
248
      break;
249
    case 109: // Date (GPRMC)
250
      _new_date = gpsatol(_term);
251
      break;
252
    case 206: // Fix data (GPGGA)
253
      _gps_data_good = _term[0] > '0';
254
      break;
255
    case 209: // Altitude (GPGGA)
256
      _new_altitude = parse_decimal();
257
      break;
258
  }
259

    
260
  return false;
261
}
262

    
263
long TinyGPS::gpsatol(const char *str)
264
{
265
  long ret = 0;
266
  while (gpsisdigit(*str))
267
    ret = 10 * ret + *str++ - '0';
268
  return ret;
269
}
270

    
271
int TinyGPS::gpsstrcmp(const char *str1, const char *str2)
272
{
273
  while (*str1 && *str1 == *str2)
274
    ++str1, ++str2;
275
  return *str1;
276
}