src/main/tracker/TinyGPS.c

   1 /*
   2 TinyGPS - a small GPS library for Arduino providing basic NMEA parsing
   3 Based on work by and "distance_to" and "course_to" courtesy of Maarten Lamers.
   4 Suggestion to add satellites(), course_to(), and cardinal(), by Matt Monson.
   5 Precision improvements suggested by Wayne Holder.
   6 Copyright (C) 2008-2013 Mikal Hart
   7 All rights reserved.
   8
   9 This library is free software; you can redistribute it and/or
  10 modify it under the terms of the GNU Lesser General Public
  11 License as published by the Free Software Foundation; either
  12 version 2.1 of the License, or (at your option) any later version.
  13
  14 This library is distributed in the hope that it will be useful,
  15 but WITHOUT ANY WARRANTY; without even the implied warranty of
  16 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  17 Lesser General Public License for more details.
  18
  19 You should have received a copy of the GNU Lesser General Public
  20 License along with this library; if not, write to the Free Software
  21 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  22 */
  23 #include <stdbool.h>
  24 #include <stdint.h>
  25 #include "TinyGPS.h"
  26 #include "Arduino.h"
  27
  28 #define COMBINE(sentence_type, term_number) (((unsigned)(sentence_type) << 5) | term_number)
  29
  30
  31
  32 int from_hex(char a);
  33 unsigned long parse_decimal();
  34 unsigned long parse_degrees();
  35 bool term_complete();
  36 long gpsatol(const char *str);
  37 float distance_between (float lat1, float long1, float lat2, float long2);
  38 float course_to (float lat1, float long1, float lat2, float long2);
  39 const char *cardinal (float course);
  40 void get_position(long *latitude, long *longitude, unsigned long *fix_age);
  41 void get_datetime(unsigned long *date, unsigned long *time, unsigned long *age);
  42 void f_get_position(float *latitude, float *longitude, unsigned long *fix_age);
  43 void crack_datetime(int *year, uint8_t *month, uint8_t *day,
  44   uint8_t *hour, uint8_t *minute, uint8_t *second, uint8_t *hundredths, unsigned long *age);
  45 float f_altitude();
  46 float f_course();
  47 float f_speed_knots();
  48 float f_speed_mph();
  49 float f_speed_mps();
  50 float f_speed_kmph();
  51 bool gpsisdigit(char c);
  52   //_term[0] = '\0';
  53 uint8_t get_sentence_type(void);
  54
  55
  56 //
  57 // public methods
  58 //
  59 /*static const float GPS_INVALID_F_ANGLE = 1000.0;
  60 static const float GPS_INVALID_F_ALTITUDE = 1000000.0;
  61 static const float GPS_INVALID_F_SPEED = -1.0;*/
  62
  63 bool TinyGPS_encode(char c)
  64 {
  65   bool valid_sentence = false;
  66
  67 #ifndef _GPS_NO_STATS
  68   ++_encoded_characters;
  69 #endif
  70   switch(c)
  71   {
  72   case ',': // term terminators
  73     _parity ^= c;
  74   case '\r':
  75   case '\n':
  76   case '*':
  77     if (_term_offset < sizeof(_term))
  78     {
  79       _term[_term_offset] = 0;
  80       valid_sentence = term_complete();
  81     }
  82     ++_term_number;
  83     _term_offset = 0;
  84     _is_checksum_term = c == '*';
  85     return valid_sentence;
  86
  87   case '$': // sentence begin
  88     _term_number = _term_offset = 0;
  89     _parity = 0;
  90     _sentence_type = _GPS_SENTENCE_OTHER;
  91     _is_checksum_term = false;
  92     _gps_data_good = false;
  93     return valid_sentence;
  94   }
  95
  96   // ordinary characters
  97   if (_term_offset < sizeof(_term) - 1)
  98     _term[_term_offset++] = c;
  99   if (!_is_checksum_term)
 100     _parity ^= c;
 101
 102   return valid_sentence;
 103 }
 104
 105 #ifndef _GPS_NO_STATS
 106 void TinyGPS_stats(unsigned long *chars, unsigned short *sentences, unsigned short *failed_cs)
 107 {
 108   if (chars) *chars = _encoded_characters;
 109   if (sentences) *sentences = _good_sentences;
 110   if (failed_cs) *failed_cs = _failed_checksum;
 111 }
 112 #endif
 113
 114 //
 115 // internal utilities
 116 //
 117 int from_hex(char a)
 118 {
 119   if (a >= 'A' && a <= 'F')
 120     return a - 'A' + 10;
 121   else if (a >= 'a' && a <= 'f')
 122     return a - 'a' + 10;
 123   else
 124     return a - '0';
 125 }
 126
 127 unsigned long parse_decimal()
 128 {
 129   char *p = _term;
 130   bool isneg = *p == '-';
 131   if (isneg) ++p;
 132   unsigned long ret = 100UL * gpsatol(p);
 133   while (gpsisdigit(*p)) ++p;
 134   if (*p == '.')
 135   {
 136     if (gpsisdigit(p[1]))
 137     {
 138       ret += 10 * (p[1] - '0');
 139       if (gpsisdigit(p[2]))
 140         ret += p[2] - '0';
 141     }
 142   }
 143   return isneg ? -ret : ret;
 144 }
 145
 146 // Parse a string in the form ddmm.mmmmmmm...
 147 unsigned long parse_degrees()
 148 {
 149   char *p;
 150   unsigned long left_of_decimal = gpsatol(_term);
 151   unsigned long hundred1000ths_of_minute = (left_of_decimal % 100UL) * 100000UL;
 152   for (p=_term; gpsisdigit(*p); ++p);
 153   if (*p == '.')
 154   {
 155     unsigned long mult = 10000;
 156     while (gpsisdigit(*++p))
 157     {
 158       hundred1000ths_of_minute += mult * (*p - '0');
 159       mult /= 10;
 160     }
 161   }
 162   return (left_of_decimal / 100) * 1000000 + (hundred1000ths_of_minute + 3) / 6;
 163 }
 164
 165
 166
 167 // Processes a just-completed term
 168 // Returns true if new sentence has just passed checksum test and is validated
 169 bool term_complete()
 170 {
 171   if (_is_checksum_term)
 172   {
 173     uint8_t checksum = 16 * from_hex(_term[0]) + from_hex(_term[1]);
 174     if (checksum == _parity)
 175     {
 176       if (_gps_data_good)
 177       {
 178 #ifndef _GPS_NO_STATS
 179         ++_good_sentences;
 180 #endif
 181         _last_time_fix = _new_time_fix;
 182         _last_position_fix = _new_position_fix;
 183
 184         switch(_sentence_type)
 185         {
 186         case _GPS_SENTENCE_GPRMC:
 187           _time      = _new_time;
 188           _date      = _new_date;
 189           _latitude  = _new_latitude;
 190           _longitude = _new_longitude;
 191           _speed     = _new_speed;
 192           _course    = _new_course;
 193           break;
 194         case _GPS_SENTENCE_GPGGA:
 195         case _GPS_SENTENCE_GNGGA:
 196           _altitude  = _new_altitude;
 197           _time      = _new_time;
 198           _latitude  = _new_latitude;
 199           _longitude = _new_longitude;
 200           _fixtype = _new_fixtype;
 201           _numsats   = _new_numsats;
 202           _hdop      = _new_hdop;
 203           break;
 204         }
 205
 206         return true;
 207       }
 208     }
 209
 210 #ifndef _GPS_NO_STATS
 211     else
 212       ++_failed_checksum;
 213 #endif
 214     return false;
 215   }
 216
 217   // the first term determines the sentence type
 218   if (_term_number == 0)
 219   {
 220     if (!gpsstrcmp(_term, _GPRMC_TERM))
 221       _sentence_type = _GPS_SENTENCE_GPRMC;
 222     else if (!gpsstrcmp(_term, _GPGGA_TERM))
 223       _sentence_type = _GPS_SENTENCE_GPGGA;
 224     else if (!gpsstrcmp(_term, _GNGGA_TERM))
 225           _sentence_type = _GPS_SENTENCE_GNGGA;
 226     else
 227       _sentence_type = _GPS_SENTENCE_OTHER;
 228     return false;
 229   }
 230
 231   if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0])
 232     switch(COMBINE(_sentence_type, _term_number))
 233   {
 234     case COMBINE(_GPS_SENTENCE_GPRMC, 1): // Time in both sentences
 235     case COMBINE(_GPS_SENTENCE_GPGGA, 1):
 236     case COMBINE(_GPS_SENTENCE_GNGGA, 1):
 237       _new_time = parse_decimal();
 238       _new_time_fix = millis();
 239       break;
 240     case COMBINE(_GPS_SENTENCE_GPRMC, 2): // GPRMC validity
 241       _gps_data_good = _term[0] == 'A';
 242       break;
 243     case COMBINE(_GPS_SENTENCE_GPRMC, 3): // Latitude
 244     case COMBINE(_GPS_SENTENCE_GPGGA, 2):
 245     case COMBINE(_GPS_SENTENCE_GNGGA, 2):
 246       _new_latitude = parse_degrees();
 247       _new_position_fix = millis();
 248       break;
 249     case COMBINE(_GPS_SENTENCE_GPRMC, 4): // N/S
 250     case COMBINE(_GPS_SENTENCE_GPGGA, 3):
 251     case COMBINE(_GPS_SENTENCE_GNGGA, 3):
 252       if (_term[0] == 'S')
 253         _new_latitude = -_new_latitude;
 254       break;
 255     case COMBINE(_GPS_SENTENCE_GPRMC, 5): // Longitude
 256     case COMBINE(_GPS_SENTENCE_GPGGA, 4):
 257     case COMBINE(_GPS_SENTENCE_GNGGA, 4):
 258       _new_longitude = parse_degrees();
 259       break;
 260     case COMBINE(_GPS_SENTENCE_GPRMC, 6): // E/W
 261     case COMBINE(_GPS_SENTENCE_GPGGA, 5):
 262     case COMBINE(_GPS_SENTENCE_GNGGA, 5):
 263       if (_term[0] == 'W')
 264         _new_longitude = -_new_longitude;
 265       break;
 266     case COMBINE(_GPS_SENTENCE_GPRMC, 7): // Speed (GPRMC)
 267       _new_speed = parse_decimal();
 268       break;
 269     case COMBINE(_GPS_SENTENCE_GPRMC, 8): // Course (GPRMC)
 270       _new_course = parse_decimal();
 271       break;
 272     case COMBINE(_GPS_SENTENCE_GPRMC, 9): // Date (GPRMC)
 273       _new_date = gpsatol(_term);
 274       break;
 275     case COMBINE(_GPS_SENTENCE_GPGGA, 6): // Fix data (GPGGA)
 276     case COMBINE(_GPS_SENTENCE_GNGGA, 6):
 277       _gps_data_good = _term[0] > '0';
 278       _new_fixtype = (unsigned char)atoi(_term);
 279       break;
 280     case COMBINE(_GPS_SENTENCE_GPGGA, 7): // Satellites used (GPGGA)
 281     case COMBINE(_GPS_SENTENCE_GNGGA, 7):
 282       _new_numsats = (unsigned char)atoi(_term);
 283       break;
 284     case COMBINE(_GPS_SENTENCE_GPGGA, 8): // HDOP
 285     case COMBINE(_GPS_SENTENCE_GNGGA, 8):
 286       _new_hdop = parse_decimal();
 287       break;
 288     case COMBINE(_GPS_SENTENCE_GPGGA, 9): // Altitude (GPGGA)
 289     case COMBINE(_GPS_SENTENCE_GNGGA, 9):
 290       _new_altitude = parse_decimal();
 291       break;
 292   }
 293
 294   return false;
 295 }
 296
 297 long gpsatol(const char *str)
 298 {
 299   long ret = 0;
 300   while (gpsisdigit(*str))
 301     ret = 10 * ret + *str++ - '0';
 302   return ret;
 303 }
 304
 305 int gpsstrcmp(const char *str1, const char *str2)
 306 {
 307   while (*str1 && *str1 == *str2)
 308     ++str1, ++str2;
 309   return *str1;
 310 }
 311
 312 /* static */
 313 float distance_between (float lat1, float long1, float lat2, float long2)
 314 {
 315   // returns distance in meters between two positions, both specified
 316   // as signed decimal-degrees latitude and longitude. Uses great-circle
 317   // distance computation for hypothetical sphere of radius 6372795 meters.
 318   // Because Earth is no exact sphere, rounding errors may be up to 0.5%.
 319   // Courtesy of Maarten Lamers
 320   float delta = radians(long1-long2);
 321   float sdlong = sin(delta);
 322   float cdlong = cos(delta);
 323   lat1 = radians(lat1);
 324   lat2 = radians(lat2);
 325   float slat1 = sin(lat1);
 326   float clat1 = cos(lat1);
 327   float slat2 = sin(lat2);
 328   float clat2 = cos(lat2);
 329   delta = (clat1 * slat2) - (slat1 * clat2 * cdlong);
 330   delta = sq(delta);
 331   delta += sq(clat2 * sdlong);
 332   delta = sqrt(delta);
 333   float denom = (slat1 * slat2) + (clat1 * clat2 * cdlong);
 334   delta = atan2(delta, denom);
 335   return delta * 6372795;
 336 }
 337
 338 float course_to (float lat1, float long1, float lat2, float long2)
 339 {
 340   // returns course in degrees (North=0, West=270) from position 1 to position 2,
 341   // both specified as signed decimal-degrees latitude and longitude.
 342   // Because Earth is no exact sphere, calculated course may be off by a tiny fraction.
 343   // Courtesy of Maarten Lamers
 344   float dlon = radians(long2-long1);
 345   lat1 = radians(lat1);
 346   lat2 = radians(lat2);
 347   float a1 = sin(dlon) * cos(lat2);
 348   float a2 = sin(lat1) * cos(lat2) * cos(dlon);
 349   a2 = cos(lat1) * sin(lat2) - a2;
 350   a2 = atan2(a1, a2);
 351   if (a2 < 0.0)
 352   {
 353     a2 += TWO_PI;
 354   }
 355   return degrees(a2);
 356 }
 357
 358 const char *cardinal (float course)
 359 {
 360   static const char* directions[] = {"N", "NNE", "NE", "ENE", "E", "ESE", "SE", "SSE", "S", "SSW", "SW", "WSW", "W", "WNW", "NW", "NNW"};
 361
 362   int direction = (int)((course + 11.25f) / 22.5f);
 363   return directions[direction % 16];
 364 }
 365
 366 // lat/long in MILLIONTHs of a degree and age of fix in milliseconds
 367 // (note: versions 12 and earlier gave this value in 100,000ths of a degree.
 368 void get_position(long *latitude, long *longitude, unsigned long *fix_age)
 369 {
 370   if (latitude) *latitude = _latitude;
 371   if (longitude) *longitude = _longitude;
 372   if (fix_age) *fix_age = _last_position_fix == GPS_INVALID_FIX_TIME ?
 373    GPS_INVALID_AGE : millis() - _last_position_fix;
 374 }
 375
 376 // date as ddmmyy, time as hhmmsscc, and age in milliseconds
 377 void get_datetime(unsigned long *date, unsigned long *time, unsigned long *age)
 378 {
 379   if (date) *date = _date;
 380   if (time) *time = _time;
 381   if (age) *age = _last_time_fix == GPS_INVALID_FIX_TIME ?
 382    GPS_INVALID_AGE : millis() - _last_time_fix;
 383 }
 384
 385 void f_get_position(float *latitude, float *longitude, unsigned long *fix_age)
 386 {
 387   long lat, lon;
 388   get_position(&lat, &lon, fix_age);
 389   *latitude = lat == GPS_INVALID_ANGLE ? GPS_INVALID_F_ANGLE : (lat / 1000000.0);
 390   *longitude = lat == GPS_INVALID_ANGLE ? GPS_INVALID_F_ANGLE : (lon / 1000000.0);
 391 }
 392
 393 uint8_t f_fixtype(void){
 394         return _fixtype;
 395 }
 396
 397 void crack_datetime(int *year, uint8_t *month, uint8_t *day,
 398   uint8_t *hour, uint8_t *minute, uint8_t *second, uint8_t *hundredths, unsigned long *age)
 399 {
 400   unsigned long date, time;
 401   get_datetime(&date, &time, age);
 402   if (year)
 403   {
 404     *year = date % 100;
 405     *year += *year > 80 ? 1900 : 2000;
 406   }
 407   if (month) *month = (date / 100) % 100;
 408   if (day) *day = date / 10000;
 409   if (hour) *hour = time / 1000000;
 410   if (minute) *minute = (time / 10000) % 100;
 411   if (second) *second = (time / 100) % 100;
 412   if (hundredths) *hundredths = time % 100;
 413 }
 414
 415 float f_altitude(void)
 416 {
 417   return _altitude == GPS_INVALID_ALTITUDE ? GPS_INVALID_F_ALTITUDE : _altitude / 100.0;
 418 }
 419
 420 float f_course(void)
 421 {
 422   return _course == GPS_INVALID_ANGLE ? GPS_INVALID_F_ANGLE : _course / 100.0;
 423 }
 424
 425 float f_speed_knots(void)
 426 {
 427   return _speed == GPS_INVALID_SPEED ? GPS_INVALID_F_SPEED : _speed / 100.0;
 428 }
 429
 430 float f_speed_mph(void)
 431 {
 432   float sk = f_speed_knots();
 433   return sk == GPS_INVALID_F_SPEED ? GPS_INVALID_F_SPEED : _GPS_MPH_PER_KNOT * sk;
 434 }
 435
 436 float f_speed_mps(void)
 437 {
 438   float sk = f_speed_knots();
 439   return sk == GPS_INVALID_F_SPEED ? GPS_INVALID_F_SPEED : _GPS_MPS_PER_KNOT * sk;
 440 }
 441
 442 float f_speed_kmph(void)
 443 {
 444   float sk = f_speed_knots();
 445   return sk == GPS_INVALID_F_SPEED ? GPS_INVALID_F_SPEED : _GPS_KMPH_PER_KNOT * sk;
 446 }
 447
 448 float f_hdop(void)
 449 {
 450         _hdop == GPS_INVALID_HDOP ? GPS_INVALID_HDOP : _hdop / 100.0;
 451 }
 452
 453 bool gpsisdigit(char c) {
 454         return (c >= '0' && c <= '9');
 455 }
 456
 457 uint8_t get_sentence_type(void){
 458         return _sentence_type;
 459 }
 460 uint8_t satellites()
 461 {
 462         return _numsats;
 463 }