From ab458173107291371d7965733df09505852cb5d6 Mon Sep 17 00:00:00 2001 From: "Eric S. Raymond" Date: Tue, 11 Dec 2007 09:36:50 +0000 Subject: Integrated Garmin Simple Text Protocol driver from Petr Slansky. --- Makefile.am | 1 + configure.ac | 15 +++ drivers.c | 51 ++++++++ garmin_txt.c | 396 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ gpsd.h-tail | 2 + gpsd.spec.in | 3 + packet.c | 4 +- packet_states.h | 4 +- 8 files changed, 472 insertions(+), 4 deletions(-) create mode 100644 garmin_txt.c diff --git a/Makefile.am b/Makefile.am index c4729a45..99589b22 100644 --- a/Makefile.am +++ b/Makefile.am @@ -137,6 +137,7 @@ libgpsd_c_sources = \ zodiac.c \ ubx.c \ garmin.c \ + garmin_txt.c \ tsip.c \ evermore.c \ italk.c \ diff --git a/configure.ac b/configure.ac index 7e71f4bc..50198726 100644 --- a/configure.ac +++ b/configure.ac @@ -363,6 +363,19 @@ else AC_MSG_RESULT([no]) fi +dnl check for Garmin Simple Text support +AC_ARG_ENABLE(garmintxt, + AC_HELP_STRING([--disable-garmintxt], + [disable Garmin Simple Text support]), + [ac_garmintxt=$enableval], [ac_garmintxt=yes]) +AC_MSG_CHECKING([for Garmin Simple Text support]) +if test x"$ac_garmintxt" = "xyes"; then + AC_MSG_RESULT([yes]) + AC_DEFINE([GARMINTXT_ENABLE], 1, [Garmin Simple Text support]) +else + AC_MSG_RESULT([no]) +fi + dnl check for True North support AC_ARG_ENABLE(tnt, AC_HELP_STRING([--enable-tnt], @@ -685,6 +698,7 @@ echo "Earthmate : $ac_earthmate" echo "EverMore : $ac_evermore" echo "FV-18 : $ac_fv18" echo "Garmin : $ac_garmin" +echo "Garmin Simple Text : $ac_garmintxt" echo "iTrax : $ac_itrax" echo "NMEA : $ac_nmea" echo "NTRIP : $ac_ntrip" @@ -719,6 +733,7 @@ if test "xdummy" = "xdummy" -a \ x"$ac_evermore" = "xno" -a \ x"$ac_fv18" = "xno" -a \ x"$ac_garmin" = "xno" -a \ + x"$ac_garmintxt" = "xno" -a \ x"$ac_itrax" = "xno" -a \ x"$ac_navcom" = "xno" -a \ x"$ac_nmea" = "xno" -a \ diff --git a/drivers.c b/drivers.c index 2c95c205..dd868107 100644 --- a/drivers.c +++ b/drivers.c @@ -75,6 +75,15 @@ gps_mask_t nmea_parse_input(struct gps_device_t *session) #endif /* GARMIN_ENABLE */ } else if (session->packet.type == NMEA_PACKET) { gps_mask_t st = 0; +#ifdef GARMINTXT_ENABLE + if (session->packet.outbuflen >= 56) { + if ((char) *session->packet.outbuffer == '@') { + /* Garmin Simple Text packet received; it starts with '@' is terminated with \r\n and has length 57 bytes */ + (void)gpsd_switch_driver(session, "Garmin Simple Text"); + return garmintxt_parse(session); + } + } +#endif /* GARMINTXT_ENABLE */ gpsd_report(LOG_IO, "<= GPS: %s", session->packet.outbuffer); if ((st=nmea_parse((char *)session->packet.outbuffer, session))==0) { #ifdef NON_NMEA_ENABLE @@ -690,6 +699,45 @@ static struct gps_type_t rtcm104 = { }; #endif /* RTCM104_ENABLE */ +#ifdef GARMINTXT_ENABLE +/************************************************************************** + * + * Garmin Simple Text protocol + * + **************************************************************************/ + +static gps_mask_t garmintxt_parse_input(struct gps_device_t *session) +{ + //gpsd_report(LOG_PROG, "Garmin Simple Text packet\n"); + return garmintxt_parse(session); +} + + +static struct gps_type_t garmintxt = { + .typename = "Garmin Simple Text", /* full name of type */ + .trigger = NULL, /* no recognition string */ + .channels = 0, /* not used */ + .probe_wakeup = NULL, /* no wakeup to be done before hunt */ + .probe_detect = NULL, /* no probe */ + .probe_subtype = NULL, /* no subtypes */ +#ifdef ALLOW_RECONFIGURE + .configurator = NULL, /* no configurator */ +#endif /* ALLOW_RECONFIGURE */ + .get_packet = generic_get, /* how to get a packet */ + .parse_packet = garmintxt_parse_input, /* */ + .rtcm_writer = NULL, /* don't send RTCM data, */ + .speed_switcher= NULL, /* no speed switcher */ + .mode_switcher = NULL, /* no mode switcher */ + .rate_switcher = NULL, /* no sample-rate switcher */ + .cycle_chars = -1, /* not relevant, no rate switch */ +#ifdef ALLOW_RECONFIGURE + .revert = NULL, /* no setting-reversion method */ +#endif /* ALLOW_RECONFIGURE */ + .wrapup = NULL, /* no wrapup code */ + .cycle = 1, /* updates every second */ +}; +#endif /* GARMINTXT_ENABLE */ + extern struct gps_type_t garmin_usb_binary, garmin_ser_binary; extern struct gps_type_t sirf_binary, tsip_binary; extern struct gps_type_t evermore_binary, italk_binary; @@ -744,6 +792,9 @@ static struct gps_type_t *gpsd_driver_array[] = { #ifdef RTCM104_ENABLE &rtcm104, #endif /* RTCM104_ENABLE */ +#ifdef GARMINTXT_ENABLE + &garmintxt, +#endif /* GARMINTXT_ENABLE */ NULL, }; /*@ +nullassign @*/ diff --git a/garmin_txt.c b/garmin_txt.c new file mode 100644 index 00000000..c5d4bf28 --- /dev/null +++ b/garmin_txt.c @@ -0,0 +1,396 @@ +/* + * Handle the Garmin simple text format supported by some Garmins. + * Tested with the Garmin eTrex Legend. + * + * Protocol info from: + * http://gpsd.berlios.de/vendor-docs/garmin/garmin_simpletext.txt + * http://www.garmin.com/support/commProtocol.html + * + * Code by: Petr Slansky + * all rights abandoned, a thank would be nice if you use this code. + * + * -D 3 = packet trace + * -D 4 = packet details + * -D 5 = more packet details + * -D 6 = very excessive details + * + * limitations: + * very simple protocol, only very basic information + * TODO + * do not have from garmin: + * pdop + * vdop + * magnetic variation + * satellite information + * + */ + +/*************************************************** +Garmin Simple Text Output Format: + +The simple text (ASCII) output contains time, position, and velocity data in +the fixed width fields (not delimited) defined in the following table: + + FIELD DESCRIPTION: WIDTH: NOTES: + ----------------------- ------- ------------------------ + Sentence start 1 Always '@' + ----------------------- ------- ------------------------ + /Year 2 Last two digits of UTC year + | ----------------------- ------- ------------------------ + | Month 2 UTC month, "01".."12" +T | ----------------------- ------- ------------------------ +i | Day 2 UTC day of month, "01".."31" +m | ----------------------- ------- ------------------------ +e | Hour 2 UTC hour, "00".."23" + | ----------------------- ------- ------------------------ + | Minute 2 UTC minute, "00".."59" + | ----------------------- ------- ------------------------ + \Second 2 UTC second, "00".."59" + ----------------------- ------- ------------------------ + /Latitude hemisphere 1 'N' or 'S' + | ----------------------- ------- ------------------------ + | Latitude position 7 WGS84 ddmmmmm, with an implied + | decimal after the 4th digit + | ----------------------- ------- ------------------------ + | Longitude hemishpere 1 'E' or 'W' + | ----------------------- ------- ------------------------ + | Longitude position 8 WGS84 dddmmmmm with an implied +P | decimal after the 5th digit +o | ----------------------- ------- ------------------------ +s | Position status 1 'd' if current 2D differential GPS position +i | 'D' if current 3D differential GPS position +t | 'g' if current 2D GPS position +i | 'G' if current 3D GPS position +o | 'S' if simulated position +n | '_' if invalid position + | ----------------------- ------- ------------------------ + | Horizontal posn error 3 EPH in meters + | ----------------------- ------- ------------------------ + | Altitude sign 1 '+' or '-' + | ----------------------- ------- ------------------------ + | Altitude 5 Height above or below mean + \ sea level in meters + ----------------------- ------- ------------------------ + /East/West velocity 1 'E' or 'W' + | direction + | ----------------------- ------- ------------------------ + | East/West velocity 4 Meters per second in tenths, + | magnitude ("1234" = 123.4 m/s) +V | ----------------------- ------- ------------------------ +e | North/South velocity 1 'N' or 'S' +l | direction +o | ----------------------- ------- ------------------------ +c | North/South velocity 4 Meters per second in tenths, +i | magnitude ("1234" = 123.4 m/s) +t | ----------------------- ------- ------------------------ +y | Vertical velocity 1 'U' (up) or 'D' (down) + | direction + | ----------------------- ------- ------------------------ + | Vertical velocity 4 Meters per second in hundredths, + \ magnitude ("1234" = 12.34 m/s) + ----------------------- ------- ------------------------ + Sentence end 2 Carriage return, '0x0D', and + line feed, '0x0A' + ----------------------- ------- ------------------------ + +If a numeric value does not fill its entire field width, the field is padded +with leading '0's (eg. an altitude of 50 meters above MSL will be output as +"+00050"). + +Any or all of the data in the text sentence (except for the sentence start +and sentence end fields) may be replaced with underscores to indicate +invalid data. + +***************************************************/ + +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include "gpsd_config.h" +#if defined (HAVE_SYS_SELECT_H) +#include +#endif + +#if defined(HAVE_STRINGS_H) +#include +#endif + +#include "gpsd.h" +#include "gps.h" +#include "timebase.h" + +#ifdef GARMINTXT_ENABLE + +/* Simple text message is fixed length, 55 chars text data + 2 characters EOL */ +/* buffer for text processing */ +#define TXT_BUFFER_SIZE 13 + +/************************************************************************** + * decode text string to double number, translate prefix to sign + * return 0: OK + * -1: data error + * -2: data not valid + * + * examples: + + * gar_decode(cbuf, 9, "EW", 100000.0, &result); + * E01412345 -> +14.12345 + + * gar_decode(cbuf, 9, "EW", 100000.0, &result); + * W01412345 -> -14.12345 + + * gar_decode(cbuf, 3, "", 10.0, &result); + * 123 -> +12.3 + +**************************************************************************/ +static int gar_decode(const char *data, const unsigned int length, const char *prefix, const double dividor, double *result) +{ + char buf[10]; + float sign = 1.0; + int preflen = strlen(prefix); + int offset = 1; /* assume one character prefix (E,W,S,N,U, D, etc) */ + int intresult; + + if (length >= sizeof(buf)) { + gpsd_report(LOG_ERROR, "internal buffer too small\n"); + return -1; + } + + bzero(buf, sizeof(buf)); + (void) strncpy(buf, data, length); + gpsd_report(LOG_RAW, "Decoded string: %s\n", buf); + + if (strchr(buf, '_') != NULL) { + /* value is not valid, ignore it */ + return -2; + } + + /* parse prefix */ + do { + if (preflen == 0 ) { + offset = 0; /* only number, no prefix */ + break; + } + /* second character in prefix is flag for negative number */ + if (preflen >= 2 ) { + if (buf[0] == prefix[1]) { + sign = -1.0; + break; + } + } + /* first character in prefix is flag for positive number */ + if (preflen >= 1 ) { + if (buf[0] == prefix[0]) { + sign = 1.0; + break; + } + } + gpsd_report(LOG_WARN, "Unexpected char \"%c\" in data \"%s\"\n", buf[0], buf); + return -1; + } while (0); + + if (strspn(buf+offset, "0123456789") != length-offset) { + gpsd_report(LOG_WARN, "Invalid value %s\n", buf); + return -1; + } + + intresult = atoi(buf+offset); + if (intresult == 0) sign = 0.0; /* don't create negatove zero */ + + *result = (double) intresult / dividor * sign; + + return 0; /* SUCCESS */ +} +/************************************************************************** + * decode integer from string, check if the result is in expected range + * return 0: OK + * -1: data error + * -2: data not valid +**************************************************************************/ +static int gar_int_decode(const char *data, const unsigned int length, const int min, const int max, int *result) +{ + char buf[3]; + int res; + + if (length >= sizeof(buf)) { + gpsd_report(LOG_ERROR, "internal buffer too small\n"); + return -1; + } + + bzero(buf, sizeof(buf)); + (void) strncpy(buf, data, length); + gpsd_report(LOG_RAW, "Decoded string: %s\n", buf); + + if (strchr(buf, '_') != NULL) { + /* value is not valid, ignore it */ + return -2; + } + + if (strspn(buf, "0123456789") != length) { + gpsd_report(LOG_WARN, "Invalid value %s\n", buf); + return -1; + } + + res = atoi(buf); + if ((res >= min) && (res <= max)) { + *result = res; + return 0; /* SUCCESS */ + } else { + gpsd_report(LOG_WARN, "Value %d out of range <%d, %d>\n", res, min, max); + return -1; + } +} + + +/************************************************************************** + * + * Entry points begin here + * + **************************************************************************/ + +gps_mask_t garmintxt_parse(struct gps_device_t *session) +{ +/* parse GARMIN Simple Text sentence, unpack it into a session structure */ + + gps_mask_t mask = 0; + + gpsd_report(LOG_PROG, "Garmin Simple Text packet, len %d\n", session->packet.outbuflen); + gpsd_report(LOG_RAW, "%s\n", gpsd_hexdump(session->packet.outbuffer, session->packet.outbuflen)); + + if (session->packet.outbuflen < 56) { + gpsd_report(LOG_WARN, "Message too short, rejected.\n"); + return ONLINE_SET; + } + + session->packet.type=GARMINTXT_PACKET; + strncpy(session->gpsdata.tag, "GTXT", MAXTAGLEN); /* TAG mesage as GTXT, Garmin Simple Text Message; any better idea? */ + + mask |= CYCLE_START_SET; /* only one message, set cycle start */ + + do { + int result; + char *buf = (char *)session->packet.outbuffer+1; + gpsd_report(LOG_PROG, "Timestamp: %.12s\n", buf); + + /* year */ + if (0 != gar_int_decode(buf+0, 2, 0, 99, &result)) break; + session->driver.nmea.date.tm_year = (CENTURY_BASE + result) - 1900; + /* month */ + if (0 != gar_int_decode(buf+2, 2, 1, 12, &result)) break; + session->driver.nmea.date.tm_mon = result-1; + /* day */ + if (0 != gar_int_decode(buf+4, 2, 1, 31, &result)) break; + session->driver.nmea.date.tm_mday = result; + /* hour */ + if (0 != gar_int_decode(buf+6, 2, 0, 23, &result)) break; + session->driver.nmea.date.tm_hour = result; /* mday update?? */ + /* minute */ + if (0 != gar_int_decode(buf+8, 2, 0, 59, &result)) break; + session->driver.nmea.date.tm_min = result; + /* second */ + if (0 != gar_int_decode(buf+10, 2, 0, 59, &result)) break; + session->driver.nmea.date.tm_sec = result; + session->driver.nmea.subseconds = 0; + session->gpsdata.fix.time = (double)mkgmtime(&session->driver.nmea.date)+session->driver.nmea.subseconds; + mask |= TIME_SET; + } while (0); + + + /* process position */ + + do { + double lat, lon; + + /* Latitude, [NS]ddmmmmm */ + if (0 != gar_decode((char *) session->packet.outbuffer+13, 8, "NS", 100000.0, &lat)) break; + /* Longitude, [EW]dddmmmmm */ + if (0 != gar_decode((char *) session->packet.outbuffer+21, 9, "EW", 100000.0, &lon)) break; + session->gpsdata.fix.latitude = lat; + session->gpsdata.fix.longitude = lon; + gpsd_report(LOG_PROG, "Lat: %.5lf, Lon: %.5lf\n", lat, lon); + mask |= LATLON_SET; + } while (0); + + /* fix mode, GPS status, [gGdDS_] */ + do { + char status = session->packet.outbuffer[30]; + gpsd_report(LOG_PROG, "GPS fix mode: %c\n", status); + + switch (status) { + case 'D': + case 'G': + case 'S': /* DEMO mode, assume 3D position */ + session->gpsdata.fix.mode = MODE_3D; + session->gpsdata.status = STATUS_FIX; + if (status == 'D') session->gpsdata.status = STATUS_DGPS_FIX; + break; + case 'd': + case 'g': + session->gpsdata.fix.mode = MODE_2D; + session->gpsdata.status = STATUS_FIX; + if (status == 'd') session->gpsdata.status = STATUS_DGPS_FIX; + break; + default: + session->gpsdata.fix.mode = MODE_NO_FIX; + session->gpsdata.status = STATUS_NO_FIX; + } + mask |= MODE_SET | STATUS_SET; + } while (0); + + /* EPH */ + do { + double eph; + if (0 != gar_decode((char *) session->packet.outbuffer+31, 3, "", 1.0, &eph)) break; + eph = eph * (GPSD_CONFIDENCE/CEP50_SIGMA); + session->gpsdata.fix.eph = eph; + gpsd_report(LOG_PROG, "HERR: %.1lf\n", eph); + mask |= HERR_SET; + } while (0); + + /* Altitude */ + do { + double alt; + if (0 != gar_decode((char *) session->packet.outbuffer+34, 6, "+-", 1.0, &alt)) break; + session->gpsdata.fix.altitude = alt; + gpsd_report(LOG_PROG, "Altitude [m]: %.1lf\n", alt); + mask |= ALTITUDE_SET; + } while (0); + + /* Velocity */ + do { + double ewvel, nsvel, speed, track; + if (0 != gar_decode((char *) session->packet.outbuffer+40, 5, "EW", 10.0, &ewvel)) break; + if (0 != gar_decode((char *) session->packet.outbuffer+45, 5, "NS", 10.0, &nsvel)) break; + speed = sqrt(ewvel * ewvel + nsvel * nsvel); /* is this correct formula? Result is in mps */ + session->gpsdata.fix.speed = speed; + gpsd_report(LOG_PROG, "Velocity [mps]: %.1lf\n", speed); + track = atan2(ewvel, nsvel) * RAD_2_DEG; /* is this correct formula? Result is in degrees */ + if (track < 0.0) track += 360.0; + session->gpsdata.fix.track = track; + gpsd_report(LOG_PROG, "Heading [degree]: %.1lf\n", track); + mask |= SPEED_SET | TRACK_SET; + } while (0); + + + /* Climb (vertical velocity) */ + do { + double climb; + if (0 != gar_decode((char *) session->packet.outbuffer+50, 5, "UD", 100.0, &climb)) break; + session->gpsdata.fix.climb = climb; /* climb in mps */ + gpsd_report(LOG_PROG, "Climb [mps]: %.2lf\n", climb); + mask |= CLIMB_SET; + } while (0); + + return mask; +} + +#endif /* GARMINTXT_ENABLE */ + diff --git a/gpsd.h-tail b/gpsd.h-tail index 985fc9fe..33c4b37e 100644 --- a/gpsd.h-tail +++ b/gpsd.h-tail @@ -63,6 +63,7 @@ struct gps_packet_t { #define GARMIN_PACKET 8 #define NAVCOM_PACKET 9 #define UBX_PACKET 10 +#define GARMINTXT_PACKET 11 unsigned int state; size_t length; unsigned char inbuffer[MAX_PACKET_LENGTH*2+1]; @@ -348,6 +349,7 @@ extern gps_mask_t sirf_parse(struct gps_device_t *, unsigned char *, size_t); extern gps_mask_t evermore_parse(struct gps_device_t *, unsigned char *, size_t); extern gps_mask_t navcom_parse(struct gps_device_t *, unsigned char *, size_t); extern gps_mask_t garmin_ser_parse(struct gps_device_t *); +extern gps_mask_t garmintxt_parse(struct gps_device_t *); extern bool dgnss_url(char *); extern int dgnss_open(struct gps_context_t *, char *); diff --git a/gpsd.spec.in b/gpsd.spec.in index e2638eda..519198e9 100644 --- a/gpsd.spec.in +++ b/gpsd.spec.in @@ -159,6 +159,9 @@ cp gpsd.hotplug gpsd.usermap "$RPM_BUILD_ROOT"%{_sysconfdir}/hotplug/usb/ %{_libdir}/X11/app-defaults/xgpsspeed %changelog +* Tue Dec 11 2007 Eric S. Raymond - 2.36dev-1 +- Integrated Garmin Simple Text Protocol driver from Peter Slanky. + * Mon Dec 10 2007 Eric S. Raymond - 2.35-1 - Navcom driver merged. Removed -d -f and -p options of gpsd; these have been undocumented for a while. Make gpsd play well with pkgconfig. diff --git a/packet.c b/packet.c index 5c0c94cd..b088af48 100644 --- a/packet.c +++ b/packet.c @@ -94,7 +94,7 @@ static void nextstate(struct gps_packet_t *lexer, break; } #endif /* NMEA_ENABLE */ -#ifdef TNT_ENABLE +#if defined(TNT_ENABLE) || defined(GARMINTXT_ENABLE) if (c == '@') { lexer->state = TNT_LEADER; break; @@ -220,7 +220,7 @@ static void nextstate(struct gps_packet_t *lexer, else lexer->state = GROUND_STATE; break; -#ifdef TNT_ENABLE +#if defined(TNT_ENABLE) || defined(GARMINTXT_ENABLE) case TNT_LEADER: lexer->state = NMEA_LEADER_END; break; diff --git a/packet_states.h b/packet_states.h index 42d03627..b3a0af95 100644 --- a/packet_states.h +++ b/packet_states.h @@ -64,8 +64,9 @@ ZODIAC_RECOGNIZED, /* found end of the Zodiac packet */ #endif /* ZODIAC_ENABLE */ -#ifdef TNT_ENABLE +#if defined(TNT_ENABLE) || defined(GARMINTXT_ENABLE) TNT_LEADER, /* saw True North status leader '@' */ + /* Garmin Simple Text starts with @ leader */ #endif #ifdef EVERMORE_ENABLE @@ -131,5 +132,4 @@ RTCM_RECOGNIZED, /* we have an RTCM packet */ #endif /* RTCM104_ENABLE */ - /* end of packet_states.h */ -- cgit v1.2.1