/* * 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 */