diff options
author | Stanley Huang <stanleyhuangyc@gmail.com> | 2013-06-10 13:21:54 +0800 |
---|---|---|
committer | Stanley Huang <stanleyhuangyc@gmail.com> | 2013-06-10 13:21:54 +0800 |
commit | f99f21926e515cf93e2af35eb82808f337998738 (patch) | |
tree | ba931f65405704517acc543a4e96fc521fe3212e /obdlogger/TinyGPS.cpp | |
parent | 47c6ff979fea8051c02fb38fe99c0021880a4ed8 (diff) | |
download | 2021-arduino-obd-f99f21926e515cf93e2af35eb82808f337998738.tar.gz 2021-arduino-obd-f99f21926e515cf93e2af35eb82808f337998738.tar.bz2 2021-arduino-obd-f99f21926e515cf93e2af35eb82808f337998738.zip |
update OBD logger
Diffstat (limited to 'obdlogger/TinyGPS.cpp')
-rw-r--r-- | obdlogger/TinyGPS.cpp | 426 |
1 files changed, 0 insertions, 426 deletions
diff --git a/obdlogger/TinyGPS.cpp b/obdlogger/TinyGPS.cpp deleted file mode 100644 index 00a683c..0000000 --- a/obdlogger/TinyGPS.cpp +++ /dev/null @@ -1,426 +0,0 @@ -/* -TinyGPS - a small GPS library for Arduino providing basic NMEA parsing -Based on work by and "distance_to" and "course_to" courtesy of Maarten Lamers. -Suggestion to add satellites(), course_to(), and cardinal(), by Matt Monson. -Copyright (C) 2008-2012 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 "TinyGPS.h" - -#define _GPRMC_TERM "GPRMC" -#define _GPGGA_TERM "GPGGA" - -TinyGPS::TinyGPS() - : _time(GPS_INVALID_TIME) - , _date(GPS_INVALID_DATE) - , _latitude(GPS_INVALID_ANGLE) - , _longitude(GPS_INVALID_ANGLE) - , _altitude(GPS_INVALID_ALTITUDE) - , _speed(GPS_INVALID_SPEED) - , _course(GPS_INVALID_ANGLE) - , _hdop(GPS_INVALID_HDOP) - , _numsats(GPS_INVALID_SATELLITES) - , _last_time_fix(GPS_INVALID_FIX_TIME) - , _last_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) -#ifndef _GPS_NO_STATS - , _encoded_characters(0) - , _good_sentences(0) - , _failed_checksum(0) -#endif -{ - _term[0] = '\0'; -} - -// -// public methods -// - -bool TinyGPS::encode(char c) -{ - bool valid_sentence = false; - -#ifndef _GPS_NO_STATS - ++_encoded_characters; -#endif - switch(c) - { - case ',': // term terminators - _parity ^= c; - 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 == '*'; - return valid_sentence; - - case '$': // sentence begin - _term_number = _term_offset = 0; - _parity = 0; - _sentence_type = _GPS_SENTENCE_OTHER; - _is_checksum_term = false; - _gps_data_good = false; - return valid_sentence; - } - - // ordinary characters - if (_term_offset < sizeof(_term) - 1) - _term[_term_offset++] = c; - if (!_is_checksum_term) - _parity ^= c; - - return valid_sentence; -} - -#ifndef _GPS_NO_STATS -void TinyGPS::stats(unsigned long *chars, unsigned short *sentences, unsigned short *failed_cs) -{ - if (chars) *chars = _encoded_characters; - if (sentences) *sentences = _good_sentences; - if (failed_cs) *failed_cs = _failed_checksum; -} -#endif - -// -// internal utilities -// -int TinyGPS::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'; -} - -unsigned long TinyGPS::parse_decimal() -{ - char *p = _term; - bool isneg = *p == '-'; - if (isneg) ++p; - unsigned long ret = 100UL * gpsatol(p); - while (gpsisdigit(*p)) ++p; - if (*p == '.') - { - if (gpsisdigit(p[1])) - { - ret += 10 * (p[1] - '0'); - if (gpsisdigit(p[2])) - ret += p[2] - '0'; - } - } - return isneg ? -ret : ret; -} - -unsigned long TinyGPS::parse_degrees() -{ - char *p; - unsigned long left = gpsatol(_term); - unsigned long tenk_minutes = (left % 100UL) * 10000UL; - for (p=_term; gpsisdigit(*p); ++p); - if (*p == '.') - { - unsigned long mult = 1000; - while (gpsisdigit(*++p)) - { - tenk_minutes += mult * (*p - '0'); - mult /= 10; - } - } - return (left / 100) * 100000 + tenk_minutes / 6; -} - -#define COMBINE(sentence_type, term_number) (((unsigned)(sentence_type) << 5) | term_number) - -// Processes a just-completed term -// Returns true if new sentence has just passed checksum test and is validated -bool TinyGPS::term_complete() -{ - if (_is_checksum_term) - { - byte checksum = 16 * from_hex(_term[0]) + from_hex(_term[1]); - if (checksum == _parity) - { - if (_gps_data_good) - { -#ifndef _GPS_NO_STATS - ++_good_sentences; -#endif - _last_time_fix = _new_time_fix; - _last_position_fix = _new_position_fix; - - switch(_sentence_type) - { - case _GPS_SENTENCE_GPRMC: - _time = _new_time; - _date = _new_date; - _latitude = _new_latitude; - _longitude = _new_longitude; - _speed = _new_speed; - _course = _new_course; - break; - case _GPS_SENTENCE_GPGGA: - _altitude = _new_altitude; - _time = _new_time; - _latitude = _new_latitude; - _longitude = _new_longitude; - _numsats = _new_numsats; - _hdop = _new_hdop; - break; - } - - return true; - } - } - -#ifndef _GPS_NO_STATS - else - ++_failed_checksum; -#endif - return false; - } - - // the first term determines the sentence type - if (_term_number == 0) - { - if (!gpsstrcmp(_term, _GPRMC_TERM)) - _sentence_type = _GPS_SENTENCE_GPRMC; - else if (!gpsstrcmp(_term, _GPGGA_TERM)) - _sentence_type = _GPS_SENTENCE_GPGGA; - else - _sentence_type = _GPS_SENTENCE_OTHER; - return false; - } - - if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) - switch(COMBINE(_sentence_type, _term_number)) - { - case COMBINE(_GPS_SENTENCE_GPRMC, 1): // Time in both sentences - case COMBINE(_GPS_SENTENCE_GPGGA, 1): - _new_time = parse_decimal(); - _new_time_fix = millis(); - break; - case COMBINE(_GPS_SENTENCE_GPRMC, 2): // GPRMC validity - _gps_data_good = _term[0] == 'A'; - break; - case COMBINE(_GPS_SENTENCE_GPRMC, 3): // Latitude - case COMBINE(_GPS_SENTENCE_GPGGA, 2): - _new_latitude = parse_degrees(); - _new_position_fix = millis(); - break; - case COMBINE(_GPS_SENTENCE_GPRMC, 4): // N/S - case COMBINE(_GPS_SENTENCE_GPGGA, 3): - if (_term[0] == 'S') - _new_latitude = -_new_latitude; - break; - case COMBINE(_GPS_SENTENCE_GPRMC, 5): // Longitude - case COMBINE(_GPS_SENTENCE_GPGGA, 4): - _new_longitude = parse_degrees(); - break; - case COMBINE(_GPS_SENTENCE_GPRMC, 6): // E/W - case COMBINE(_GPS_SENTENCE_GPGGA, 5): - if (_term[0] == 'W') - _new_longitude = -_new_longitude; - break; - case COMBINE(_GPS_SENTENCE_GPRMC, 7): // Speed (GPRMC) - _new_speed = parse_decimal(); - break; - case COMBINE(_GPS_SENTENCE_GPRMC, 8): // Course (GPRMC) - _new_course = parse_decimal(); - break; - case COMBINE(_GPS_SENTENCE_GPRMC, 9): // Date (GPRMC) - _new_date = gpsatol(_term); - break; - case COMBINE(_GPS_SENTENCE_GPGGA, 6): // Fix data (GPGGA) - _gps_data_good = _term[0] > '0'; - break; - case COMBINE(_GPS_SENTENCE_GPGGA, 7): // Satellites used (GPGGA) - _new_numsats = (unsigned char)atoi(_term); - break; - case COMBINE(_GPS_SENTENCE_GPGGA, 8): // HDOP - _new_hdop = parse_decimal(); - break; - case COMBINE(_GPS_SENTENCE_GPGGA, 9): // Altitude (GPGGA) - _new_altitude = parse_decimal(); - break; - } - - return false; -} - -long TinyGPS::gpsatol(const char *str) -{ - long ret = 0; - while (gpsisdigit(*str)) - ret = 10 * ret + *str++ - '0'; - return ret; -} - -int TinyGPS::gpsstrcmp(const char *str1, const char *str2) -{ - while (*str1 && *str1 == *str2) - ++str1, ++str2; - return *str1; -} - -/* static */ -float TinyGPS::distance_between (float lat1, float long1, float lat2, float long2) -{ - // returns distance in meters between two positions, both specified - // as signed decimal-degrees latitude and longitude. Uses great-circle - // distance computation for hypothetical sphere of radius 6372795 meters. - // Because Earth is no exact sphere, rounding errors may be up to 0.5%. - // Courtesy of Maarten Lamers - float delta = radians(long1-long2); - float sdlong = sin(delta); - float cdlong = cos(delta); - lat1 = radians(lat1); - lat2 = radians(lat2); - float slat1 = sin(lat1); - float clat1 = cos(lat1); - float slat2 = sin(lat2); - float clat2 = cos(lat2); - delta = (clat1 * slat2) - (slat1 * clat2 * cdlong); - delta = sq(delta); - delta += sq(clat2 * sdlong); - delta = sqrt(delta); - float denom = (slat1 * slat2) + (clat1 * clat2 * cdlong); - delta = atan2(delta, denom); - return delta * 6372795; -} - -float TinyGPS::course_to (float lat1, float long1, float lat2, float long2) -{ - // returns course in degrees (North=0, West=270) from position 1 to position 2, - // both specified as signed decimal-degrees latitude and longitude. - // Because Earth is no exact sphere, calculated course may be off by a tiny fraction. - // Courtesy of Maarten Lamers - float dlon = radians(long2-long1); - lat1 = radians(lat1); - lat2 = radians(lat2); - float a1 = sin(dlon) * cos(lat2); - float a2 = sin(lat1) * cos(lat2) * cos(dlon); - a2 = cos(lat1) * sin(lat2) - a2; - a2 = atan2(a1, a2); - if (a2 < 0.0) - { - a2 += TWO_PI; - } - return degrees(a2); -} - -const char *TinyGPS::cardinal (float course) -{ - static const char* directions[] = {"N", "NNE", "NE", "ENE", "E", "ESE", "SE", "SSE", "S", "SSW", "SW", "WSW", "W", "WNW", "NW", "NNW"}; - - int direction = (int)((course + 11.25f) / 22.5f); - return directions[direction % 16]; -} - -// lat/long in hundred thousandths of a degree and age of fix in milliseconds -void TinyGPS::get_position(long *latitude, long *longitude, unsigned long *fix_age) -{ - if (latitude) *latitude = _latitude; - if (longitude) *longitude = _longitude; - if (fix_age) *fix_age = _last_position_fix == GPS_INVALID_FIX_TIME ? -GPS_INVALID_AGE : millis() - _last_position_fix; -} - -// date as ddmmyy, time as hhmmsscc, and age in milliseconds -void TinyGPS::get_datetime(unsigned long *date, unsigned long *time, unsigned long *age) -{ - if (date) *date = _date; - if (time) *time = _time; - if (age) *age = _last_time_fix == GPS_INVALID_FIX_TIME ? -GPS_INVALID_AGE : millis() - _last_time_fix; -} - -void TinyGPS::f_get_position(float *latitude, float *longitude, unsigned long *fix_age) -{ - long lat, lon; - get_position(&lat, &lon, fix_age); - *latitude = lat == GPS_INVALID_ANGLE ? GPS_INVALID_F_ANGLE : (lat / 100000.0); - *longitude = lat == GPS_INVALID_ANGLE ? GPS_INVALID_F_ANGLE : (lon / 100000.0); -} - -void TinyGPS::crack_datetime(int *year, byte *month, byte *day, - byte *hour, byte *minute, byte *second, byte *hundredths, unsigned long *age) -{ - unsigned long date, time; - get_datetime(&date, &time, age); - if (year) - { - *year = date % 100; - *year += *year > 80 ? 1900 : 2000; - } - if (month) *month = (date / 100) % 100; - if (day) *day = date / 10000; - if (hour) *hour = time / 1000000; - if (minute) *minute = (time / 10000) % 100; - if (second) *second = (time / 100) % 100; - if (hundredths) *hundredths = time % 100; -} - -float TinyGPS::f_altitude() -{ - return _altitude == GPS_INVALID_ALTITUDE ? GPS_INVALID_F_ALTITUDE : _altitude / 100.0; -} - -float TinyGPS::f_course() -{ - return _course == GPS_INVALID_ANGLE ? GPS_INVALID_F_ANGLE : _course / 100.0; -} - -float TinyGPS::f_speed_knots() -{ - return _speed == GPS_INVALID_SPEED ? GPS_INVALID_F_SPEED : _speed / 100.0; -} - -float TinyGPS::f_speed_mph() -{ - float sk = f_speed_knots(); - return sk == GPS_INVALID_F_SPEED ? GPS_INVALID_F_SPEED : _GPS_MPH_PER_KNOT * f_speed_knots(); -} - -float TinyGPS::f_speed_mps() -{ - float sk = f_speed_knots(); - return sk == GPS_INVALID_F_SPEED ? GPS_INVALID_F_SPEED : _GPS_MPS_PER_KNOT * f_speed_knots(); -} - -float TinyGPS::f_speed_kmph() -{ - float sk = f_speed_knots(); - return sk == GPS_INVALID_F_SPEED ? GPS_INVALID_F_SPEED : _GPS_KMPH_PER_KNOT * f_speed_knots(); -} - -const float TinyGPS::GPS_INVALID_F_ANGLE = 1000.0; -const float TinyGPS::GPS_INVALID_F_ALTITUDE = 1000000.0; -const float TinyGPS::GPS_INVALID_F_SPEED = -1.0; |