From f9dacd3f634df224bf8ea93ecb991fef390512aa Mon Sep 17 00:00:00 2001 From: Chris Kuethe Date: Mon, 2 Mar 2009 20:04:36 +0000 Subject: repository beautification, part 2: rename the packet drivers to driver_$(protocol).c --- Makefile.am | 62 +-- driver_evermore.c | 522 +++++++++++++++++++++ driver_garmin.c | 1247 +++++++++++++++++++++++++++++++++++++++++++++++++ driver_garmin_txt.c | 405 ++++++++++++++++ driver_italk.c | 527 +++++++++++++++++++++ driver_italk.h | 498 ++++++++++++++++++++ driver_navcom.c | 1276 +++++++++++++++++++++++++++++++++++++++++++++++++++ driver_nmea.c | 936 +++++++++++++++++++++++++++++++++++++ driver_rtcm2.c | 739 +++++++++++++++++++++++++++++ driver_rtcm2.h | 497 ++++++++++++++++++++ driver_rtcm3.c | 473 +++++++++++++++++++ driver_sirf.c | 1037 +++++++++++++++++++++++++++++++++++++++++ driver_superstar2.c | 528 +++++++++++++++++++++ driver_superstar2.h | 59 +++ driver_tsip.c | 856 ++++++++++++++++++++++++++++++++++ driver_ubx.c | 688 +++++++++++++++++++++++++++ driver_ubx.h | 101 ++++ driver_zodiac.c | 496 ++++++++++++++++++++ evermore.c | 522 --------------------- garmin.c | 1247 ------------------------------------------------- garmin_txt.c | 405 ---------------- italk.c | 527 --------------------- italk.h | 498 -------------------- navcom.c | 1276 --------------------------------------------------- nmea_parse.c | 936 ------------------------------------- rtcm2.c | 739 ----------------------------- rtcm2.h | 497 -------------------- rtcm3.c | 473 ------------------- sirf.c | 1037 ----------------------------------------- superstar2.c | 528 --------------------- superstar2.h | 59 --- tsip.c | 856 ---------------------------------- ubx.c | 688 --------------------------- ubx.h | 101 ---- zodiac.c | 496 -------------------- 35 files changed, 10916 insertions(+), 10916 deletions(-) create mode 100644 driver_evermore.c create mode 100644 driver_garmin.c create mode 100644 driver_garmin_txt.c create mode 100644 driver_italk.c create mode 100644 driver_italk.h create mode 100644 driver_navcom.c create mode 100644 driver_nmea.c create mode 100644 driver_rtcm2.c create mode 100644 driver_rtcm2.h create mode 100644 driver_rtcm3.c create mode 100644 driver_sirf.c create mode 100644 driver_superstar2.c create mode 100644 driver_superstar2.h create mode 100644 driver_tsip.c create mode 100644 driver_ubx.c create mode 100644 driver_ubx.h create mode 100644 driver_zodiac.c delete mode 100644 evermore.c delete mode 100644 garmin.c delete mode 100644 garmin_txt.c delete mode 100644 italk.c delete mode 100644 italk.h delete mode 100644 navcom.c delete mode 100644 nmea_parse.c delete mode 100644 rtcm2.c delete mode 100644 rtcm2.h delete mode 100644 rtcm3.c delete mode 100644 sirf.c delete mode 100644 superstar2.c delete mode 100644 superstar2.h delete mode 100644 tsip.c delete mode 100644 ubx.c delete mode 100644 ubx.h delete mode 100644 zodiac.c diff --git a/Makefile.am b/Makefile.am index d86a39d7..eb8288bb 100644 --- a/Makefile.am +++ b/Makefile.am @@ -118,47 +118,47 @@ libgps_la_LDFLAGS = -version-number 17:0:0 lib_LTLIBRARIES = libgps.la libgpsd_c_sources = \ - netlib.c \ - nmea_parse.c \ - serial.c \ - libgpsd_core.c \ - subframe.c \ - ntpshm.c \ - libgps.c \ - packet.c \ - gpsutils.c \ - geoid.c \ + bits.c \ + bsd-base64.c \ + crc24q.c \ dgnss.c \ dgpsip.c \ + gpsutils.c \ + geoid.c \ + hex.c \ + isgps.c \ + libgps.c \ + libgpsd_core.c \ + netlib.c \ + ntpshm.c \ ntrip.c \ - sirf.c \ - superstar2.c \ + packet.c \ report.c \ - isgps.c \ - rtcm2.c \ - rtcm3.c \ + serial.c \ srecord.c \ - bsd-base64.c \ strl.c \ - hex.c \ + subframe.c \ drivers.c \ - zodiac.c \ - ubx.c \ - garmin.c \ - garmin_txt.c \ - tsip.c \ - evermore.c \ - italk.c \ - navcom.c \ - bits.c \ - crc24q.c + driver_evermore.c \ + driver_garmin.c \ + driver_garmin_txt.c \ + driver_italk.c \ + driver_navcom.c \ + driver_nmea.c \ + driver_rtcm2.c \ + driver_rtcm3.c \ + driver_sirf.c \ + driver_superstar2.c \ + driver_tsip.c \ + driver_ubx.c \ + driver_zodiac.c libgpsd_h_sources = \ - italk.h \ + driver_italk.h \ bsd-base64.h \ timebase.h \ bits.h \ - rtcm2.h \ + driver_rtcm2.h \ crc24q.h BUILT_SOURCES = packet_names.h gpsd.h @@ -173,7 +173,7 @@ gpsd.h: gpsd.h-head gpsd.h-tail gpsd_config.h cat $(srcdir)/gpsd.h-tail >>gpsd.h libgps_la_SOURCES = $(libgpsd_c_sources) $(libgpsd_h_sources) \ - rtcm2.h libgpsmm.cpp packet_states.h + driver_rtcm2.h libgpsmm.cpp packet_states.h nodist_libgps_la_SOURCES = packet_names.h libgps_la_LIBADD = $(LIBM) $(LIBC) $(LIBNSL) $(LIBSOCKET) $(LIBPTHREAD) @@ -289,7 +289,7 @@ gpsflash.1: gpsflash.xml endif -noinst_HEADERS = ubx.h superstar2.h +noinst_HEADERS = driver_ubx.h driver_superstar2.h nodist_include_HEADERS = gpsd.h include_HEADERS = gps.h libgpsmm.h diff --git a/driver_evermore.c b/driver_evermore.c new file mode 100644 index 00000000..41031121 --- /dev/null +++ b/driver_evermore.c @@ -0,0 +1,522 @@ +/* $Id$ */ +/* + * This is the gpsd driver for EverMore GPSes operating in binary mode. + * About the only thing this gives us that NMEA won't is TDOP. + * But we'll get atomic position reports from it, which is good. + * + * The vendor site is . + * + * This driver was written by Petr Slansky based on a framework by Eric S. + * Raymond. The following remarks are by Petr Slansky. + * + * Snooping on the serial the communication between a Windows program and + * an Evermore chipset reveals some messages not described in the vendor + * documentation (Issue C of Aug 2002): + * + * 10 02 06 84 00 00 00 84 10 03 switch to binary mode (84 00 00 00) + * 10 02 06 84 01 00 00 85 10 03 switch to NMEA mode (84 01 00 00) + * + * 10 02 06 89 01 00 00 8a 10 03 set baud rate 4800 + * 10 02 06 89 01 01 00 8b 10 03 set baud rate 9600 + * 10 02 06 89 01 02 00 8c 10 03 set baud rate 19200 + * 10 02 06 89 01 03 00 8d 10 03 set baud rate 38400 + * + * 10 02 06 8D 00 01 00 8E 10 03 switch to datum ID 001 (WGS-84) + * 10 02 06 8D 00 D8 00 65 10 03 switch to datum ID 217 (WGS-72) + * + * These don't entail a reset of GPS as the 0x80 message does. + * + * 10 02 04 38 85 bd 10 03 answer from GPS to 0x85 message; ACK message + * 10 02 04 38 8d c5 10 03 answer from GPS to 0x8d message; ACK message + * 10 02 04 38 8e c6 10 03 answer from GPS to 0x8e message; ACK message + * 10 02 04 38 8f c7 10 03 answer from GPS to 0x8f message; ACK message + * + * The chip sometimes sends vendor extension messages with the prefix + * $PEMT,100. After restart, it sends a $PEMT,100 message describing the + * chip's configuration. Here is a sample: + * + * $PEMT,100,05.42g,100303,180,05,1,20,15,08,0,0,2,1*5A + * 100 - message type + * 05.42g - firmware version + * 100303 - date of firmware release DDMMYY + * 180 - datum ID; 001 is WGS-84 + * 05 - default elevation mask; see message 0x86 + * 1 - default DOP select (1 is auto DOP mask); see message 0x87 + * 20 - default GDOP; see message 0x87 + * 15 - default PDOP + * 08 - default HDOP + * 0 - Normal mode, without 1PPS + * 0 - default position pinning control (0 disable, 1 enable) + * 2 - altitude hold mode (0 disable, 1 always, 2 auto) + * 1 - 2/1 satellite nav mode (0,1,2,3,4) + * 0 disable 2/1 sat nav mode + * 1 hold direction (2 sat) + * 2 clock hold only (2 sat) + * 3 direction hold then clock hold (1 sat) + * 4 clock hold then direction hold (1 sat) + * + * Message $PEMT,100 could be forced with message 0x85 (restart): + * 10 02 12 85 00 00 00 00 00 01 01 00 00 00 00 00 00 00 00 87 10 03 + * 0x85 ID, Restart + * 0x00 restart mode (0 default, 1 hot, 2 warm, 3 cold, 4 test) + * 0x00 test start search PRN (1-32) + * 0x00 UTC second (0-59) + * 0x00 UTC Minute (0-59) + * 0x00 UTC Hour (0-23) + * 0x01 UTC Day (1-31) + * 0x01 UTC Month (1-12) + * 0x0000 UTC year (1980+x, uint16) + * 0x0000 Latitude WGS-84 (+/-900, 1/10 degree, + for N, int16) + * 0x0000 Longtitude WGS-84 (+/-1800, 1/10 degree, + for E, int16) + * 0x0000 Altitude WGS-84 (-1000..+18000, meters, int16) + * 0x87 CRC + * + * With message 0x8e it is possible to define how often each NMEA + * message is sent (0-255 seconds). It is possible with message 0x8e + * to activate PEMT,101 messages that have information about time, + * position, velocity and HDOP. + * + * $PEMT,101,1,02,00.0,300906190446,5002.5062,N,01427.6166,E,00259,000,0000*27 + * $PEMT,101,2,06,02.1,300906185730,5002.7546,N,01426.9524,E,00323,020,0011*26 + * 101 - message type, Compact Navigation Solution + * 2 - position status (1,2,3,4,5,6) + * (1 invalid, 2 2D fix, 3 3D fix, 4 2D with DIFF, 5 3D with DIFF, + * 6 2/1 sat degrade mode) + * 06 - number of used satelites + * 02.1 - DOP (00.0 no fix, HDOP 2D fix, PDOP 3D fix) + * 300906185730 - date and time, UTC ddmmyyHHMMSS (30/09/2006 18:57:30) + * 5002.7546,N - Latitude (degree) + * 01426.9524,E - Longitude (degree) + * 00323 - Altitude (323 metres) + * 020 - heading (20 degrees from true north) + * 0011 - speed over ground (11 metres per second); documentation says km per h + * + * This is an exampe of an 0x8e message that activates all NMEA sentences + * with 1s period: + * 10 02 12 8E 7F 01 01 01 01 01 01 01 01 00 00 00 00 00 00 15 10 03 + * + * There is a way to probe for this chipset. When binary message 0x81 is sent: + * 10 02 04 81 13 94 10 03 + * + * EverMore will reply with message like this: + * *10 *02 *0D *20 E1 00 00 *00 0A 00 1E 00 32 00 5B *10 *03 + * bytes marked with * are fixed + * Message in reply is information about logging configuration of GPS + * + * Another way to detect the EverMore chipset is to send one of the messages + * 0x85, 0x8d, 0x8e or 0x8f and check for a reply. + * The reply message from an EverMore GPS will look like this: + * *10 *02 *04 *38 8d c5 *10 *03 + * 8d indicates that message 0x8d was sent; + * c5 is EverMore checksum, other bytes are fixed + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "gpsd_config.h" +#include "gpsd.h" +#if defined(EVERMORE_ENABLE) && defined(BINARY_ENABLE) + +#define GET_ORIGIN 1 +#define PUT_ORIGIN 0 +#include "bits.h" + +#define EVERMORE_CHANNELS 12 + +/*@ +charint -usedef -compdef @*/ +static ssize_t evermore_control_send(struct gps_device_t *session, char *buf, size_t len) +{ + unsigned int crc; + size_t i; + char *cp; + + /*@ +charint +ignoresigns @*/ + /* prepare a DLE-stuffed copy of the message */ + cp = session->msgbuf; + *cp++ = 0x10; /* message starts with DLE STX */ + *cp++ = 0x02; + + session->msgbuflen = (size_t)(len + 2); /* len < 254 !! */ + *cp++ = (char)session->msgbuflen; /* message length */ + if (session->msgbuflen == 0x10) *cp++ = 0x10; + + /* payload */ + crc = 0; + for (i = 0; i < len; i++) { + *cp++ = buf[i]; + if (buf[i] == 0x10) + *cp++ = 0x10; + crc += buf[i]; + } + + crc &= 0xff; + + /* enter CRC after payload */ + *cp++ = crc; + if (crc == 0x10) + *cp++ = 0x10; + + *cp++ = 0x10; /* message ends with DLE ETX */ + *cp++ = 0x03; + + session->msgbuflen = (size_t)(cp - session->msgbuf); + /*@ -charint -ignoresigns @*/ + +#ifdef ALLOW_RECONFIGURE + return gpsd_write(session, session->msgbuf, session->msgbuflen); +#else + return -1; +#endif /* ALLOW_RECONFIGURE */ +} +/*@ -charint +usedef +compdef @*/ + +/*@ +charint @*/ +gps_mask_t evermore_parse(struct gps_device_t *session, unsigned char *buf, size_t len) +{ + unsigned char buf2[MAX_PACKET_LENGTH], *cp, *tp; + size_t i, datalen; + unsigned int used, visible, satcnt; + double version; + gps_mask_t mask = 0; + + if (len == 0) + return 0; + + /* time to unstuff it and discard the header and footer */ + cp = buf + 2; + tp = buf2; + if (*cp == 0x10) cp++; + datalen = (size_t)*cp++; + + gpsd_report(LOG_RAW, "raw EverMore packet type 0x%02x, length %zd: %s\n", + *cp, len, gpsd_hexdump_wrapper(buf, len, LOG_RAW)); + + datalen -= 2; + + for (i = 0; i < (size_t)datalen; i++) { + *tp = *cp++; + if (*tp == 0x10) cp++; + tp++; + } + + /*@ -usedef -compdef @*/ + gpsd_report(LOG_RAW, "EverMore packet type 0x%02x, length %zd: %s\n", + buf2[0], datalen, gpsd_hexdump_wrapper(buf2, datalen, LOG_RAW)); + /*@ +usedef +compdef @*/ + + (void)snprintf(session->gpsdata.tag, sizeof(session->gpsdata.tag), + "EID%d",(int)buf2[0]); + + switch (getub(buf2, 1)) + { + case 0x02: /* Navigation Data Output */ + session->gpsdata.fix.time = session->gpsdata.sentence_time + = gpstime_to_unix((int)getleuw(buf2, 2), getleul(buf2, 4)*0.01) - session->context->leap_seconds; + ecef_to_wgs84fix(&session->gpsdata, + getlesl(buf2, 8)*1.0, getlesl(buf2, 12)*1.0, getlesl(buf2, 16)*1.0, + getlesw(buf2, 20)/10.0, getlesw(buf2, 22)/10.0, getlesw(buf2, 24)/10.0); + used = getub(buf2, 26) & 0x0f; + visible = (getub(buf2, 26) & 0xf0) >> 4; + version = getleuw(buf2, 27)/100.0; + /* that's all the information in this packet */ + if (used < 3) + session->gpsdata.fix.mode = MODE_NO_FIX; + else if (used == 3) + session->gpsdata.fix.mode = MODE_2D; + else { + session->gpsdata.fix.mode = MODE_3D; + mask |= ALTITUDE_SET | CLIMB_SET; + } + gpsd_report(LOG_PROG, "NDO 0x02: version %3.2f, mode=%d, status=%d, visible=%d, used=%d\n", + version, + session->gpsdata.fix.mode, + session->gpsdata.status, + visible, + used); + mask |= TIME_SET | LATLON_SET | TRACK_SET | SPEED_SET | MODE_SET | CYCLE_START_SET; + if (session->subtype[0] == '\0') { + (void)snprintf(session->subtype, sizeof(session->subtype), + "%3.2f", version); + mask |= DEVICEID_SET; + } + return mask; + + case 0x04: /* DOP Data Output */ + session->gpsdata.fix.time = session->gpsdata.sentence_time + = gpstime_to_unix((int)getleuw(buf2, 2), getleul(buf2, 4)*0.01) - session->context->leap_seconds; + session->gpsdata.gdop = (double)getub(buf2, 8)*0.1; + session->gpsdata.pdop = (double)getub(buf2, 9)*0.1; + session->gpsdata.hdop = (double)getub(buf2, 10)*0.1; + session->gpsdata.vdop = (double)getub(buf2, 11)*0.1; + session->gpsdata.tdop = (double)getub(buf2, 12)*0.1; + switch (getub(buf2, 13)) { + case 0: /* no position fix */ + case 1: /* manual calls this "1D navigation" */ + session->gpsdata.status = STATUS_NO_FIX; + session->gpsdata.fix.mode = MODE_NO_FIX; + break; + case 2: /* 2D navigation */ + session->gpsdata.status = STATUS_FIX; + session->gpsdata.fix.mode = MODE_2D; + break; + case 3: /* 3D navigation */ + session->gpsdata.status = STATUS_FIX; + session->gpsdata.fix.mode = MODE_3D; + break; + case 4: /* 3D navigation with DGPS */ + session->gpsdata.status = STATUS_DGPS_FIX; + session->gpsdata.fix.mode = MODE_3D; + break; + } + /* that's all the information in this packet */ + gpsd_report(LOG_PROG, "DDO 0x04: mode=%d, status=%d\n", + session->gpsdata.fix.mode, + session->gpsdata.status); + return TIME_SET | DOP_SET | MODE_SET | STATUS_SET; + + case 0x06: /* Channel Status Output */ + session->gpsdata.fix.time = session->gpsdata.sentence_time + = gpstime_to_unix((int)getleuw(buf2, 2), getleul(buf2, 4)*0.01) - session->context->leap_seconds; + session->gpsdata.satellites = (int)getub(buf2, 8); + session->gpsdata.satellites_used = 0; + memset(session->gpsdata.used, 0, sizeof(session->gpsdata.used)); + if (session->gpsdata.satellites > 12) { + gpsd_report(LOG_WARN, "Warning: EverMore packet has information about %d satellites!\n", + session->gpsdata.satellites); + } + if (session->gpsdata.satellites > EVERMORE_CHANNELS) session->gpsdata.satellites = EVERMORE_CHANNELS; + satcnt = 0; + for (i = 0; i < (size_t)session->gpsdata.satellites; i++) { + int prn; + // channel = getub(buf2, 7*i+7+2) + prn = (int)getub(buf2, 7*i+7+3); + if (prn == 0) continue; /* satellite record is not valid */ + session->gpsdata.PRN[satcnt] = prn; + session->gpsdata.azimuth[satcnt] = (int)getleuw(buf2, 7*i+7+4); + session->gpsdata.elevation[satcnt] = (int)getub(buf2, 7*i+7+6); + session->gpsdata.ss[satcnt] = (int)getub(buf2, 7*i+7+7); + /* + * Status bits at offset 8: + * bit0 = 1 satellite acquired + * bit1 = 1 code-tracking loop locked + * bit2 = 1 carrier-tracking loop locked + * bit3 = 1 data-bit synchronization done + * bit4 = 1 frame synchronization done + * bit5 = 1 ephemeris data collected + * bit6 = 1 used for position fix + */ + if (getub(buf2, 7*i+7+8) & 0x40) { + session->gpsdata.used[session->gpsdata.satellites_used++]=prn; + } + + satcnt++; + + } + session->gpsdata.satellites = (int)satcnt; + /* that's all the information in this packet */ + gpsd_report(LOG_PROG, "CSO 0x04: %d satellites used\n", + session->gpsdata.satellites_used); + return TIME_SET | SATELLITE_SET | USED_SET; + + case 0x08: /* Measurement Data Output */ + /* clock offset is a manufacturer diagnostic */ + /* (int)getleuw(buf2, 8); clock offset, 29000..29850 ?? */ + session->gpsdata.fix.time = session->gpsdata.sentence_time + = gpstime_to_unix((int)getleuw(buf2, 2), getleul(buf2, 4)*0.01) - session->context->leap_seconds; + visible = getub(buf2, 10); + /* FIXME: read full statellite status for each channel */ + /* we can get pseudo range (m), delta-range (m/s), doppler (Hz) and status for each channel */ + /* gpsd_report(LOG_PROG, "MDO 0x04: visible=%d\n", visible); */ + gpsd_report(LOG_PROG, "MDO 0x04:\n"); + return TIME_SET; + + case 0x20: /* LogConfig Info, could be used as a probe for EverMore GPS */ + gpsd_report(LOG_IO, "LogConfig EverMore packet, length %zd: %s\n", + datalen, gpsd_hexdump_wrapper(buf2, datalen, LOG_IO)); + return ONLINE_SET; + + case 0x22: /* LogData */ + gpsd_report(LOG_IO, "LogData EverMore packet, length %zd: %s\n", + datalen, gpsd_hexdump_wrapper(buf2, datalen, LOG_IO)); + return ONLINE_SET; + + case 0x38: /* ACK */ + gpsd_report(LOG_PROG, "EverMore command %02X ACK\n", getub(buf2, 2)); + return ONLINE_SET; + + default: + gpsd_report(LOG_WARN, + "unknown EverMore packet id 0x%02x, length %zd: %s\n", buf2[0], + datalen, gpsd_hexdump_wrapper(buf2, datalen, LOG_IO)); + return 0; + } +} +/*@ -charint @*/ + +static gps_mask_t evermore_parse_input(struct gps_device_t *session) +{ + gps_mask_t st; + + if (session->packet.type == EVERMORE_PACKET){ + st = evermore_parse(session, session->packet.outbuffer, session->packet.outbuflen); + return st; +#ifdef NMEA_ENABLE + } else if (session->packet.type == NMEA_PACKET) { + st = nmea_parse((char *)session->packet.outbuffer, session); + return st; +#endif /* NMEA_ENABLE */ + } else + return 0; +} + +static bool evermore_speed(struct gps_device_t *session, + speed_t speed, char parity, int stopbits) +{ + /*@ -type @*/ + gpsd_report(LOG_PROG, "evermore_speed(%d%c%d)\n", speed, parity, stopbits); + /* parity and stopbit switching aren't available on this chip */ + if (parity!=session->gpsdata.parity || stopbits!=session->gpsdata.parity) { + return false; + } else { + unsigned char tmp8; + char msg[] = { + 0x89, /* 0: msg ID, Serial Port Configuration */ + 0x01, /* 1: bit 0 cfg for main serial, bit 1 cfg for DGPS port */ + 0x00, /* 2: baud rate for main serial; 4800(0), 9600(1), 19200(2), 38400(3) */ + 0x00, /* 3: baud rate for DGPS serial port; 4800(0), 9600(1), etc */ + }; + switch (speed) { + case 4800: tmp8 = 0; break; + case 9600: tmp8 = 1; break; + case 19200: tmp8 = 2; break; + case 38400: tmp8 = 3; break; + default: return false; + } + msg[2] = tmp8; + return (evermore_control_send(session, msg, sizeof(msg)) != -1); + } + /*@ +type @*/ +} + +static bool evermore_protocol(struct gps_device_t *session, int protocol) +{ + /*@ +charint */ + char tmp8; + char evrm_protocol_config[] = { + 0x84, /* 0: msg ID, Protocol Configuration */ + 0x00, /* 1: mode; EverMore binary(0), NMEA(1) */ + 0x00, /* 2: reserved */ + 0x00, /* 3: reserved */ + }; + /*@ -charint */ + gpsd_report(LOG_PROG, "evermore_protocol(%d)\n", protocol); + /*@i1@*/tmp8 = (protocol != 0) ? 1 : 0; /* NMEA : binary */ + evrm_protocol_config[1] = tmp8; + return (evermore_control_send(session, evrm_protocol_config, sizeof(evrm_protocol_config)) != -1); +} + +static bool evermore_nmea_config(struct gps_device_t *session, int mode) +/* mode = 0 : EverMore default */ +/* mode = 1 : gpsd best */ +/* mode = 2 : EverMore search, activate PEMT101 message */ +{ + char tmp8; + /*@ +charint */ + char evrm_nmeaout_config[] = { + 0x8e, /* 0: msg ID, NMEA Message Control */ + 0xff, /* 1: NMEA sentence bitmask, GGA(0), GLL(1), GSA(2), GSV(3), ... */ + 0x01, /* 2: nmea checksum no(0), yes(1) */ + 1, /* 3: GPGGA, interval 0-255s */ + 0, /* 4: GPGLL, interval 0-255s */ + 1, /* 5: GPGSA, interval 0-255s */ + 1, /* 6: GPGSV, interval 0-255s */ + 1, /* 7: GPRMC, interval 0-255s */ + 0, /* 8: GPVTG, interval 0-255s */ + 0, /* 9: PEMT,101, interval 0-255s */ + 0, 0, 0, 0, 0, 0, /* 10-15: reserved */ + }; + /*@ -charint */ + gpsd_report(LOG_PROG, "evermore_nmea_config(%d)\n", mode); + /*@i1@*/tmp8 = (mode == 1) ? 5 : 1; /* NMEA GPGSV, gpsd */ + evrm_nmeaout_config[6] = tmp8; /* GPGSV, 1s or 5s */ + /*@i1@*/tmp8 = (mode == 2) ? 1 : 0; /* NMEA PEMT101 */ + evrm_nmeaout_config[9] = tmp8; /* PEMT101, 1s or 0s */ + return (evermore_control_send(session, evrm_nmeaout_config, sizeof(evrm_nmeaout_config)) != -1); +} + +static void evermore_mode(struct gps_device_t *session, int mode) +{ + gpsd_report(LOG_PROG, "evermore_mode(%d), %d\n", mode, session->back_to_nmea ? 1 : 0); + if (mode == MODE_NMEA) { + /* NMEA */ + (void) evermore_protocol(session, 1); + session->gpsdata.driver_mode = MODE_NMEA; + (void) evermore_nmea_config(session, 1); /* configure NMEA messages for gpsd */ + } else { + /* binary */ + (void) evermore_protocol(session, 0); + session->back_to_nmea = false; + session->gpsdata.driver_mode = MODE_BINARY; + } +} + +#ifdef ALLOW_RECONFIGURE +static void evermore_configurator(struct gps_device_t *session, unsigned int seq) +{ + gpsd_report(LOG_PROG, "evermore_configurator(%d)\n", seq); + (void) evermore_nmea_config(session, 1); /* configure NMEA messages for gpsd (GPGSV every 5s) */ + if (seq == 0) { + if (session->packet.type == NMEA_PACKET) { + gpsd_report(LOG_WARN, "NMEA_PACKET packet\n"); + } + (void) evermore_mode(session, 1); /* switch GPS to binary mode */ + session->back_to_nmea = true; + } +} +#endif /* ALLOW_RECONFIGURE */ + + +static void evermore_wrap(struct gps_device_t *session) +{ + gpsd_report(LOG_PROG, "evermore_wrap\n"); + (void) evermore_nmea_config(session, 0); /* configure NMEA messages to default */ +} + + +/* this is everything we export */ +const struct gps_type_t evermore_binary = +{ + .type_name = "EverMore binary", /* full name of type */ + .packet_type = NMEA_PACKET, /* lexer packet type */ + .trigger = "$PEMT,", /* recognize the type */ + .channels = EVERMORE_CHANNELS, /* consumer-grade GPS */ + .control_send = evermore_control_send, /* how to send a control string */ + .probe_wakeup = NULL, /* no wakeup to be done before hunt */ + .probe_detect = NULL, /* no probe */ + .probe_subtype = NULL, /* no subtype probing */ +#ifdef ALLOW_RECONFIGURE + .configurator = evermore_configurator, /* switch to binary */ +#endif /* ALLOW_RECONFIGURE */ + .get_packet = generic_get, /* use generic one */ + .parse_packet = evermore_parse_input, /* parse message packets */ + .rtcm_writer = pass_rtcm, /* send RTCM data straight */ + .speed_switcher = evermore_speed, /* we can change baud rates */ + .mode_switcher = evermore_mode, /* there is a mode switcher */ + .rate_switcher = NULL, /* no sample-rate switcher */ + .cycle_chars = -1, /* ignore, no rate switch */ +#ifdef ALLOW_RECONFIGURE + .revert = NULL, /* reversion code */ +#endif /* ALLOW_RECONFIGURE */ + .wrapup = evermore_wrap, /* wrapup method */ + .cycle = 1, /* updates every second */ +}; +#endif /* defined(EVERMORE_ENABLE) && defined(BINARY_ENABLE) */ diff --git a/driver_garmin.c b/driver_garmin.c new file mode 100644 index 00000000..7d763dce --- /dev/null +++ b/driver_garmin.c @@ -0,0 +1,1247 @@ +/* $Id$ */ +/* + * Handle the Garmin binary packet format supported by the USB Garmins + * tested with the Garmin 18 and other models. This driver is NOT for + * serial port connected Garmins, they provide adequate NMEA support. + * + * This code is partly from the Garmin IOSDK and partly from the + * sample code in the Linux garmin_gps driver. + * + * This code supports both Garmin on a serial port and USB Garmins. + * + * USB Garmins need the Linux garmin_gps driver and will not function + * without it. This code has been tested and at least at one time is + * known to work on big- and little-endian CPUs and 32 and 64 bit cpu + * modes. + * + * Protocol info from: + * 425_TechnicalSpecification.pdf + * ( formerly GPS18_TechnicalSpecification.pdf ) + * iop_spec.pdf + * http://www.garmin.com/support/commProtocol.html + * + * bad code by: Gary E. Miller + * 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: + * + * do not have from garmin: + * pdop + * hdop + * vdop + * magnetic variation + * + * known bugs: + * hangs in the fread loop instead of keeping state and returning. + * may or may not work on a little-endian machine + */ + +#define __USE_POSIX199309 1 +#include +#include // for nanosleep() + +#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" + +#ifdef GARMIN_ENABLE + +#define USE_RMD 0 + +#define ETX 0x03 +#define ACK 0x06 +#define DLE 0x10 +#define NAK 0x15 + +#define GARMIN_LAYERID_TRANSPORT (uint8_t) 0 +#define GARMIN_LAYERID_APPL (uint32_t) 20 +// Linux Garmin USB driver layer-id to use for some control mechanisms +#define GARMIN_LAYERID_PRIVATE 0x01106E4B + +// packet ids used in private layer +#define PRIV_PKTID_SET_DEBUG 1 +#define PRIV_PKTID_SET_MODE 2 +#define PRIV_PKTID_INFO_REQ 3 +#define PRIV_PKTID_INFO_RESP 4 +#define PRIV_PKTID_RESET_REQ 5 +#define PRIV_PKTID_SET_DEF_MODE 6 + +#define MODE_NATIVE 0 +#define MODE_GARMIN_SERIAL 1 + +#define GARMIN_PKTID_TRANSPORT_START_SESSION_REQ 5 +#define GARMIN_PKTID_TRANSPORT_START_SESSION_RESP 6 + +#define GARMIN_PKTID_PROTOCOL_ARRAY 253 +#define GARMIN_PKTID_PRODUCT_RQST 254 +#define GARMIN_PKTID_PRODUCT_DATA 255 +/* 0x29 ')' */ +#define GARMIN_PKTID_RMD41_DATA 41 +/* 0x33 '3' */ +#define GARMIN_PKTID_PVT_DATA 51 +/* 0x33 '4' */ +#define GARMIN_PKTID_RMD_DATA 52 +/* 0x72 'r' */ +#define GARMIN_PKTID_SAT_DATA 114 + +#define GARMIN_PKTID_L001_XFER_CMPLT 12 +#define GARMIN_PKTID_L001_COMMAND_DATA 10 +#define GARMIN_PKTID_L001_DATE_TIME_DATA 14 +#define GARMIN_PKTID_L001_RECORDS 27 +#define GARMIN_PKTID_L001_WPT_DATA 35 + +#define CMND_ABORT 0 +#define CMND_START_PVT_DATA 49 +#define CMND_STOP_PVT_DATA 50 +#define CMND_START_RM_DATA 110 + +#define MAX_BUFFER_SIZE 4096 + +#define GARMIN_CHANNELS 12 + +// something magic about 64, garmin driver will not return more than +// 64 at a time. If you read less than 64 bytes the next read will +// just get the last of the 64 byte buffer. +#define ASYNC_DATA_SIZE 64 + + +#pragma pack(1) +// This is the data format of the satellite data from the garmin USB +typedef struct { + uint8_t svid; + int16_t snr; // 0 - 0xffff + uint8_t elev; + uint16_t azmth; + uint8_t status; // bit 0, has ephemeris, 1, has diff correction + // bit 2 used in solution + // bit 3?? +} cpo_sat_data; + +/* Garmin D800_Pvt_Datetype_Type */ +/* packet type: GARMIN_PKTID_PVT_DATA 52 */ +/* This is the data format of the position data from the garmin USB */ +typedef struct { + float alt; /* altitude above WGS 84 (meters) */ + float epe; /* estimated position error, 2 sigma (meters) */ + float eph; /* epe, but horizontal only (meters) */ + float epv; /* epe but vertical only (meters ) */ + int16_t fix; /* 0 - failed integrity check + * 1 - invalid or unavailable fix + * 2 - 2D + * 3 - 3D + * 4 - 2D Diff + * 5 - 3D Diff + */ + double gps_tow; /* gps time os week (seconds) */ + double lat; /* ->latitude (radians) */ + double lon; /* ->longitude (radians) */ + float lon_vel; /* velocity east (meters/second) */ + float lat_vel; /* velocity north (meters/second) */ + float alt_vel; /* velocity up (meters/sec) */ + // Garmin GPS25 uses pkt_id 0x28 and does not output the + // next 3 items + float msl_hght; /* height of WGS 84 above MSL (meters) */ + int16_t leap_sec; /* diff between GPS and UTC (seconds) */ + int32_t grmn_days; +} cpo_pvt_data; + +typedef struct { + uint32_t cycles; + double pr; + uint16_t phase; + int8_t slp_dtct; + uint8_t snr_dbhz; + uint8_t svid; + int8_t valid; +} cpo_rcv_sv_data; + +/* packet type: GARMIN_PKTID_RMD_DATA 53 */ +/* seems identical to the packet id 0x29 from the Garmin GPS 25 */ +typedef struct { + double rcvr_tow; + int16_t rcvr_wn; + cpo_rcv_sv_data sv[GARMIN_CHANNELS]; +} cpo_rcv_data; + +// This is the packet format to/from the Garmin USB +typedef struct { + uint8_t mPacketType; + uint8_t mReserved1; + uint16_t mReserved2; + uint16_t mPacketId; + uint16_t mReserved3; + uint32_t mDataSize; + union { + int8_t chars[MAX_BUFFER_SIZE]; + uint8_t uchars[MAX_BUFFER_SIZE]; + cpo_pvt_data pvt; + cpo_sat_data sats; + } mData; +} Packet_t; + +// useful funcs to read/write ints +// floats and doubles are Intel order only... +static inline void set_int16(uint8_t *buf, uint32_t value) +{ + buf[0] = (uint8_t)(0x0FF & value); + buf[1] = (uint8_t)(0x0FF & (value >> 8)); +} + +static inline void set_int32(uint8_t *buf, uint32_t value) +{ + buf[0] = (uint8_t)(0x0FF & value); + buf[1] = (uint8_t)(0x0FF & (value >> 8)); + buf[2] = (uint8_t)(0x0FF & (value >> 16)); + buf[3] = (uint8_t)(0x0FF & (value >> 24)); +} + +static inline uint16_t get_uint16(const uint8_t *buf) +{ + return (uint16_t)(0xFF & buf[0]) + | ((uint16_t)(0xFF & buf[1]) << 8); +} + +static inline uint32_t get_int32(const uint8_t *buf) +{ + return (uint32_t)(0xFF & buf[0]) + | ((uint32_t)(0xFF & buf[1]) << 8) + | ((uint32_t)(0xFF & buf[2]) << 16) + | ((uint32_t)(0xFF & buf[3]) << 24); +} + +// convert radians to degrees +static inline double radtodeg( double rad) { + return (double)(rad * RAD_2_DEG ); +} + +static gps_mask_t PrintSERPacket(struct gps_device_t *session, unsigned char pkt_id, int pkt_len, unsigned char *buf ); +static gps_mask_t PrintUSBPacket(struct gps_device_t *session, Packet_t *pkt ); + +gps_mask_t PrintSERPacket(struct gps_device_t *session, unsigned char pkt_id + , int pkt_len, unsigned char *buf ) +{ + + gps_mask_t mask = 0; + int i = 0, j = 0; + uint16_t prod_id = 0; + uint16_t ver = 0; + int maj_ver; + int min_ver; + time_t time_l = 0; + double track; + char msg_buf[512] = ""; + char *msg = NULL; + cpo_sat_data *sats = NULL; + cpo_pvt_data *pvt = NULL; + cpo_rcv_data *rmd = NULL; + + gpsd_report(LOG_IO, "PrintSERPacket(, %#02x, %#02x, )\n", pkt_id, pkt_len); + + switch( pkt_id ) { + case ACK: + gpsd_report(LOG_PROG, "ACK\n"); + break; + case NAK: + gpsd_report(LOG_PROG, "NAK\n"); + break; + case GARMIN_PKTID_L001_COMMAND_DATA: + prod_id = get_uint16((uint8_t *)buf); + /*@ -branchstate @*/ + switch ( prod_id ) { + case CMND_ABORT: + msg = "Abort current xfer"; + break; + case CMND_START_PVT_DATA: + msg = "Start Xmit PVT data"; + break; + case CMND_STOP_PVT_DATA: + msg = "Stop Xmit PVT data"; + break; + case CMND_START_RM_DATA: + msg = "Start RMD data"; + break; + default: + (void)snprintf(msg_buf, sizeof(msg_buf), "Unknown: %u", + (unsigned int)prod_id); + msg = msg_buf; + break; + } + /*@ +branchstate @*/ + gpsd_report(LOG_PROG, "Appl, Command Data: %s\n", msg); + break; + case GARMIN_PKTID_PRODUCT_RQST: + gpsd_report(LOG_PROG, "Appl, Product Data req\n"); + break; + case GARMIN_PKTID_PRODUCT_DATA: + prod_id = get_uint16((uint8_t *)buf); + ver = get_uint16((uint8_t *)&buf[2]); + maj_ver = (int)(ver / 100); + min_ver = (int)(ver - (maj_ver * 100)); + gpsd_report(LOG_PROG, "Appl, Product Data, sz: %d\n", pkt_len); + (void)snprintf(session->subtype, sizeof(session->subtype), + "%d: %d.%02d" + , (int)prod_id, maj_ver, min_ver); + gpsd_report(LOG_INF, "Garmin Product ID: %d, SoftVer: %d.%02d\n" + , prod_id, maj_ver, min_ver); + gpsd_report(LOG_INF, "Garmin Product Desc: %s\n" + , &buf[4]); + mask |= DEVICEID_SET; + break; + case GARMIN_PKTID_PVT_DATA: + gpsd_report(LOG_PROG, "Appl, PVT Data Sz: %d\n", pkt_len); + + pvt = (cpo_pvt_data*) buf; + + // 631065600, unix seconds for 31 Dec 1989 Zulu + time_l = (time_t)(631065600 + (pvt->grmn_days * 86400)); + time_l -= pvt->leap_sec; + session->context->leap_seconds = pvt->leap_sec; + session->context->valid = LEAP_SECOND_VALID; + // gps_tow is always like x.999 or x.998 so just round it + time_l += (time_t) round(pvt->gps_tow); + session->gpsdata.fix.time + = session->gpsdata.sentence_time + = (double)time_l; + gpsd_report(LOG_PROG, "time_l: %ld\n", (long int)time_l); + + session->gpsdata.fix.latitude = radtodeg(pvt->lat); + /* sanity check the lat */ + if ( 90.0 < session->gpsdata.fix.latitude ) { + session->gpsdata.fix.latitude = 90.0; + gpsd_report(LOG_INF, "ERROR: Latitude overrange\n"); + } else if ( -90.0 > session->gpsdata.fix.latitude ) { + session->gpsdata.fix.latitude = -90.0; + gpsd_report(LOG_INF, "ERROR: Latitude negative overrange\n"); + } + session->gpsdata.fix.longitude = radtodeg(pvt->lon); + /* sanity check the lon */ + if ( 180.0 < session->gpsdata.fix.longitude ) { + session->gpsdata.fix.longitude = 180.0; + gpsd_report(LOG_INF, "ERROR: Longitude overrange\n"); + } else if ( -180.0 > session->gpsdata.fix.longitude ) { + session->gpsdata.fix.longitude = -180.0; + gpsd_report(LOG_INF, "ERROR: Longitude negative overrange\n"); + } + + // altitude over WGS84 converted to MSL + session->gpsdata.fix.altitude = pvt->alt + pvt->msl_hght; + + // geoid separation from WGS 84 + // gpsd sign is opposite of garmin sign + session->gpsdata.separation = -pvt->msl_hght; + + // Estimated position error in meters. + // We follow the advice at . + // If this assumption changes here, it should also change in + // nmea_parse.c where we analyze PGRME. + session->gpsdata.epe = pvt->epe * (GPSD_CONFIDENCE/CEP50_SIGMA); + session->gpsdata.fix.eph = pvt->eph * (GPSD_CONFIDENCE/CEP50_SIGMA); + session->gpsdata.fix.epv = pvt->epv * (GPSD_CONFIDENCE/CEP50_SIGMA); + + // convert lat/lon to directionless speed + session->gpsdata.fix.speed = hypot(pvt->lon_vel, pvt->lat_vel); + + // keep climb in meters/sec + session->gpsdata.fix.climb = pvt->alt_vel; + + track = atan2(pvt->lon_vel, pvt->lat_vel); + if (track < 0) { + track += 2 * GPS_PI; + } + session->gpsdata.fix.track = radtodeg(track); + + switch ( pvt->fix) { + case 0: + case 1: + default: + // no fix + session->gpsdata.status = STATUS_NO_FIX; + session->gpsdata.fix.mode = MODE_NO_FIX; + break; + case 2: + // 2D fix + session->gpsdata.status = STATUS_FIX; + session->gpsdata.fix.mode = MODE_2D; + break; + case 3: + // 3D fix + session->gpsdata.status = STATUS_FIX; + session->gpsdata.fix.mode = MODE_3D; + break; + case 4: + // 2D Differential fix + session->gpsdata.status = STATUS_DGPS_FIX; + session->gpsdata.fix.mode = MODE_2D; + break; + case 5: + // 3D differential fix + session->gpsdata.status = STATUS_DGPS_FIX; + session->gpsdata.fix.mode = MODE_3D; + break; + } +#ifdef NTPSHM_ENABLE + if (session->context->enable_ntpshm && session->gpsdata.fix.mode > MODE_NO_FIX) + (void) ntpshm_put(session, session->gpsdata.fix.time); +#endif /* NTPSHM_ENABLE */ + + gpsd_report(LOG_PROG, "Appl, mode %d, status %d\n" + , session->gpsdata.fix.mode + , session->gpsdata.status); + + gpsd_report(LOG_INF, "UTC Time: %lf\n", session->gpsdata.fix.time); + gpsd_report(LOG_INF + , "Geoid Separation (MSL-WGS84): from garmin %lf, calculated %lf\n" + , -pvt->msl_hght + , wgs84_separation(session->gpsdata.fix.latitude + , session->gpsdata.fix.longitude)); + + gpsd_report(LOG_INF, "Alt: %.3f, Epe: %.3f, Eph: %.3f, Epv: %.3f, Fix: %d, Gps_tow: %f, Lat: %.3f, Lon: %.3f, LonVel: %.3f, LatVel: %.3f, AltVel: %.3f, MslHgt: %.3f, Leap: %d, GarminDays: %d\n" + , pvt->alt + , pvt->epe + , pvt->eph + , pvt->epv + , pvt->fix + , pvt->gps_tow + , session->gpsdata.fix.latitude + , session->gpsdata.fix.longitude + , pvt->lon_vel + , pvt->lat_vel + , pvt->alt_vel + , pvt->msl_hght + , pvt->leap_sec + , pvt->grmn_days); + + mask |= TIME_SET | LATLON_SET | ALTITUDE_SET | STATUS_SET | MODE_SET | SPEED_SET | TRACK_SET | CLIMB_SET | HERR_SET | VERR_SET | PERR_SET | CYCLE_START_SET; + break; + case GARMIN_PKTID_RMD_DATA: + case GARMIN_PKTID_RMD41_DATA: + rmd = (cpo_rcv_data *) buf; + gpsd_report(LOG_IO, "PVT RMD Data Sz: %d\n", pkt_len); + gpsd_report(LOG_PROG, "PVT RMD rcvr_tow: %f, rcvr_wn: %d\n", + rmd->rcvr_tow, rmd->rcvr_wn); + for ( i = 0 ; i < GARMIN_CHANNELS ; i++ ) { + gpsd_report(LOG_INF, + "PVT RMD Sat: %3u, cycles: %9u, pr: %16.6f, " + "phase: %7.3f, slp_dtct: %3s, snr: %3u, Valid: %3s\n", + rmd->sv[i].svid + 1, rmd->sv[i].cycles, rmd->sv[i].pr, + (rmd->sv[i].phase * 360.0)/2048.0, + rmd->sv[i].slp_dtct!='\0' ? "Yes" : "No", + rmd->sv[i].snr_dbhz, + rmd->sv[i].valid!='\0' ? "Yes" : "No"); + } + break; + + case GARMIN_PKTID_SAT_DATA: + gpsd_report(LOG_PROG, "SAT Data Sz: %d\n", pkt_len); + sats = (cpo_sat_data *)buf; + + session->gpsdata.satellites_used = 0; + memset(session->gpsdata.used,0,sizeof(session->gpsdata.used)); + gpsd_zero_satellites(&session->gpsdata); + for ( i = 0, j = 0 ; i < GARMIN_CHANNELS ; i++, sats++ ) { + gpsd_report(LOG_INF," Sat %3d, snr: %5d, elev: %2d, Azmth: %3d, Stat: %x\n" + , sats->svid + , sats->snr + , sats->elev + , sats->azmth + , sats->status); + + if ( 255 == (int)sats->svid ) { + // Garmin uses 255 for empty + // gpsd uses 0 for empty + continue; + } + + session->gpsdata.PRN[j] = (int)sats->svid; + session->gpsdata.azimuth[j] = (int)sats->azmth; + session->gpsdata.elevation[j] = (int)sats->elev; + // Garmin does not document this. snr is in dB*100 + // Known, but not seen satellites have a dB value of -1*100 + session->gpsdata.ss[j] = (int)round((float)sats->snr / 100); + if (session->gpsdata.ss[j] < 0) { + session->gpsdata.ss[j] = 0; + } + // FIXME: Garmin documents this, but Daniel Dorau + // says the behavior on his GPSMap60CSX + // doesn't match it. + if ( (uint8_t)0 != (sats->status & 4 ) ) { + // used in solution? + session->gpsdata.used[session->gpsdata.satellites_used++] + = (int)sats->svid; + } + session->gpsdata.satellites++; + j++; + + } + mask |= SATELLITE_SET | USED_SET; + break; + case GARMIN_PKTID_PROTOCOL_ARRAY: + // this packet is never requested, it just comes, in some case + // after a GARMIN_PKTID_PRODUCT_RQST + gpsd_report(LOG_INF, "Appl, Product Capability, sz: %d\n", pkt_len); + for ( i = 0; i < pkt_len ; i += 3 ) { + gpsd_report(LOG_INF, " %c%03d\n", buf[i], get_uint16((uint8_t *)&buf[i+1] ) ); + } + break; + default: + gpsd_report(LOG_WARN, "Unknown packet id: %#02x, Sz: %#02x, pkt:%s\n", + pkt_id, pkt_len, + gpsd_hexdump_wrapper(buf, (size_t)pkt_len, LOG_WARN)); + break; + } + gpsd_report(LOG_IO, "PrintSERPacket(, %#02x, %#02x, ) = %#02x\n", + pkt_id, pkt_len, mask); + return mask; +} + + +/*@ -branchstate @*/ +// For debugging, decodes and prints some known packets. +static gps_mask_t PrintUSBPacket(struct gps_device_t *session, Packet_t *pkt) +{ + gps_mask_t mask = 0; + int maj_ver; + int min_ver; + uint32_t mode = 0; + uint16_t prod_id = 0; + uint32_t veri = 0; + uint32_t serial; + uint32_t mDataSize = get_int32( (uint8_t*)&pkt->mDataSize); + +// + uint8_t *buffer = (uint8_t*)pkt; + + gpsd_report(LOG_PROG, "PrintUSBPacket()\n"); +// gem + if ( DLE == pkt->mPacketType) { + gpsd_report(LOG_PROG, "really a SER packet!\n"); + return PrintSERPacket ( session, + (unsigned char)buffer[1], + (int)buffer[2], + (unsigned char*)(buffer + 3)); + } + +// gem + if ( 4096 < mDataSize) { + gpsd_report(LOG_WARN, "bogus packet, size too large=%d\n", mDataSize); + return 0; + } + + (void)snprintf(session->gpsdata.tag, sizeof(session->gpsdata.tag), "%u" + , (unsigned int)pkt->mPacketType); + switch ( pkt->mPacketType ) { + case GARMIN_LAYERID_TRANSPORT: + /* Garmin USB layer specific */ + switch( pkt->mPacketId ) { + case GARMIN_PKTID_TRANSPORT_START_SESSION_REQ: + gpsd_report(LOG_PROG, "Transport, Start Session req\n"); + break; + case GARMIN_PKTID_TRANSPORT_START_SESSION_RESP: + mode = get_int32(&pkt->mData.uchars[0]); + gpsd_report(LOG_PROG, "Transport, Start Session resp, unit: 0x%x\n" + , mode); + break; + default: + gpsd_report(LOG_PROG, "Transport, Packet: Type %d %d %d, ID: %d, Sz: %d\n" + , pkt->mPacketType + , pkt->mReserved1 + , pkt->mReserved2 + , pkt->mPacketId + , mDataSize); + break; + } + break; + case GARMIN_LAYERID_APPL: + /* raw data transport, shared with Garmin Serial Driver */ + + mask = PrintSERPacket(session, + (unsigned char)pkt->mPacketId, + (int)mDataSize, + (unsigned char *)pkt->mData.uchars); + break; + case 75: + // private, garmin USB kernel driver specific + switch( pkt->mPacketId ) { + case PRIV_PKTID_SET_MODE: + prod_id = get_uint16(&pkt->mData.uchars[0]); + gpsd_report(LOG_PROG, "Private, Set Mode: %d\n", prod_id); + break; + case PRIV_PKTID_INFO_REQ: + gpsd_report(LOG_PROG, "Private, ID: Info Req\n"); + break; + case PRIV_PKTID_INFO_RESP: + veri = get_int32(pkt->mData.uchars); + maj_ver = (int)(veri >> 16); + min_ver = (int)(veri & 0xffff); + mode = get_int32(&pkt->mData.uchars[4]); + serial = get_int32(&pkt->mData.uchars[8]); + gpsd_report(LOG_PROG, "Private, ID: Info Resp\n"); + gpsd_report(LOG_INF, "Garmin USB Driver found, Version %d.%d, Mode: %d, GPS Serial# %u\n" + , maj_ver, min_ver, mode, serial); + break; + default: + gpsd_report(LOG_PROG, "Private, Packet: ID: %d, Sz: %d\n" + , pkt->mPacketId + , mDataSize); + break; + } + break; + default: + gpsd_report(LOG_PROG, "Packet: Type %d %d %d, ID: %d, Sz: %d\n" + , pkt->mPacketType + , pkt->mReserved1 + , pkt->mReserved2 + , pkt->mPacketId + , mDataSize); + break; + } + + return mask; +} +/*@ +branchstate @*/ + + +/* build and send a packet w/ USB protocol */ +static void Build_Send_USB_Packet( struct gps_device_t *session, + uint32_t layer_id, uint32_t pkt_id, uint32_t length, uint32_t data ) +{ + uint8_t *buffer = (uint8_t *)session->driver.garmin.Buffer; + Packet_t *thePacket = (Packet_t*)buffer; + ssize_t theBytesReturned = 0; + ssize_t theBytesToWrite = 12 + (ssize_t)length; + + set_int32(buffer, layer_id); + set_int32(buffer+4, pkt_id); + set_int32(buffer+8, length); + if ( 2 == length ) { + set_int16(buffer+12, data); + } else if ( 4 == length ) { + set_int32(buffer+12, data); + } + +#if 0 + gpsd_report(LOG_IO, "SendPacket(), writing %d bytes: %s\n", + theBytesToWrite, + gpsd_hexdump_wrapper(thePacket, theBytesToWrite, LOG_IO)); +#endif + (void)PrintUSBPacket ( session, thePacket); + + theBytesReturned = gpsd_write( session , thePacket, + (size_t)theBytesToWrite); + gpsd_report(LOG_IO, "SendPacket(), wrote %zd bytes\n", + theBytesReturned); + + // Garmin says: + // If the packet size was an exact multiple of the USB packet + // size, we must make a final write call with no data + + // as a practical matter no known packets are 64 bytes long so + // this is untested + + // So here goes just in case + if( 0 == (theBytesToWrite % ASYNC_DATA_SIZE) ) { + char *n = ""; + theBytesReturned = gpsd_write( session , &n, 0); + } +} +/* build and send a packet in serial protocol */ +/* layer_id unused */ +// FIXME: This should go through the common message buffer someday +static void Build_Send_SER_Packet( struct gps_device_t *session, + uint32_t layer_id UNUSED, uint32_t pkt_id, uint32_t length, + uint32_t data ) +{ + uint8_t *buffer = (uint8_t *)session->driver.garmin.Buffer; + uint8_t *buffer0 = buffer; + Packet_t *thePacket = (Packet_t*)buffer; + ssize_t theBytesReturned = 0; + ssize_t theBytesToWrite = 6 + (ssize_t)length; + uint8_t chksum = 0; + + *buffer++ = (uint8_t)DLE; + *buffer++ = (uint8_t)pkt_id; + chksum = pkt_id; + *buffer++ = (uint8_t)length; + chksum += length; + if ( 2 == length ) { + /* carefull! no DLE stuffing here! */ + set_int16(buffer, data); + chksum += buffer[0]; + chksum += buffer[1]; + } else if ( 4 == length ) { + /* carefull! no DLE stuffing here! */ + set_int32(buffer, data); + chksum += buffer[0]; + chksum += buffer[1]; + chksum += buffer[2]; + chksum += buffer[3]; + } + buffer += length; + + // Add checksum + *buffer++ = -chksum; + if ( DLE == -chksum ) { + /* stuff another DLE */ + *buffer++ = (uint8_t)DLE; + theBytesToWrite++; + } + + // Add DLE, ETX + *buffer++ = (uint8_t)DLE; + *buffer++ = (uint8_t)ETX; + +#if 1 + gpsd_report(LOG_IO, "SendPacket(), writing %zd bytes: %s\n", + theBytesToWrite, + gpsd_hexdump_wrapper(thePacket, (size_t)theBytesToWrite, LOG_IO)); +#endif + (void)PrintSERPacket ( session, + (unsigned char)buffer0[1], + (int)buffer0[2], + (unsigned char *)(buffer0 + 3)); + + theBytesReturned = gpsd_write(session, thePacket, + (size_t)theBytesToWrite); + gpsd_report(LOG_IO, "SendPacket(), wrote %zd bytes\n", + theBytesReturned); + +} + + +/* + * garmin_detect() + * + * check that the garmin_gps driver is installed in the kernel + * and that an active USB device is using it. + * + * It does not yet check that the currect device is the one + * attached to the garmin. So if you have a garmin and another + * gps this could be a problem. + * + * this is very linux specific. + * + * return 1 if garmin_gps device found + * return 0 if not + * + */ +static bool garmin_detect(struct gps_device_t *session) +{ + + FILE *fp = NULL; + char buf[256]; + bool ok = false; + + /* check for garmin USB serial driver -- very Linux-specific */ + if (access("/sys/module/garmin_gps", R_OK) != 0) { + gpsd_report(LOG_WARN, "garmin_gps not active.\n"); + return false; + } + // check for a garmin_gps device in /proc + if ( !(fp = fopen( "/proc/bus/usb/devices", "r") ) ) { + gpsd_report(LOG_ERROR, "Can't open /proc/bus/usb/devices\n"); + return false; + } + + ok = false; + while ( 0 != fgets( buf, (int)sizeof(buf), fp ) ) { + if ( strstr( buf, "garmin_gps") ) { + ok = true; + break; + } + } + (void)fclose(fp); + if ( !ok ) { + // no device using garmin now + gpsd_report(LOG_WARN, "garmin_gps not in /proc/bus/usb/devices.\n"); + return false; + } + + if (!gpsd_set_raw(session)) { + gpsd_report(LOG_ERROR, "garmin_detect: error changing port attributes: %s\n", + strerror(errno)); + return false; + } + +#ifdef __UNUSED + Packet_t *thePacket = NULL; + uint8_t *buffer = NULL; + /* reset the buffer and buffer length */ + memset( session->driver.garmin.Buffer, 0, sizeof(session->driver.garmin.Buffer) ); + session->driver.garmin.BufferLen = 0; + + if (sizeof(session->driver.garmin.Buffer) < sizeof(Packet_t)) { + gpsd_report(LOG_ERROR, "garmin_detect: Compile error, garmin.Buffer too small.\n", + strerror(errno)); + return false; + } + + buffer = (uint8_t *)session->driver.garmin.Buffer; + thePacket = (Packet_t*)buffer; +#endif /* __UNUSED__ */ + + // set Mode 1, mode 0 is broken somewhere past 2.6.14 + // but how? + gpsd_report(LOG_PROG, "Set garmin_gps driver mode = 0\n"); + Build_Send_USB_Packet( session, GARMIN_LAYERID_PRIVATE + , PRIV_PKTID_SET_MODE, 4, MODE_GARMIN_SERIAL); + // expect no return packet !? + + return true; +} + +static void garmin_probe_subtype(struct gps_device_t *session, unsigned int seq) +{ + if (seq == 0) { + // Tell the device to send product data + gpsd_report(LOG_PROG, "Get Garmin Product Data\n"); + Build_Send_SER_Packet(session, GARMIN_LAYERID_APPL + , GARMIN_PKTID_PRODUCT_RQST, 0, 0); + + // turn on PVT data 49 + gpsd_report(LOG_PROG, "Set Garmin to send reports every 1 second\n"); + + Build_Send_SER_Packet(session, GARMIN_LAYERID_APPL + , GARMIN_PKTID_L001_COMMAND_DATA, 2, CMND_START_PVT_DATA); + +#if USE_RMD + // turn on RMD data 110 + gpsd_report(LOG_PROG, "Set Garmin to send Raw sat data\n"); + Build_Send_SER_Packet(session, GARMIN_LAYERID_APPL + , GARMIN_PKTID_L001_COMMAND_DATA, 2, CMND_START_RM_DATA); +#endif + } +} + +static void garmin_close(struct gps_device_t *session UNUSED) +{ + /* FIXME -- do we need to put the garmin to sleep? or is closing the port + sufficient? */ + gpsd_report(LOG_PROG, "garmin_close()\n"); + return; +} + +#define Send_ACK() Build_Send_SER_Packet(session, 0, ACK, 0, 0) +#define Send_NAK() Build_Send_SER_Packet(session, 0, NAK, 0, 0) + +/*@ +charint @*/ +gps_mask_t garmin_ser_parse(struct gps_device_t *session) +{ + unsigned char *buf = session->packet.outbuffer; + size_t len = session->packet.outbuflen; + unsigned char data_buf[MAX_BUFFER_SIZE]; + unsigned char c; + int i = 0; + size_t n = 0; + int data_index = 0; + int got_dle = 0; + unsigned char pkt_id = 0; + unsigned char pkt_len = 0; + unsigned char chksum = 0; + gps_mask_t mask = 0; + + gpsd_report(LOG_RAW, "garmin_ser_parse()\n"); + if ( 6 > len ) { + /* WTF? */ + /* minimum packet; [pkt id] [length=0] [chksum] */ + Send_NAK(); + gpsd_report(LOG_RAW+1, "Garmin serial too short: %zd\n", len); + return 0; + } + /* debug */ + for ( i = 0 ; i < (int)len ; i++ ) { + gpsd_report(LOG_RAW+1, "Char: %#02x\n", buf[i]); + } + + if ( '\x10' != buf[0] ) { + Send_NAK(); + gpsd_report(LOG_RAW+1, "buf[0] not DLE\n"); + return 0; + } + n = 1; + pkt_id = buf[n++]; + chksum = pkt_id; + if ( '\x10' == pkt_id ) { + if ( '\x10' != buf[n++] ) { + Send_NAK(); + gpsd_report(LOG_RAW+1, "Bad pkt_id %#02x\n", pkt_id); + return 0; + } + } + + pkt_len = buf[n++]; + chksum += pkt_len; + if ( '\x10' == pkt_len ) { + if ( '\x10' != buf[n++] ) { + gpsd_report(LOG_RAW+1, "Bad pkt_len %#02x\n", pkt_len); + Send_NAK(); + return 0; + } + } + data_index = 0; + for ( i = 0; i < 256 ; i++ ) { + + if ( (int)pkt_len == data_index ) { + // got it all + break; + } + if ( len < n + i ) { + gpsd_report(LOG_RAW+1, "Packet too short %zd < %zd\n", len, n + i); + Send_NAK(); + return 0; + } + c = buf[n + i]; + if ( got_dle ) { + got_dle = 0; + if ( '\x10' != c ) { + Send_NAK(); + gpsd_report(LOG_RAW+1, "Bad DLE %#02x\n", c); + return 0; + } + } else { + chksum += c; + data_buf[ data_index++ ] = c; + if ( '\x10' == c ) { + got_dle = 1; + } + } + } + /* get checksum */ + if ( len < n + i ) { + Send_NAK(); + gpsd_report(LOG_RAW+1, "No checksum, Packet too short %zd < %zd\n", + len, n + i); + return 0; + } + c = buf[n + i++]; + chksum += c; + /* get final DLE */ + if ( len < n + i ) { + Send_NAK(); + gpsd_report(LOG_RAW+1, "No final DLE, Packet too short %zd < %zd\n", + len, n + i); + return 0; + } + c = buf[n + i++]; + if ( '\x10' != c ) { + Send_NAK(); + gpsd_report(LOG_RAW+1, "Final DLE not DLE\n"); + return 0; + } + /* get final ETX */ + if ( len < n + i ) { + Send_NAK(); + gpsd_report(LOG_RAW+1, "No final ETX, Packet too short %zd < %zd\n", + len, n + i); + return 0; + } + c = buf[n + i++]; + if ( '\x03' != c ) { + Send_NAK(); + gpsd_report(LOG_RAW+1, "Final ETX not ETX\n"); + return 0; + } + + /* debug */ + /*@ -usedef -compdef @*/ + for ( i = 0 ; i < data_index ; i++ ) { + gpsd_report(LOG_RAW+1, "Char: %#02x\n", data_buf[i]); + } + + + gpsd_report(LOG_IO, + "garmin_ser_parse() Type: %#02x, Len: %#02x, chksum: %#02x\n", + pkt_id, pkt_len, chksum); + mask = PrintSERPacket(session, pkt_id, pkt_len, data_buf); + + // sending ACK too soon might hang the session + // so send ACK last, after a pause + (void)usleep(300); + Send_ACK(); + /*@ +usedef +compdef @*/ + return mask; +} +/*@ -charint @*/ + +#ifdef ALLOW_RECONFIGURE +static void settle(void) +{ + struct timespec delay, rem; + /*@ -type -unrecog @*/ + memset( &delay, 0, sizeof(delay)); + delay.tv_sec = 0; + delay.tv_nsec = 333000000L; + nanosleep(&delay, &rem); + /*@ +type +unrecog @*/ +} +#endif /* ALLOW_RECONFIGURE */ + +static void garmin_switcher(struct gps_device_t *session, int mode) +{ +#ifdef ALLOW_RECONFIGURE + if (mode == MODE_NMEA) { + /*@ +charint @*/ + const char switcher[] = {0x10,0x0A,0x02,0x26,0x00,0xCE,0x10,0x03}; + // Note hard-coded string length in the next line... + int status = (int)gpsd_write(session, switcher, sizeof(switcher)); + /*@ -charint @*/ + if (status == (int)sizeof(switcher)) { + gpsd_report(LOG_IO, "=> GPS: turn off binary %02x %02x %02x... \n" + , switcher[0], switcher[1], switcher[2]); + } else { + gpsd_report(LOG_ERROR, "=> GPS: FAILED\n"); + } + settle(); // wait 333mS, essential! + + /* once a sec, no binary, no averaging, NMEA 2.3, WAAS */ + (void)nmea_send(session, "$PGRMC1,1,1"); + //(void)nmea_send(fd, "$PGRMC1,1,1,1,,,,2,W,N"); + (void)nmea_send(session, "$PGRMI,,,,,,,R"); + settle(); // wait 333mS, essential! + } else { + (void)nmea_send(session, "$PGRMC1,1,2,1,,,,2,W,N"); + (void)nmea_send(session, "$PGRMI,,,,,,,R"); + // garmin serial binary is 9600 only! + gpsd_report(LOG_ERROR, "NOTE: Garmin binary is 9600 baud only!\n"); + settle(); // wait 333mS, essential! + } +#endif /* ALLOW_RECONFIGURE */ +} + +static ssize_t garmin_control_send(struct gps_device_t *session, + char *buf, size_t buflen) +/* not used by the daemon, it's for gpsctl and friends */ +{ + /*@ -mayaliasunique **/ + session->msgbuflen = buflen; + (void)memcpy(session->msgbuf, buf, buflen); + return gpsd_write(session, session->msgbuf, session->msgbuflen); + /*@ +mayaliasunique **/ +} + +/* this is everything we export */ +#ifdef __UNUSED__ +static int GetPacket (struct gps_device_t *session ); +//----------------------------------------------------------------------------- +// Gets a single packet. +// this is odd, the garmin usb driver will only return 64 bytes, or less +// at a time, no matter what you ask for. +// +// is you ask for less than 64 bytes then the next packet will include +// just the remaining bytes of the last 64 byte packet. +// +// Reading a packet of length Zero, or less than 64, signals the end of +// the entire packet. +// +// The Garmin sample WinXX code also assumes the same behavior, so +// maybe it is something in the USB protocol. +// +// Return: 0 = got a good packet +// -1 = error +// 1 = got partial packet +static int GetPacket (struct gps_device_t *session ) +{ + struct timespec delay, rem; + int cnt = 0; + // int x = 0; // for debug dump + + memset( session->driver.garmin.Buffer, 0, sizeof(Packet_t)); + memset( &delay, 0, sizeof(delay)); + session->driver.garmin.BufferLen = 0; + session->packet.outbuflen = 0; + + gpsd_report(LOG_IO, "GetPacket()\n"); + + for( cnt = 0 ; cnt < 10 ; cnt++ ) { + size_t pkt_size; + // Read async data until the driver returns less than the + // max async data size, which signifies the end of a packet + + // not optimal, but given the speed and packet nature of + // the USB not too bad for a start + ssize_t theBytesReturned = 0; + uint8_t *buf = (uint8_t *)session->driver.garmin.Buffer; + Packet_t *thePacket = (Packet_t*)buf; + + theBytesReturned = read(session->gpsdata.gps_fd + , buf + session->driver.garmin.BufferLen + , ASYNC_DATA_SIZE); + // zero byte returned is a legal value and denotes the end of a + // binary packet. + if ( 0 > theBytesReturned ) { + // read error... + // or EAGAIN, but O_NONBLOCK is never set + gpsd_report(LOG_ERROR, "GetPacket() read error=%d, errno=%d\n" + , theBytesReturned, errno); + continue; + } + gpsd_report(LOG_RAW, "got %d bytes\n", theBytesReturned); +#if 1 + gpsd_report(LOG_IO, "getPacket(), got %d bytes: %s\n", + theBytesReturned, + gpsd_hexdump_wrapper(thePacket, theBytesReturned, LOG_IO)); +#endif + + session->driver.garmin.BufferLen += theBytesReturned; + if ( 256 <= session->driver.garmin.BufferLen ) { + // really bad read error... + gpsd_report(LOG_ERROR, "GetPacket() packet too long, %ld > 255 !\n" + , session->driver.garmin.BufferLen); + session->driver.garmin.BufferLen = 0; + break; + } + pkt_size = 12 + get_int32((uint8_t*)&thePacket->mDataSize); + if ( 12 <= session->driver.garmin.BufferLen) { + // have enough data to check packet size + if ( session->driver.garmin.BufferLen > pkt_size) { + // wrong amount of data in buffer + gpsd_report(LOG_ERROR + , "GetPacket() packet size wrong! Packet: %ld, s/b %ld\n" + , session->driver.garmin.BufferLen + , pkt_size); + session->driver.garmin.BufferLen = 0; + break; + } + } + if ( 64 > theBytesReturned ) { + // zero length, or short, read is a flag for got the whole packet + break; + } + + + /*@ ignore @*/ + delay.tv_sec = 0; + delay.tv_nsec = 3330000L; + while (nanosleep(&delay, &rem) < 0) + continue; + /*@ end @*/ + } + // dump the individual bytes, debug only + // for ( x = 0; x < session->driver.garmin.BufferLen; x++ ) { + // gpsd_report(LOG_RAW+1, "p[%d] = %x\n", x, session->driver.garmin.Buffer[x]); + // } + if ( 10 <= cnt ) { + gpsd_report(LOG_ERROR, "GetPacket() packet too long or too slow!\n"); + return -1; + } + + gpsd_report(LOG_RAW, "GotPacket() sz=%d \n", session->driver.garmin.BufferLen); + session->packet.outbuflen = session->driver.garmin.BufferLen; + return 0; +} +static gps_mask_t garmin_usb_parse(struct gps_device_t *session) +{ + gpsd_report(LOG_PROG, "garmin_usb_parse()\n"); + return PrintUSBPacket(session, (Packet_t*)session->driver.garmin.Buffer); +} + +static ssize_t garmin_get_packet(struct gps_device_t *session) +{ + return (ssize_t)( 0 == GetPacket( session ) ? 1 : 0); +} + +const struct gps_type_t garmin_usb_binary_old = +{ + .type_name = "Garmin USB binary", /* full name of type */ + .packet_type = GARMIN_PACKET; /* associated lexer packet type */ + .trigger = NULL, /* no trigger, it has a probe */ + .channels = GARMIN_CHANNELS, /* consumer-grade GPS */ + .control_send = garmin_control_send, /* send raw bytes */ + .probe_wakeup = NULL, /* no wakeup to be done before hunt */ + .probe_detect = garmin_detect, /* how to detect at startup time */ + .probe_subtype = garmin_probe_subtype, /* get subtype info */ +#ifdef ALLOW_RECONFIGURE + .configurator = NULL, /* does not allow reconfiguration */ +#endif /* ALLOW_RECONFIGURE */ + .get_packet = garmin_get_packet,/* how to grab a packet */ + .parse_packet = garmin_usb_parse, /* parse message packets */ + .rtcm_writer = NULL, /* don't send DGPS corrections */ + .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 = garmin_close, /* close hook */ + .cycle = 1, /* updates every second */ +}; +#endif /* __UNUSED__ */ + +const struct gps_type_t garmin_usb_binary = +{ + .type_name = "Garmin USB binary", /* full name of type */ + .packet_type = GARMIN_PACKET, /* associated lexer packet type */ + .trigger = NULL, /* no trigger, it has a probe */ + .channels = GARMIN_CHANNELS, /* consumer-grade GPS */ + .control_send = garmin_control_send, /* send raw bytes */ + .probe_wakeup = NULL, /* no wakeup to be done before hunt */ + .probe_detect = garmin_detect, /* how to detect at startup time */ + .probe_subtype = garmin_probe_subtype, /* get subtype info */ +#ifdef ALLOW_RECONFIGURE + .configurator = NULL, /* enable what we need */ +#endif /* ALLOW_RECONFIGURE */ + .get_packet = generic_get, /* how to grab a packet */ + .parse_packet = garmin_ser_parse, /* parse message packets */ + .rtcm_writer = NULL, /* don't send DGPS corrections */ + .speed_switcher = NULL, /* no speed switcher */ + .mode_switcher = garmin_switcher, /* how to change modes */ + .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 = garmin_close, /* close hook */ + .cycle = 1, /* updates every second */ +}; + +const struct gps_type_t garmin_ser_binary = +{ + .type_name = "Garmin Serial binary", /* full name of type */ + .packet_type = GARMIN_PACKET, /* associated lexer packet type */ + .trigger = NULL, /* no trigger, it has a probe */ + .channels = GARMIN_CHANNELS, /* consumer-grade GPS */ + .control_send = garmin_control_send, /* send raw bytes */ + .probe_wakeup = NULL, /* no wakeup to be done before hunt */ + .probe_detect = NULL, /* how to detect at startup time */ + .probe_subtype = NULL, /* initialize the device */ +#ifdef ALLOW_RECONFIGURE + .configurator = NULL, /* enable what we need */ +#endif /* ALLOW_RECONFIGURE */ + .get_packet = generic_get, /* how to grab a packet */ + .parse_packet = garmin_ser_parse, /* parse message packets */ + .rtcm_writer = NULL, /* don't send DGPS corrections */ + .speed_switcher = NULL, /* no speed switcher */ + .mode_switcher = garmin_switcher, /* how to change modes */ + .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, /* close hook */ + .cycle = 1, /* updates every second */ +}; + +#endif /* GARMIN_ENABLE */ + diff --git a/driver_garmin_txt.c b/driver_garmin_txt.c new file mode 100644 index 00000000..ecd13aac --- /dev/null +++ b/driver_garmin_txt.c @@ -0,0 +1,405 @@ +/* + * 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 size_t length, const char *prefix, const double dividor, /*@out@*/double *result) +{ + char buf[10]; + float sign = 1.0; + int preflen = (int)strlen(prefix); + int offset = 1; /* assume one character prefix (E,W,S,N,U, D, etc) */ + int intresult; + + /* splint is buggy here, thinks buf can be a null pointer */ + /*@ -mustdefine -nullderef -nullpass @*/ + if (length >= sizeof(buf)) { + gpsd_report(LOG_ERROR, "internal buffer too small\n"); + return -1; + } + + bzero(buf, (int)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; + } + /*@ +mustdefine +nullderef +nullpass @*/ + + 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 size_t length, const int min, const int max, /*@out@*/int *result) +{ + char buf[3]; + int res; + + /*@ -mustdefine @*/ + if (length >= sizeof(buf)) { + gpsd_report(LOG_ERROR, "internal buffer too small\n"); + return -1; + } + + bzero(buf, (int)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; + } + + /*@ -nullpass @*/ /* splint bug */ + 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; + } + /*@ +mustdefine +nullpass @*/ +} + + +/************************************************************************** + * + * 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_wrapper(session->packet.outbuffer, + session->packet.outbuflen, LOG_RAW)); + + 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 = (char)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/driver_italk.c b/driver_italk.c new file mode 100644 index 00000000..64ae6ae1 --- /dev/null +++ b/driver_italk.c @@ -0,0 +1,527 @@ +/* $Id$ */ +/* + * Driver for the iTalk binary protocol used by FasTrax + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "gpsd_config.h" +#include "gpsd.h" +#if defined(ITRAX_ENABLE) && defined(BINARY_ENABLE) + +#include "bits.h" +#include "driver_italk.h" + +static gps_mask_t italk_parse(struct gps_device_t *, unsigned char *, size_t); +static gps_mask_t decode_itk_navfix(struct gps_device_t *, unsigned char *, size_t); +static gps_mask_t decode_itk_prnstatus(struct gps_device_t *, unsigned char *, size_t); +static gps_mask_t decode_itk_utcionomodel(struct gps_device_t *, unsigned char *, size_t); + +static gps_mask_t decode_itk_navfix(struct gps_device_t *session, unsigned char *buf, size_t len) +{ + unsigned int tow; + unsigned short gps_week, flags, cflags, pflags; + gps_mask_t mask = 0; + double epx, epy, epz, evx, evy, evz; + double t; + + if (len != 296){ + gpsd_report(LOG_PROG, "ITALK: bad NAV_FIX (len %zu, should be 296)\n", + len); + return -1; + } + + flags = getleuw(buf, 7 + 4); + cflags = getleuw(buf, 7 + 6); + pflags = getleuw(buf, 7 + 8); + + session->gpsdata.status = STATUS_NO_FIX; + session->gpsdata.fix.mode = MODE_NO_FIX; + mask = ONLINE_SET | MODE_SET | STATUS_SET | CYCLE_START_SET; + + /* just bail out if this fix is not marked valid */ + if (0 != (pflags & FIX_FLAG_MASK_INVALID) || 0 == (flags & FIXINFO_FLAG_VALID)) + return mask; + + gps_week = (ushort)getlesw(buf, 7 + 82); + tow = getleul(buf, 7 + 84); + t = gpstime_to_unix((int)gps_week, tow/1000.0) - session->context->leap_seconds; + session->gpsdata.sentence_time = t; + session->gpsdata.fix.time = t; + mask |= TIME_SET; + + epx = (double)(getlesl(buf, 7 + 96)/100.0); + epy = (double)(getlesl(buf, 7 + 100)/100.0); + epz = (double)(getlesl(buf, 7 + 104)/100.0); + evx = (double)(getlesl(buf, 7 + 186)/1000.0); + evy = (double)(getlesl(buf, 7 + 190)/1000.0); + evz = (double)(getlesl(buf, 7 + 194)/1000.0); + ecef_to_wgs84fix(&session->gpsdata, epx, epy, epz, evx, evy, evz); + mask |= LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET | CLIMB_SET ; + session->gpsdata.fix.eph = (double)(getlesl(buf, 7 + 252)/100.0); + session->gpsdata.fix.eps = (double)(getlesl(buf, 7 + 254)/100.0); + + session->gpsdata.satellites_used = 0xffff ^ getub(buf, 7 + 16); + mask |= USED_SET ; + + if (flags & FIX_CONV_DOP_VALID){ + session->gpsdata.hdop = (double)(getleuw(buf, 7 + 56)/100.0); + session->gpsdata.gdop = (double)(getleuw(buf, 7 + 58)/100.0); + session->gpsdata.pdop = (double)(getleuw(buf, 7 + 60)/100.0); + session->gpsdata.vdop = (double)(getleuw(buf, 7 + 62)/100.0); + session->gpsdata.tdop = (double)(getleuw(buf, 7 + 64)/100.0); + mask |= HDOP_SET | GDOP_SET | PDOP_SET | VDOP_SET | TDOP_SET; + } + + if ((pflags & FIX_FLAG_MASK_INVALID) == 0 && (flags & FIXINFO_FLAG_VALID) != 0){ + if (pflags & FIX_FLAG_3DFIX) + session->gpsdata.fix.mode = MODE_3D; + else + session->gpsdata.fix.mode = MODE_2D; + + if (pflags & FIX_FLAG_DGPS_CORRECTION) + session->gpsdata.status = STATUS_DGPS_FIX; + else + session->gpsdata.status = STATUS_FIX; + } + + return mask; +} + +static gps_mask_t decode_itk_prnstatus(struct gps_device_t *session, unsigned char *buf, size_t len) +{ + unsigned int i, tow, nsv, nchan, st; + unsigned short gps_week; + double t; + + if (len < 62){ + gpsd_report(LOG_PROG, "ITALK: runt PRN_STATUS (len=%zu)\n", len); + return ERROR_SET; + } + + gps_week = getleuw(buf, 7 + 4); + tow = getleul(buf, 7 + 6); + t = gpstime_to_unix((int)gps_week, tow/1000.0) - session->context->leap_seconds; + session->gpsdata.sentence_time = session->gpsdata.fix.time = t; + + gpsd_zero_satellites(&session->gpsdata); + nchan = (unsigned int)((len - 10 - 52) / 20); + nsv = 0; + for (i = st = 0; i < nchan; i++) { + unsigned int off = 7+ 52 + 20 * i; + unsigned short flags; + + flags = getleuw(buf, off); + session->gpsdata.ss[i] = (int)getleuw(buf, off+2)&0xff; + session->gpsdata.PRN[i] = (int)getleuw(buf, off+4)&0xff; + session->gpsdata.elevation[i] = (int)getlesw(buf, off+6)&0xff; + session->gpsdata.azimuth[i] = (int)getlesw(buf, off+8)&0xff; + if (session->gpsdata.PRN[i]){ + st++; + if (flags & PRN_FLAG_USE_IN_NAV) + session->gpsdata.used[nsv++] = session->gpsdata.PRN[i]; + } + } + session->gpsdata.satellites = (int)st; + session->gpsdata.satellites_used = (int)nsv; + + return USED_SET | SATELLITE_SET | TIME_SET; +} + +static gps_mask_t decode_itk_utcionomodel(struct gps_device_t *session, unsigned char *buf, size_t len) +{ + unsigned int tow; + int leap; + unsigned short gps_week, flags; + double t; + + if (len != 64){ + gpsd_report(LOG_PROG, + "ITALK: bad UTC_IONO_MODEL (len %zu, should be 64)\n", len); + return ERROR_SET; + } + + flags = getleuw(buf, 7); + if (0 == (flags & UTC_IONO_MODEL_UTCVALID)) + return 0; + + leap = (int)getleuw(buf, 7 + 24); + if (session->context->leap_seconds < leap) + session->context->leap_seconds = leap; + + gps_week = getleuw(buf, 7 + 36); + tow = getleul(buf, 7 + 38); + t = gpstime_to_unix((int)gps_week, tow/1000.0) - session->context->leap_seconds; + session->gpsdata.sentence_time = session->gpsdata.fix.time = t; + + return TIME_SET; +} + +/*@ +charint -usedef -compdef @*/ +static ssize_t italk_control_send(struct gps_device_t *session, + char *msg, size_t msglen) +{ + ssize_t status; + + /*@ -mayaliasunique **/ + session->msgbuflen = msglen; + (void)memcpy(session->msgbuf, msg, msglen); + /*@ +mayaliasunique **/ + /* we may need to dump the message */ + gpsd_report(LOG_IO, "writing italk control type %02x:%s\n", + msg[0], gpsd_hexdump_wrapper(msg, msglen, LOG_IO)); +#ifdef ALLOW_RECONFIGURE + status = write(session->gpsdata.gps_fd, msg, msglen); + (void)tcdrain(session->gpsdata.gps_fd); +#else + status = -1; +#endif /* ALLOW_RECONFIGURE */ + return(status); +} +/*@ -charint +usedef +compdef @*/ + +/*@ +charint @*/ +static gps_mask_t italk_parse(struct gps_device_t *session, unsigned char *buf, size_t len) +{ + unsigned int type; + gps_mask_t mask = 0; + + if (len == 0) + return 0; + + type = (uint)getub(buf, 4); + /* we may need to dump the raw packet */ + gpsd_report(LOG_RAW, "raw italk packet type 0x%02x length %zu: %s\n", + type, len, gpsd_hexdump_wrapper(buf, len, LOG_RAW)); + + switch (type) + { + case ITALK_NAV_FIX: + gpsd_report(LOG_IO, "iTalk NAV_FIX len %zu\n", len); + mask = decode_itk_navfix(session, buf, len); + break; + case ITALK_PRN_STATUS: + gpsd_report(LOG_IO, "iTalk PRN_STATUS len %zu\n", len); + mask = decode_itk_prnstatus(session, buf, len); + break; + case ITALK_UTC_IONO_MODEL: + gpsd_report(LOG_IO, "iTalk UTC_IONO_MODEL len %zu\n", len); + mask = decode_itk_utcionomodel(session, buf, len); + break; + + case ITALK_ACQ_DATA: + gpsd_report(LOG_IO, "iTalk ACQ_DATA len %zu\n", len); + break; + case ITALK_TRACK: + gpsd_report(LOG_IO, "iTalk TRACK len %zu\n", len); + break; + case ITALK_PSEUDO: + gpsd_report(LOG_IO, "iTalk PSEUDO len %zu\n", len); + break; + case ITALK_RAW_ALMANAC: + gpsd_report(LOG_IO, "iTalk RAW_ALMANAC len %zu\n", len); + break; + case ITALK_RAW_EPHEMERIS: + gpsd_report(LOG_IO, "iTalk RAW_EPHEMERIS len %zu\n", len); + break; + case ITALK_SUBFRAME: + gpsd_report(LOG_IO, "iTalk SUBFRAME len %zu\n", len); + break; + case ITALK_BIT_STREAM: + gpsd_report(LOG_IO, "iTalk BIT_STREAM len %zu\n", len); + break; + + case ITALK_AGC: + case ITALK_SV_HEALTH: + case ITALK_PRN_PRED: + case ITALK_FREQ_PRED: + case ITALK_DBGTRACE: + case ITALK_START: + case ITALK_STOP: + case ITALK_SLEEP: + case ITALK_STATUS: + case ITALK_ITALK_CONF: + case ITALK_SYSINFO: + case ITALK_ITALK_TASK_ROUTE: + case ITALK_PARAM_CTRL: + case ITALK_PARAMS_CHANGED: + case ITALK_START_COMPLETED: + case ITALK_STOP_COMPLETED: + case ITALK_LOG_CMD: + case ITALK_SYSTEM_START: + case ITALK_STOP_SEARCH: + case ITALK_SEARCH: + case ITALK_PRED_SEARCH: + case ITALK_SEARCH_DONE: + case ITALK_TRACK_DROP: + case ITALK_TRACK_STATUS: + case ITALK_HANDOVER_DATA: + case ITALK_CORE_SYNC: + case ITALK_WAAS_RAWDATA: + case ITALK_ASSISTANCE: + case ITALK_PULL_FIX: + case ITALK_MEMCTRL: + case ITALK_STOP_TASK: + gpsd_report(LOG_IO, + "iTalk not processing packet: id 0x%02x length %zu\n", + type, len); + break; + default: + gpsd_report(LOG_IO, "iTalk unknown packet: id 0x%02x length %zu\n", + type, len); + } + if (mask == ERROR_SET) + mask = 0; + else + (void)snprintf(session->gpsdata.tag, sizeof(session->gpsdata.tag), + "ITK-%02x",type); + + return mask | ONLINE_SET; +} +/*@ -charint @*/ + +static gps_mask_t italk_parse_input(struct gps_device_t *session) +{ + gps_mask_t st; + + if (session->packet.type == ITALK_PACKET){ + st = italk_parse(session, session->packet.outbuffer, session->packet.outbuflen); + session->gpsdata.driver_mode = MODE_BINARY; /* binary */ + return st; +#ifdef NMEA_ENABLE + } else if (session->packet.type == NMEA_PACKET) { + st = nmea_parse((char *)session->packet.outbuffer, session); + session->gpsdata.driver_mode = MODE_NMEA; /* NMEA */ + return st; +#endif /* NMEA_ENABLE */ + } else + return 0; +} + +static bool italk_set_mode(struct gps_device_t *session UNUSED, + speed_t speed UNUSED, + char parity UNUSED, int stopbits UNUSED, + bool mode UNUSED) +{ +#if 0 + /*@ +charint @*/ + char msg[] = {0,}; + + /* HACK THE MESSAGE */ + + return (italk_control_send(session, msg, sizeof(msg)) != -1); + /*@ +charint @*/ +#endif + + return false; /* until this actually works */ +} + +static bool italk_speed(struct gps_device_t *session, + speed_t speed, char parity, int stopbits) +{ + return italk_set_mode(session, speed, parity, stopbits, true); +} + +static void italk_mode(struct gps_device_t *session, int mode) +{ + if (mode == MODE_NMEA) { + (void)italk_set_mode(session, + session->gpsdata.baudrate, + (char)session->gpsdata.parity, + (int)session->gpsdata.stopbits, + false); + } +} + +#ifdef ALLOW_RECONFIGURE +static void italk_configurator(struct gps_device_t *session, unsigned int seq) +{ + if (seq == 0 && session->packet.type == NMEA_PACKET) + (void)italk_set_mode(session, + session->gpsdata.baudrate, + (char)session->gpsdata.parity, + (int)session->gpsdata.stopbits, + true); +} +#endif /* ALLOW_RECONFIGURE */ + +#ifdef __not_yet__ +static void italk_ping(struct gps_device_t *session) +/* send a "ping". it may help us detect an itrax more quickly */ +{ + char *ping = ""; + (void)gpsd_write(session, ping, 3); +} +#endif /* __not_yet__ */ + +/* this is everything we export */ +const struct gps_type_t italk_binary = +{ + .type_name = "iTalk binary", /* full name of type */ + .packet_type = ITALK_PACKET, /* associated lexer packet type */ + .trigger = NULL, /* recognize the type */ + .channels = 12, /* consumer-grade GPS */ + .control_send = italk_control_send, /* how to send a control string */ + .probe_wakeup = NULL, /* no wakeup to be done before hunt */ + .probe_detect = NULL, /* how to detect at startup time */ + .probe_subtype = NULL, /* initialize the device */ +#ifdef ALLOW_RECONFIGURE + .configurator = italk_configurator,/* configure the device */ +#endif /* ALLOW_RECONFIGURE */ + .get_packet = generic_get, /* use generic packet grabber */ + .parse_packet = italk_parse_input,/* parse message packets */ + .rtcm_writer = pass_rtcm, /* send RTCM data straight */ + .speed_switcher = italk_speed, /* we can change baud rates */ + .mode_switcher = italk_mode, /* there is a 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 close hook */ + .cycle = 1, /* updates every second */ +}; +#endif /* defined(ITRAX_ENABLE) && defined(BINARY_ENABLE) */ + +#ifdef ANCIENT_ITRAX_ENABLE +/************************************************************************** + * + * The NMEA mode of the iTrax chipset, as used in the FastTrax and others. + * + * As described by v1.31 of the NMEA Protocol Specification for the + * iTrax02 Evaluation Kit, 2003-06-12. + * v1.18 of the manual, 2002-19-6, describes effectively + * the same protocol, but without ZDA. + * + **************************************************************************/ + +/* + * Enable GGA=0x2000, RMC=0x8000, GSA=0x0002, GSV=0x0001, ZDA=0x0004. + * Disable GLL=0x1000, VTG=0x4000, FOM=0x0020, PPS=0x0010. + * This is 82+75+67+(3*60)+34 = 438 characters + * + * 1200 => at most 1 fix per 4 seconds + * 2400 => at most 1 fix per 2 seconds + * 4800 => at most 1 fix per 1 seconds + * 9600 => at most 2 fixes per second + * 19200 => at most 4 fixes per second + * 57600 => at most 13 fixes per second + * 115200 => at most 26 fixes per second + * + * We'd use FOM, but they don't specify a confidence interval. + */ +#define ITRAX_MODESTRING "$PFST,NMEA,A007,%d\r\n" + +static int literal_send(int fd, const char *fmt, ... ) +/* ship a raw command to the GPS */ +{ + ssize_t status; + va_list ap; + + va_start(ap, fmt) ; + (void)vsnprintf(session->msgbuf, sizeof(session->msgbuf), fmt, ap); + va_end(ap); + session->msgbuflen = strlen(session->msgbuf); + return gpsd_write(fd, session->msgbuf, session->msgbuflen); +} + +static void itrax_probe_subtype(struct gps_device_t *session, unsigned int seq) +/* start it reporting */ +{ + if (seq == 0) { + /* initialize GPS clock with current system time */ + struct tm when; + double integral, fractional; + time_t intfixtime; + char buf[31], frac[6]; + fractional = modf(timestamp(), &integral); + intfixtime = (time_t)integral; + (void)gmtime_r(&intfixtime, &when); + /* FIXME: so what if my local clock is wrong? */ + (void)strftime(buf, sizeof(buf), "$PFST,INITAID,%H%M%S.XX,%d%m%y\r\n", &when); + (void)snprintf(frac, sizeof(frac), "%.2f", fractional); + buf[21] = frac[2]; buf[22] = frac[3]; + (void)literal_send(session->gpsdata.gps_fd, buf); + /* maybe this should be considered a reconfiguration? */ + (void)literal_send(session->gpsdata.gps_fd, "$PFST,START\r\n"); + } +} + +#ifdef ALLOW_RECONFIGURE +static void itrax_configurator(struct gps_device_t *session, int seq) +/* set synchronous mode */ +{ + if (seq == 0) { + (void)literal_send(session->gpsdata.gps_fd, "$PFST,SYNCMODE,1\r\n"); + (void)literal_send(session->gpsdata.gps_fd, + ITRAX_MODESTRING, session->gpsdata.baudrate); + } +} +#endif /* ALLOW_RECONFIGURE */ + +static bool itrax_speed(struct gps_device_t *session, + speed_t speed, char parity UNUSED, int stopbits UNUSED) +/* change the baud rate */ +{ +#ifdef ALLOW_RECONFIGURE + return literal_send(session->gpsdata.gps_fd, ITRAX_MODESTRING, speed) >= 0; +#else + return false; +#endif /* ALLOW_RECONFIGURE */ +} + +static bool itrax_rate(struct gps_device_t *session, double rate) +/* change the sample rate of the GPS */ +{ +#ifdef ALLOW_RECONFIGURE + return literal_send(session->gpsdata.gps_fd, "$PSFT,FIXRATE,%d\r\n", rate) >= 0; +#else + return false; +#endif /* ALLOW_RECONFIGURE */ +} + +static void itrax_wrap(struct gps_device_t *session) +/* stop navigation, this cuts the power drain */ +{ +#ifdef ALLOW_RECONFIGURE + (void)literal_send(session->gpsdata.gps_fd, "$PFST,SYNCMODE,0\r\n"); +#endif /* ALLOW_RECONFIGURE */ + (void)literal_send(session->gpsdata.gps_fd, "$PFST,STOP\r\n"); +} + +/*@ -redef @*/ +const static struct gps_type_t itrax = { + .type_name = "iTrax", /* full name of type */ + .packet_type = NMEA_PACKET; /* associated lexer packet type */ + .trigger = "$PFST,OK", /* tells us to switch to Itrax */ + .channels = 12, /* consumer-grade GPS */ + .control_send = italk_control_send,/* how to send a control string */ + .probe_wakeup = NULL, /* no wakeup to be done before hunt */ + .probe_detect = NULL, /* no probe */ + .probe_subtype = itrax_probe_subtype, /* initialize */ +#ifdef ALLOW_RECONFIGURE + .configurator = itrax_configurator,/* set synchronous mode */ +#endif /* ALLOW_RECONFIGURE */ + .get_packet = generic_get, /* how to get a packet */ + .parse_packet = nmea_parse_input, /* how to interpret a packet */ + .rtcm_writer = NULL, /* iTrax doesn't support DGPS/WAAS/EGNOS */ + .speed_switcher= itrax_speed, /* how to change speeds */ + .mode_switcher = NULL, /* no mode switcher */ + .rate_switcher = itrax_rate, /* there's a sample-rate switcher */ + .cycle_chars = 438, /* not relevant, no rate switch */ +#ifdef ALLOW_RECONFIGURE + .revert = NULL, /* no setting-reversion method */ +#endif /* ALLOW_RECONFIGURE */ + .wrapup = itrax_wrap, /* sleep the receiver */ + .cycle = 1, /* updates every second */ +}; +/*@ -redef @*/ +#endif /* ITRAX_ENABLE */ + diff --git a/driver_italk.h b/driver_italk.h new file mode 100644 index 00000000..ec7ee111 --- /dev/null +++ b/driver_italk.h @@ -0,0 +1,498 @@ +/* $Id$ */ +#ifndef _GPSD_ITALK_H_ +#define _GPSD_ITALK_H_ + +/* 0 and 1 are responses to the ping for iTalk and NMEA respectively */ +#define PROTO_ITALK 0 +#define PROTO_NMEA 1 + +/* + * Assistance from Timo Ylhainen of Fastrax is acknowledged and appreciated. + * + * iTalk is a messaging system which communicates between tasks, which may + * be running on different devices (nodes). For our purposes (receiver + * configuration), we will probably be sending to the SYSTEM task. + */ + +#define TASK_MASK 0x1f /* 5 low bits of src/dst fields */ +#define NODE_MASK 0xe0 /* 3 high bits of src/dst fields */ +#define NODE_UNDEF 0x00 /* Used in message routing */ +#define NODE_ITRAX 0x20 /* The receiver */ +#define NODE_HOST 0x40 /* Software on your computer */ +#define NODE_GPSWB 0x60 /* GPSWorkbench seems to be HOST|ITRAX */ + +/* XXX These defines will likely be replaced by an enum + * once I map every message to the task that sent it. + */ +/* System controller on the receiver */ +#define TASK_SYSTEM 0 +/* Acquisition & Tracking messages (PD) */ +#define TASK_TRACK1 2 +#define TASK_TRACK2 3 +/* Data decoding messages (PD) */ +#define TASK_DATA 4 +/* Navigation messages are sent by these tasks (PD) */ +#define TASK_NAV1 7 +#define TASK_NAV2 8 +#define TASK_NAV3 9 +/* Host controller software (PD) */ +#define TASK_HOST 31 + +/* iTalk Message IDs - isuite.fastrax.fi/sdk/331/Protocols/PRO_MsgId.html */ +#define ITALK_ACQ_DATA 1 +#define ITALK_PRN_STATUS 2 +#define ITALK_TRACK 3 +#define ITALK_PSEUDO 4 +#define ITALK_AGC 6 +#define ITALK_NAV_FIX 7 +#define ITALK_RAW_ALMANAC 9 +#define ITALK_RAW_EPHEMERIS 10 +#define ITALK_SV_HEALTH 11 +#define ITALK_UTC_IONO_MODEL 12 +#define ITALK_PRN_PRED 13 +#define ITALK_FREQ_PRED 14 +#define ITALK_SUBFRAME 15 +#define ITALK_BIT_STREAM 18 +#define ITALK_DBGTRACE 19 +#define ITALK_START 64 +#define ITALK_STOP 65 +#define ITALK_SLEEP 66 +#define ITALK_STATUS 67 +#define ITALK_ITALK_CONF 68 +#define ITALK_SYSINFO 69 +#define ITALK_ITALK_TASK_ROUTE 70 +#define ITALK_PARAM_CTRL 71 +#define ITALK_PARAMS_CHANGED 72 +#define ITALK_START_COMPLETED 73 +#define ITALK_STOP_COMPLETED 74 +#define ITALK_LOG_CMD 75 +#define ITALK_SYSTEM_START 76 +#define ITALK_STOP_SEARCH 79 +#define ITALK_SEARCH 80 +#define ITALK_PRED_SEARCH 81 +#define ITALK_SEARCH_DONE 82 +#define ITALK_TRACK_DROP 88 +#define ITALK_TRACK_STATUS 90 +#define ITALK_HANDOVER_DATA 92 +#define ITALK_CORE_SYNC 93 +#define ITALK_WAAS_RAWDATA 96 +#define ITALK_ASSISTANCE 98 +#define ITALK_PULL_FIX 99 +#define ITALK_MEMCTRL 112 +#define ITALK_STOP_TASK 255 + +/* NAV_FIX */ +#define FIX_CONV_VEL_VALID 0x0002 +#define FIX_CONV_ACC_VALID 0x0004 +#define FIX_CONV_DOP_VALID 0x0010 +#define FIX_CONV_ERR_VALID 0x0020 +#define FIX_CONV_UTC_VALID 0x0040 +#define FIX_CONV_UND_VALID 0x0080 +#define FIX_CONV_MAG_VALID 0x0100 +#define FIX_CONV_GRID_VALID 0x0200 +#define FIX_CONV_VEL_ESTIMATED 0x0400 + +#define FIX_FLAG_POS_REJECT_FOM 0x0003 +#define FIX_FLAG_POS_REJECT_DOP 0x0004 +#define FIX_FLAG_POS_PINNING 0x0020 + +#define FIX_FLAG_VEL_REJECT_RES 0x0003 +#define FIX_FLAG_ACCELERATION 0x4000 +#define FIX_FLAG_VEL_RELIABLE 0x0020 +#define FIX_FLAG_VEL_RELIABLE_3D 0x0040 + +#define FIX_FLAG_MASK_INVALID 0x0007 +#define FIX_FLAG_REJECT_NUM_SV 0x0001 +#define FIX_FLAG_REJECT_POSTRAIM 0x0002 +#define FIX_FLAG_REJECT_OTHER 0x0007 +#define FIX_FLAG_RELIABLE 0x0008 +#define FIX_FLAG_PF_RAIM 0x0010 +#define FIX_FLAG_3DFIX 0x0100 +#define FIX_FLAG_DGPS_CORRECTION 0x0200 +#define FIX_FLAG_TROPO 0x0400 +#define FIX_FLAG_IONO 0x0800 +#define FIX_FLAG_INS 0x2000 + +#define FIXINFO_FLAG_VALID 0x0002 +#define FIXINFO_FLAG_NEW_FIX 0x0004 +#define FIXINFO_FLAG_SKY_FIX 0x0008 +#define FIXINFO_FLAG_AID_GPSTIME 0x0010 +#define FIXINFO_FLAG_AID_TIMESTAMP 0x0020 +#define FIXINFO_FLAG_AID_EPHEMERIS 0x0040 +#define FIXINFO_FLAG_AID_ALTITUDE 0x0080 +#define FIXINFO_FLAG_KALMAN 0x1000 +#define FIXINFO_FLAG_INTERNAL 0x2000 +#define FIXINFO_FLAG_FIRSTFIX 0x4000 + +/* PRN_STATUS */ +#define PRN_FLAG_FOUND 0x0001 +#define PRN_FLAG_TRACKING 0x0002 +#define PRN_FLAG_USE_IN_NAV 0x0004 + +/* UTC_IONO_MODEL */ +#define UTC_IONO_MODEL_UTCVALID 0x0001 +#define UTC_IONO_MODEL_IONOVALID 0x0002 + + +/* MEMCTRL */ +#define MEM_WRITE 0x0002 +#define MEM_READD 0x0003 +#define MEM_BOOT 0x0004 +#define MEM_ERASE 0x0006 +#define MEM_XTAL_CALIBRATE 0x000a +/* BOOT flags based on isuite.fastrax.fi/sdk/331/Protocols/PRO_NMEA.html */ +#define MEM_BOOT_NORMAL 0x0000 +#define MEM_BOOT_INT_FWLOADER 0x0001 +#define MEM_BOOT_DL_FWLOADER 0x0002 +#define MEM_BOOT_RELOC_ALTFW 0x0003 + +/* Config Parameters - isuite.fastrax.fi/sdk/331/System/SYS_Parameters.html */ +/* System parameters */ +#define SYS_SET_ID 0x0001 +#define SYS_FACTORY_SET_ID 0x0002 +#define SYS_AUTOSTART 0x0380 +#define START_MODE_AUTO 0x0301 +#define SYS_LKG_SAVE_TIME_LIMIT 0x0008 +#define SYS_LKG_SAVE_DIST_LIMIT 0x0009 +#define SYS_LKG_SAVE_STOP_TIME_LIMIT 0x000a +#define SYS_WATCHDOG 0x0028 +#define SYS_WATCHDOG_TIMEOUT 0x0029 +#define SYS_BOOT_ERASE_PARAMS 0x0080 +#define SYS_ENABLE_UI_LEDS 0x0081 + +/* Protocols parameters */ +#define SYS_ITALK_PORT 0x0010 +#define SYS_ITALK_SPEED 0x0011 +#define SYS_ITALK_MASK 0x0012 +#define SYS_NMEA_PORT 0x0020 +#define SYS_NMEA_SPEED 0x0021 +#define SYS_NMEA_MASK 0x0022 +#define TRACK_ALT_MSG_ROUTING 0x047f +#define OBS_ALT_MSG_ROUTING 0x047e + +/* Fix Conversion parameters */ +#define NAV_DATUM_ID 0x0b08 +#define NAV_GRID_ID 0x0b09 +#define NAV_GRID_NUMBER 0x0b0a +#define NAV_HEAD_VEL_THR 0x0b0b +#define NAV_HEAD_VEL_FILTER 0x0b0c +#define NAV_HEAD_VEL_THRMAX 0x0b0d +#define NAV_HEAD_VEL_THR_PLL 0x0b0e +#define NAV_HEAD_VEL_THRMAX_PLL 0x0b0f +#define NAV_HOLD_HEADING_IF_NO_FIX 0x0bd0 + +/* General navigation parameters */ +#define NAV_MODE 0x0b01 +#define NAV_FIX_INTERVAL 0x0b02 +#define NAV_OUTPUT_INTERVAL 0x0b03 +#define NAV_FOM_LIMIT 0x0b10 +#define NAV_VEL_FOM_LIMIT 0x0b15 +#define NAV_HDOP_LIMIT 0x0b11 +#define NAV_VDOP_LIMIT 0x0b12 +#define NAV_ALT_LIMIT 0x0b13 +#define NAV_VEL_LIMIT 0x0b14 +#define NAV_EXT_AIDING_ALT 0x0b20 +#define NAV_CS_INIT_VAR 0x0b30 +#define NAV_CS_PROC_VAR 0x0b31 +#define NAV_CS_MEAS_VAR 0x0b32 +#define NAV_FILTER_VEL_LOW 0x0b33 +#define NAV_FILTER_VEL_HIGH 0x0b34 +#define NAV_MAX_LKGAGE 0x0b40 +#define NAV_MAX_2D_FIX_SEC 0x0b41 +#define NAV_CARRIERSMOOTHING_ENA 0x0b81 +#define NAV_OLD_DATA_ENA 0x0b82 +#define NAV_SNR_WEIGHTING_ENA 0x0b83 +#define NAV_NORMAL_ENV_ENA 0x0b84 +#define NAV_IONO_ENA 0x0b85 +#define NAV_TROPO_ENA 0x0b87 +#define NAV_DGPS_ENA 0x0b88 +#define NAV_VEL_FILTER_ENA 0x0b8b +#define NAV_ALT_LIMIT_ENA 0x0b8c +#define NAV_VEL_LIMIT_ENA 0x0b8d +#define NAV_EXT_AIDING_ALT_ENA 0x0b8e +#define NAV_FOM_ENA 0x0b8f +#define NAV_HDOP_ENA 0x0b90 +#define NAV_VDOP_ENA 0x0b91 +#define NAV_TENTATIVE_ENA 0x0b96 +#define NAV_PULLFIX_ENA 0x0b97 +#define NAV_2D_FIX_ENA 0x0ba0 +#define NAV_RESERVED_001 0x0ba1 +#define NAV_OUTPUT_LAST_POS_IF_NO_FIX 0x0bb0 +#define NAV_ESTIMATE_VEL_WITHOUT_PLL 0x0bb1 +#define NAV_OUTPUT_LAST_VEL_IF_NO_FIX 0x0bb2 + +/* Position pinning parameters */ +#define NAV_PIN_VEL 0x0b35 +#define NAV_PIN_DRIFT_ERR 0x0b36 +#define NAV_PIN_XYZ_ERR 0x0b37 +#define NAV_PIN_TIMEOUT 0x0b38 +#define NAV_PIN_START_DELAY 0x0b39 +#define NAV_PINNING_ENA 0x0b8a + +/* Interval mode parameters */ +#define NAV_INTMODE_NBR_FIXES 0x0b22 +#define NAV_INTMODE_FIX_INTERVAL 0x0b23 +#define NAV_INTMODE_TRY_FIND_SV 0x0b24 +#define NAV_INTMODE_TRY_GET_FIX 0x0b25 +#define NAV_INTMODE_MAX_STAY_UP 0x0b26 +#define NAV_INTMODE_NUM_IGNORED_FIXES 0x0b27 +#define NAV_INTERVAL_MODE_ENA 0x0ba2 + +/* Kalman navigation parameters */ +#define KLM_MODE 0x0801 +#define KLM_MAX_NUM_STATES 0x0802 +#define KLM_START_FLAGS 0x0803 +#define KLM_OUTPUT_FLAGS 0x0804 +#define KLM_NUM_OBS_LIMIT 0x0805 +#define KLM_MEAS_FLAGS 0x0806 +#define KLM_COV_LIMIT 0x0807 +#define KLM_DOPPLER_NOISE 0x0810 +#define KLM_RANGE_NOISE 0x0811 +#define KLM_DOPPLER_NOISE_LOW 0x0812 +#define KLM_RANGE_NOISE_LOW 0x0813 +#define KLM_NOISE_SNR_LOW 0x0814 +#define KLM_DOPPLER_NOISE_PLL 0x0815 +#define KLM_RANGE_NOISE_PLL 0x0816 +#define KLM_CLOCK_OFFSET_NOISE 0x0820 +#define KLM_CLOCK_DRIFT_NOISE 0x0821 +#define KLM_POS_NOISE 0x0822 +#define KLM_POS_NOISE_VERT 0x0823 +#define KLM_VEL_NOISE 0x0824 +#define KLM_VEL_NOISE_VERT 0x0825 +#define KLM_ACC_NOISE 0x0826 +#define KLM_ACC_NOISE_VERT 0x0827 +#define KLM_ACC_NOISE_PARAM 0x0828 +#define KLM_POS_INIT_UNC 0x0830 +#define KLM_VEL_INIT_UNC 0x0831 +#define KLM_CLOCK_OFFSET_INIT_UNC 0x0832 +#define KLM_CLOCK_DRIFT_INIT_UNC 0x0833 +#define KLM_RESERVED_001 0x0841 +#define KLM_RESERVED_002 0x0842 +#define KLM_RESERVED_003 0x0843 +#define KLM_RESERVED_004 0x0844 +#define KLM_RESERVED_005 0x0845 +#define KLM_RESERVED_006 0x0846 +#define KLM_RESERVED_007 0x0847 +#define KLM_RESERVED_008 0x0848 + +/* Observation parameters */ +#define TRACK_MEAS_INTERVAL 0x0420 +#define TRACK_CHANNELS 0x041d +#define OBS_ELEV_LIMIT 0x0101 +#define OBS_SNR_LIMIT 0x0102 +#define OBS_SNR_RAIM_LIMIT 0x0103 +#define OBS_CROSS_CORR_SNR_DIFF 0x0120 +#define OBS_MAX_SNR 0x0121 +#define OBS_PLL_CROSS_CORR_THR 0x0122 +#define OBS_FLL_CROSS_CORR_THR 0x0123 +#define OBS_FREQ_CROSS_CORR_THR 0x0124 +#define OBS_EPOCH_LIMIT 0x0130 +#define OBS_ELEV_LIMIT_ENA 0x0181 +#define OBS_SNR_LIMIT_ENA 0x0182 +#define OBS_SNR_RAIM_ENA 0x0183 +#define SAT_ORBIT_FIT_UPDATE 0x0203 +#define SAT_FIRST_WEEK 0x0204 +#define SAT_NUM_LEAP 0x0205 +#define SAT_PRED_MAX_LKGAGE 0x0220 +#define SAT_PRED_PHASE_TIMEOUT 0x0221 +#define SAT_PRED_LKG_TIMEOUT 0x0222 +#define SAT_ORBIT_CHECK 0x0281 + +/* Unav Tracking parameters */ +#define TRACK_DLL_ALPHA 0x0401 +#define TRACK_DLL_BETA 0x0402 +#define TRACK_DLL_THR_HIGH 0x0403 +#define TRACK_DLL_THR_LOW 0x0404 +#define TRACK_DLL_POW_NARROW 0x0405 +#define TRACK_DLL_POW_WIDE 0x0406 +#define TRACK_FLL_RESPONSE_TIME 0x0407 +#define TRACK_POW_CALIBRATION 0x0408 +#define TRACK_FLL_THR 0x0409 +#define TRACK_FLL_POW_NARROW 0x040b +#define TRACK_FLL_POW_WIDE 0x040c +#define TRACK_PLL_CTH 0x040d +#define TRACK_PLL_CDTH 0x040e +#define TRACK_PLL_CD2TH 0x040f +#define TRACK_RESERVED_000 0x0410 +#define TRACK_RESERVED_001 0x0411 +#define TRACK_RESERVED_002 0x0412 +#define TRACK_RESERVED_003 0x0413 +#define TRACK_RESERVED_004 0x0414 +#define TRACK_RESERVED_005 0x0415 +#define TRACK_RESERVED_006 0x0416 +#define TRACK_RESERVED_007 0x0417 +#define TRACK_RESERVED_008 0x0418 +#define TRACK_RESERVED_009 0x0419 +#define TRACK_RESERVED_010 0x0425 +#define TRACK_RESERVED_011 0x0426 +#define TRACK_RESERVED_012 0x0427 +#define TRACK_RESERVED_013 0x0428 +#define TRACK_RESERVED_014 0x0429 +#define TRACK_RESERVED_016 0x042a +#define TRACK_RESERVED_017 0x042b +#define TRACK_RESERVED_015 0x0483 +#define SUBF_CHECK_FLAGS 0x0432 + +/* Unav Track task parameters */ +#define TRACK_GROUP_1 0x041a +#define TRACK_GROUP_2 0x041b +#define TRACK_GROUP_2_DELAY 0x041c +#define TRACK_CC_DELAY 0x041e +#define TRACK_CC_THR 0x041f +#define TRACK_PLL_ENA 0x0480 +#define TRACK_NAVAID_ENA 0x0482 +#define TRACK_SHIFT_REG 0x0421 + +/* Agc config parameters */ +#define TRACK_AGC_LO 0x0422 +#define TRACK_AGC_HI 0x0423 +#define TRACK_AGC_MAX_HI 0x0424 +#define TRACK_AGC_ENA 0x0481 + +/* PPS parameters */ +#define PPS_DUTYCYCLE 0x0440 +#define PPS_FREQ 0x0441 +#define PPS_DELAY 0x0442 +#define PPS_SURVEYLEN 0x0443 +#define PPS_MEAS_MS 0x0444 +#define PPS_ENA 0x0490 +#define PPS_SYNC_TRACK 0x0491 +#define PPS_ENA_PRED 0x0492 +#define PPS_INVERT 0x0493 + +/* Frequency plan parameters */ +#define FREQ_XTAL 0x0501 +#define FREQ_MCLK_NOM 0x0502 +#define FREQ_MCLK_DENOM 0x0503 +#define FREQ_RF_NOM 0x0504 +#define FREQ_RF_DENOM 0x0505 +#define FREQ_MIXER_OFFSET 0x0506 +#define FREQ_TME2 0x0507 +#define FREQ_PARAM_ENA 0x0581 + +/* Search parameters */ +#define SEARCH_XTAL_UNC 0x0701 +#define SEARCH_DOPPLER_UNC 0x0702 +#define SEARCH_WIN_PRED_EVEN 0x0703 +#define SEARCH_WIN_PRED_ODD 0x0704 +#define SEARCH_SENS_FULL_R1 0x0705 +#define SEARCH_SENS_FULL_R2 0x0706 +#define SEARCH_SENS_FULL_R3 0x0707 +#define SEARCH_SENS_PRED_EVEN 0x0708 +#define SEARCH_SENS_PRED_ODD 0x0709 +#define SEARCH_PRED_ROUNDS 0x070a +#define SEARCH_BACK_PRNS 0x070b +#define SEARCH_GPS_MASK 0x070c +#define SEARCH_WAAS_MASK 0x070d +#define SEARCH_AUTO_PD_ROUNDS 0x070e +#define SEARCH_FLAGS 0x070f +#define SEARCH_PREC_PRED_TIMEOUT 0x0710 +#define SEARCH_PRED_TIMEOUT 0x0711 +#define SEARCH_FERRY_COND 0x0712 +#define SEARCH_IFFERRY_PRED_COND 0x0713 +#define SEARCH_TUNNEL_IN_SNR 0x0714 +#define SEARCH_TUNNEL_OUT_SNR 0x0715 +#define SEARCH_PRED_ENA 0x0781 +#define SEARCH_BITSYNC_ENA 0x0782 +#define SEARCH_AUTO_PRED_ENA 0x0783 +#define SEARCH_AUTO_PD_ENA 0x0784 +#define SEARCH_SE_PD 0x0785 + +/* Unav Acquisition parameters */ +#define ACQ_SENS_9_COH 0x0901 +#define ACQ_SENS_9_NONCOH 0x0902 +#define ACQ_SENS_9_THR 0x0903 +#define ACQ_SENS_9_BIN 0x0904 +#define ACQ_SENS_10_COH 0x0905 +#define ACQ_SENS_10_NONCOH 0x0906 +#define ACQ_SENS_10_THR 0x0907 +#define ACQ_SENS_10_BIN 0x0908 +#define ACQ_MSG_ENA 0x0981 +#define ACQ_QUICK_SEARCH_ENA 0x0982 +#define SE_NONCOH_SHIFT 0x0940 +#define SE_IR_SHIFT 0x0941 +#define SE_THR 0x0942 +#define SE_INT_ENA 0x09a0 + +/* Logging parameters */ +#define LOG_MODE 0x0d01 +#define LOG_INTERVAL_MIN 0x0d02 +#define LOG_INTERVAL_MAX 0x0d03 +#define LOG_MOVE_MIN 0x0d04 +#define LOG_MOVE_MAX 0x0d05 +#define LOG_VELOCITY_MIN 0x0d06 +#define LOG_VELOCITY_MAX 0x0d07 +#define LOG_MAXITEMS 0x0d08 +#define LOG_STORE_LAT_LONG 0x0d80 +#define LOG_STORE_ALT 0x0d81 +#define LOG_STORE_ALT_FULL 0x0d82 +#define LOG_STORE_GPSTIME 0x0d83 +#define LOG_STORE_GPSTIME_MS 0x0d84 +#define LOG_STORE_DIRECTION 0x0d85 +#define LOG_STORE_VEL 0x0d86 +#define LOG_STORE_VEL_VERT 0x0d87 +#define LOG_STORE_FIXINFO 0x0d88 + +/* SBAS parameters */ +#define WAAS_TIMEOUT_MODE 0x0b60 +#define WAAS_MAX_CHANNELS 0x0b61 +#define WAAS_ENA 0x0bc0 +#define WAAS_MSG_0_ENA 0x0bc1 +#define WAAS_STRICT_ENA 0x0bc2 + +/* Sony Track parameters */ +#define TRACK_DLL_COEFF_GPS 0x0f01 +#define TRACK_DLL_COEFF_DISCR 0x0f02 +#define TRACK_DLL_LIM_GPS 0x0f03 +#define TRACK_DLL4_COEFF_A 0x0f04 +#define TRACK_DLL4_COEFF_B 0x0f05 +#define TRACK_DLL4_COEFF_C 0x0f06 +#define TRACK_DLL4_COEFF_D 0x0f07 +#define TRACK_DLL4_FASTADJ_THRES 0x0f08 +#define TRACK_ELGATE_NARROW 0x0f09 +#define TRACK_COSTASLF_GPS 0x0f0a +#define TRACK_COSTASLF_WAAS 0x0f0b +#define TRACK_LPF_GPS_ACQ 0x0f0c +#define TRACK_LPF_GPS_LOCK 0x0f0d +#define TRACK_LPF_WAAS_LOCK 0x0f0e +#define TRACK_LPF_NOISE 0x0f0f +#define TRACK_SIGDETECT_TH 0x0f10 +#define TRACK_SIGDETECT_TH_HS 0x0f11 +#define TRACK_TIMEOUT_ACQ 0x0f12 +#define TRACK_TIMEOUT_ACQHS 0x0f13 +#define TRACK_TIMEOUT_REACQ 0x0f14 +#define TRACK_HANDOVER_OFFSET 0x0f15 +#define TRACK_CROSSCORR_THRES 0x0f16 +#define TRACK_DLLCTRL_INTERVAL 0x0f17 +#define TRACK_BITEXTRACT 0x0f18 +#define TRACK_RESERVED001 0x0f19 +#define TRACK_RESERVED002 0x0f1a +#define TRACK_WAAS_PRN_BITSTREAM 0x0f1b +#define TRACK_COSTAS_ERROR_TH 0x0f1d +#define TRACK_CARRFLT_OUT_TH 0x0f1e +#define TRACK_CARRFLT_MIDDLE_TH 0x0f1f +#define TRACK_CARRFLT_OUT_DIV 0x0f20 +#define TRACK_CARRFLT_MIDDLE_DIV 0x0f21 +#define TRACK_CARRFLT_INBAND_DIV 0x0f22 +#define TRACK_LATCHTIME_OFFSET 0x0f23 +#define TRACK_DIRECTHANDOVER_OFFSET 0x0f24 +#define TRACK_EN_HS 0x0f80 +#define TRACK_CARR_AID 0x0f81 +#define WAAS_EN_DECODE 0x0f82 +#define TRACK_CARRCHKATLOCK 0x0f83 +#define TRACK_BL_REACQ 0x0f84 + +/* Sony Test parameters */ +#define SONYTEST_DISABLE_PORTS 0x0f85 + +/* Sony Acq parameters */ +#define SACQ_SEARCH_CH_NUM 0x0f30 +#define SACQ_NOISE_COUNT_NUM 0x0f31 +#define SACQ_NOISE_VALID_TIME 0x0f32 +#define SACQ_NOISE_K 0x0f33 +#define SACQ_PEAK_FD 0x0f34 +#define SACQ_PEAK_NFD 0x0f35 +#define SACQ_RESERVE 0x0f36 +#define SACQ_SEARCH_CH_NUM_VALID 0x0f96 + +#endif /* _GPSD_ITALK_H_ */ diff --git a/driver_navcom.c b/driver_navcom.c new file mode 100644 index 00000000..b24b39aa --- /dev/null +++ b/driver_navcom.c @@ -0,0 +1,1276 @@ +/* + * Driver for Navcom receivers using propietary NCT messages, a binary protocol. + * + * Vendor website: http://www.navcomtech.com/ + * Technical references: http://www.navcomtech.com/support/docs.cfm + * + * Tested with two SF-2040G models + * + * At this stage, this driver implements the following commands: + * + * 0x20: Data Request (tell the unit which responses you want) + * 0x3f: LED Configuration (controls the front panel LEDs -- for testing) + * 0x1c: Test Support Block (again, blinks the front panel lights) + * + * and it understands the following responses: + * + * 0x06: Acknowledgement (without error) + * 0x15: Negative Acknowledge + * 0x86: Channel Status + * 0xae: Identification Block + * 0xb0: Raw Meas. Data Block + * 0xb1: PVT Block + * 0xb5: Pseudorange Noise Statistics + * 0xd3: LBM DSP Status Block + * 0xef: Clock Drift and Offset + * + * FIXME - I'm not too sure of the way I have computed the vertical positional error + * I have used FOM as a scaling factor for VDOP, thusly VRMS = FOM/HDOP*VDOP + * + * By Diego Berge. Contact via web form at http://www.navlost.eu/contact + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "gpsd_config.h" +#include "gpsd.h" + +#if defined(NAVCOM_ENABLE) && defined(BINARY_ENABLE) +#include "bits.h" + +/* Have data which is 24 bits long */ +#define getlesl24(buf,off) (int32_t)(((u_int32_t)getub((buf), (off)+2)<<24 | (u_int32_t)getub((buf), (off)+1)<<16 | (u_int32_t)getub((buf), (off))<<8)>>8) +#define getleul24(buf,off) (u_int32_t)(((u_int32_t)getub((buf), (off)+2)<<24 | (u_int32_t)getub((buf), (off)+1)<<16 | (u_int32_t)getub((buf), (off))<<8)>>8) + +/* And just to be difficult, Navcom is little endian but the GPS data stream + is big endian. Some messages contain raw GPS data */ +#define getlesw_be(buf, off) (int16_t)((((u_int16_t)getub(buf, (off)) << 8) \ + | (u_int16_t)getub(buf, (off)+1))) +#define getleuw_be(buf, off) (u_int16_t)((((u_int16_t)getub(buf, (off)) << 8) \ + | (u_int16_t)getub(buf, (off)+1))) +#define getlesl_be(buf, off) (int32_t)((((u_int16_t)getleuw_be(buf, (off)) << 16) \ + | getleuw_be(buf, (off)+2))) +#define getleul_be(buf, off) (u_int32_t)((((u_int16_t)getleuw_be(buf, (off)) << 16) \ + | getleuw_be(buf, (off)+2))) +#define getlesL_be(buf, off) (int64_t)((((u_int64_t)getleul_be(buf, (off)) << 32) \ + | getleul_be(buf, (off)+4))) +#define getleuL_be(buf, off) (u_int64_t)((((u_int64_t)getleul_be(buf, (off)) << 32) \ + | getleul_be(buf, (off)+4))) +#define getlesl24_be(buf,off) (int32_t)(((u_int32_t)getub((buf), (off))<<24 \ + | (u_int32_t)getub((buf), (off)+1)<<16 \ + | (u_int32_t)getub((buf), (off)+2)<<8)>>8) + +#define NAVCOM_CHANNELS 12 + +static u_int8_t checksum(unsigned char *buf, size_t len) +{ + size_t n; + u_int8_t csum = (u_int8_t)0x00; + for(n = 0; n < len; n++) + csum ^= buf[n]; + return csum; +} + +static ssize_t navcom_control_send(struct gps_device_t *session, + char *buf, size_t len) +{ + /*@ +ignoresigns -mayaliasunique @*/ + putbyte(session->msgbuf, 0, 0x02); + putbyte(session->msgbuf, 1, 0x99); + putbyte(session->msgbuf, 2, 0x66); + putbyte(session->msgbuf, 3, buf[0]); /* Cmd ID */ + putleword(session->msgbuf, 4, len+4); /* Length */ + memcpy(session->msgbuf, buf+6, len-1); + putbyte(session->msgbuf, 6 + len, checksum((unsigned char *)session->msgbuf+3, len+5)); + putbyte(session->msgbuf, 7 + len, 0x03); + session->msgbuflen = len+9; + /*@ -ignoresigns +mayaliasunique @*/ + gpsd_report(LOG_RAW, "Navcom: control dump: %s\n", + gpsd_hexdump_wrapper(session->msgbuf, session->msgbuflen, LOG_RAW)); + return gpsd_write(session, session->msgbuf, session->msgbuflen); +} + +static bool navcom_send_cmd(struct gps_device_t *session, unsigned char *cmd, size_t len) +{ + gpsd_report(LOG_RAW, "Navcom: command dump: %s\n", + gpsd_hexdump_wrapper(cmd, len, LOG_RAW)); + return (gpsd_write(session, cmd, len) == (ssize_t)len); +} + +/* Data Request */ +static void navcom_cmd_0x20(struct gps_device_t *session, u_int8_t block_id, u_int16_t rate) +{ + unsigned char msg[18]; + putbyte(msg, 0, 0x02); + putbyte(msg, 1, 0x99); + putbyte(msg, 2, 0x66); + putbyte(msg, 3, 0x20); /* Cmd ID */ + putleword(msg, 4, 0x000e); /* Length */ + putbyte(msg, 6, 0x00); /* Action */ + putbyte(msg, 7, 0x01); /* Count of blocks */ + putbyte(msg, 8, block_id); /* Data Block ID */ + putbyte(msg, 9, 0x02); /* Logical Ports */ + putleword(msg, 10, rate); /* Data rate */ + putbyte(msg, 12, 0x71); + putbyte(msg, 13, 0x00); + putleword(msg, 14, 0x0000); + putbyte(msg, 16, checksum(msg+3, 13)); + putbyte(msg, 17, 0x03); + (void)navcom_send_cmd(session, msg, 18); + gpsd_report(LOG_PROG, + "Navcom: sent command 0x20 (Data Request) " + "- data block id = %02x at rate %02x\n", + block_id, rate); +} + +/*@ unused @*/ +/* Changes the LED settings in the receiver */ +static void UNUSED navcom_cmd_0x3f(struct gps_device_t *session) +{ + unsigned char msg[12]; + putbyte(msg, 0, 0x02); + putbyte(msg, 1, 0x99); + putbyte(msg, 2, 0x66); + putbyte(msg, 3, 0x3f); /* Cmd ID */ + putleword(msg, 4, 0x0008); + putbyte(msg, 6, 0x01); /* Action */ + putbyte(msg, 7, 0x00); /* Reserved */ + putbyte(msg, 8, 0x02); /* Link LED setting */ + putbyte(msg, 9, 0x0a); /* Battery LED setting */ + putbyte(msg, 10, checksum(msg+3, 7)); + putbyte(msg, 11, 0x03); + (void)navcom_send_cmd(session, msg, 12); + gpsd_report(LOG_PROG, + "Navcom: sent command 0x3f (LED Configuration Block)\n"); +} + +/* Test Support Block - Blinks the LEDs */ +static void navcom_cmd_0x1c(struct gps_device_t *session, u_int8_t mode, u_int8_t length) +{ + unsigned char msg[12]; + putbyte(msg, 0, 0x02); + putbyte(msg, 1, 0x99); + putbyte(msg, 2, 0x66); + putbyte(msg, 3, 0x1c); /* Cmd ID */ + putleword(msg, 4, 0x0008); + putbyte(msg, 6, 0x04); /* Use ACK/NAK */ + putbyte(msg, 7, mode); /* 0x01 or 0x02 */ + putbyte(msg, 8, length); /* Only if mode == 0x01 */ + putbyte(msg, 9, 0x00); + putbyte(msg, 10, checksum(msg+3, 7)); + putbyte(msg, 11, 0x03); + (void)navcom_send_cmd(session, msg, 12); + gpsd_report(LOG_PROG, + "Navcom: sent command 0x1c (Test Support Block)\n"); + gpsd_report(LOG_IO, + "Navcom: command 0x1c mode = %02x, length = %u\n", + mode, length); +} + +/* Serial Port Configuration */ +static void navcom_cmd_0x11(struct gps_device_t *session, u_int8_t port_selection) +{ + /* NOTE - We only allow changing one port at a time, + although the message supports doing both at once. */ + unsigned char msg[12]; + putbyte(msg, 0, 0x02); + putbyte(msg, 1, 0x99); + putbyte(msg, 2, 0x66); + putbyte(msg, 3, 0x11); /* Cmd ID */ + putleword(msg, 4, 0x0008); /* Length */ + putbyte(msg, 6, 0x04); /* Action - Use ACK/NAK) */ + putbyte(msg, 7, port_selection); + putbyte(msg, 8, 0x00); /* Reserved */ + putbyte(msg, 9, 0x00); /* Reserved */ + putbyte(msg, 10, checksum(msg+3, 7)); + putbyte(msg, 11, 0x03); + (void)navcom_send_cmd(session, msg, 12); + gpsd_report(LOG_PROG, + "Navcom: sent command 0x11 (Serial Port Configuration)\n"); + gpsd_report(LOG_IO, + "Navcom: serial port selection: 0x%02x\n", port_selection); +} + +static void navcom_probe_subtype(struct gps_device_t *session, unsigned int seq) +{ + /* Request the following messages: */ + if (seq==0) { + /*@ +charint @*/ + navcom_cmd_0x1c(session, 0x01, 5); /* Blink LEDs on receiver */ + navcom_cmd_0x20(session, 0xae, 0x1770); /* Identification Block - send every 10 min*/ + navcom_cmd_0x20(session, 0xb1, 0x4000); /* PVT Block */ + navcom_cmd_0x20(session, 0xb5, 0x00c8); /* Pseudorange Noise Statistics - send every 20s */ + navcom_cmd_0x20(session, 0xb0, 0x4000); /* Raw Meas Data Block */ + navcom_cmd_0x20(session, 0x81, 0x0000); /* Packed Ephemeris Data - send once */ + navcom_cmd_0x20(session, 0x81, 0x4000); /* Packed Ephemeris Data */ + navcom_cmd_0x20(session, 0x86, 0x4000); /* Channel Status */ + navcom_cmd_0x20(session, 0x83, 0x4000); /* Ionosphere and UTC Data */ + navcom_cmd_0x20(session, 0xef, 0x0bb8); /* Clock Drift - send every 5 min */ + /*@ -charint @*/ + } +} + +static void navcom_ping(struct gps_device_t *session) +{ +/* NOTE - This allows us to know into which of the unit's various + serial ports we are connected. + Its value gets updated every time we receive a 0x06 (Ack) + message. Note that if commands are being fed into the + unit from more than one port (which is entirely possible + although not necessarily a bright idea), there is a good + chance that we might misidentify our port */ + /*@ -type @*/ + session->driver.navcom.physical_port = 0xFF; + + navcom_cmd_0x1c(session, 0x02, 0); /* Test Support Block */ + navcom_cmd_0x20(session, 0xae, 0x0000); /* Identification Block */ + navcom_cmd_0x20(session, 0x86, 0x000a); /* Channel Status */ + /*@ +type @*/ +} + +static bool navcom_speed(struct gps_device_t *session, + unsigned int speed, char parity, int stopbits) +{ +#ifdef ALLOW_RECONFIGURE + /* parity and stopbit switching aren't implemented */ + if (parity!=(char)session->gpsdata.parity || stopbits!=(int)session->gpsdata.parity) { + return false; + } else { + u_int8_t port_selection; + u_int8_t baud; + if (session->driver.navcom.physical_port == (unsigned char)0xFF) { + /* We still don't know which port we're connected to */ + return false; + } + /*@ +charint @*/ + switch (speed) { + /* NOTE - The spec says that certain baud combinations + on ports A and B are not allowed, those are + 1200/115200, 2400/57600, and 2400/115200. + To try and minimise the possibility of those + occurring, we do not allow baud rates below + 4800. We could also disallow 57600 and 115200 + to totally prevent this, but I do not consider + that reasonable. Finding which baud speed the + other port is set at would also be too much + trouble, so we do not do it. */ + case 4800: + baud = 0x04; + break; + case 9600: + baud = 0x06; + break; + case 19200: + baud = 0x08; + break; + case 38400: + baud = 0x0a; + break; + case 57600: + baud = 0x0c; + break; + case 115200: + baud = 0x0e; + break; + default: + /* Unsupported speed */ + return false; + } + /*@ -charint @*/ + + /* Proceed to construct our message */ + port_selection = session->driver.navcom.physical_port | baud; + + /* Send it off */ + navcom_cmd_0x11(session, port_selection); + + /* And cheekily return true, even though we have + no way to know if the speed change succeeded + until and if we receive an ACK (message 0x06), + which will be at the new baud speed if the + command was successful. Bottom line, the client + should requery gpsd to see if the new speed is + different than the old one */ + return true; + } +#else + return false; +#endif /* ALLOW_RECONFIGURE */ +} + +/* Ionosphere and UTC Data */ +static gps_mask_t handle_0x83(struct gps_device_t *session) +{ + /* NOTE - At the present moment this is only being used + for determining the GPS-UTC time difference, + for which the iono data is not needed as far + as we are concerned. However, I am still + reporting it (if debuglevel >= LOG_IO) as a + matter of interest */ +/* 2^-30 */ +#define SF_A0 (0.000000000931322574615478515625) +/* 2^-50 */ +#define SF_A1 (0.000000000000000888178419700125) +/* 2^12 */ +#define SF_TOT (4096) +/* 2^-30 */ +#define SF_ALPHA0 (0.000000000931322574615478515625) +/* 2^-27 */ +#define SF_ALPHA1 (0.000000007450580596923828125) +/* 2^-24 */ +#define SF_ALPHA2 (0.000000059604644775390625) +/* 2^-24 */ +#define SF_ALPHA3 (0.000000059604644775390625) +/* 2^11 */ +#define SF_BETA0 (2048) +/* 2^14 */ +#define SF_BETA1 (16384) +/* 2^16 */ +#define SF_BETA2 (65536) +/* 2^16 */ +#define SF_BETA3 (65536) + unsigned char *buf = session->packet.outbuffer + 3; + u_int16_t week = getleuw(buf, 3); + u_int32_t tow = getleul(buf, 5); + int8_t alpha0 = getsb(buf, 9); + int8_t alpha1 = getsb(buf, 10); + int8_t alpha2 = getsb(buf, 11); + int8_t alpha3 = getsb(buf, 12); + int8_t beta0 = getsb(buf, 13); + int8_t beta1 = getsb(buf, 14); + int8_t beta2 = getsb(buf, 15); + int8_t beta3 = getsb(buf, 16); + int32_t a1 = getlesl(buf, 17); + int32_t a0 = getlesl(buf, 21); + u_int8_t tot = getub(buf, 25); + u_int8_t wnt = getub(buf, 26); + int8_t dtls = getsb(buf, 27); + u_int8_t wnlsf = getub(buf, 28); + u_int8_t dn = getub(buf, 29); + int8_t dtlsf = getsb(buf, 30); + + /*@ +charint +relaxtypes @*/ + /* Ref.: ICD-GPS-200C 20.3.3.5.2.4 */ + if ((week%256)*604800+tow/1000.0 < wnlsf*604800+dn*86400) { + /* Effectivity time is in the future, use dtls */ + session->context->leap_seconds = (int)dtls; + } else { + /* Effectivity time is not in the future, use dtlsf */ + session->context->leap_seconds = (int)dtlsf; + } + /*@ -relaxtypes -charint @*/ + + session->gpsdata.sentence_time = gpstime_to_unix((int)week, tow/1000.0) + - session->context->leap_seconds; + + gpsd_report(LOG_PROG, + "Navcom: received packet type 0x83 (Ionosphere and UTC Data)\n"); + gpsd_report(LOG_IO, + "Navcom: Scaled parameters follow:\n"); + gpsd_report(LOG_IO, + "Navcom: GPS Week: %u, GPS Time of Week: %u (GPS Time: %f)\n", + week, tow, week*604800+tow/1000.0); + gpsd_report(LOG_IO, + "Navcom: a0: %12.4E, a1: %12.4E, a2: %12.4E, a3: %12.4E, " + "b0: %12.4E, b1: %12.4E, b2: %12.4E, b3: %12.4E\n", + (double)alpha0*SF_ALPHA0, (double)alpha1*SF_ALPHA1, + (double)alpha2*SF_ALPHA2, (double)alpha3*SF_ALPHA3, + (double)beta0*SF_BETA0, (double)beta1*SF_BETA1, + (double)beta2*SF_BETA2, (double)beta3*SF_BETA3); + gpsd_report(LOG_IO, + "Navcom: A0: %19.12E, A1: %19.12E\n", (double)a0*SF_A0, (double)a1*SF_A1); + gpsd_report(LOG_IO, + "Navcom: UTC Ref. Time: %lu, UTC Ref. Week: %u, dTls: %d\n", + (unsigned long)tot*SF_TOT, wnt, dtls); + gpsd_report(LOG_IO, + "Navcom: Week of leap seconds: %u, Day number of leap seconds: %u, dTlsf: %d\n", + wnlsf, dn, dtlsf); + + return 0; /* No flag for update of leap seconds (Not part of a fix) */ + +#undef SF_A0 +#undef SF_A1 +#undef SF_TOT +#undef SF_ALPHA0 +#undef SF_ALPHA1 +#undef SF_ALPHA2 +#undef SF_ALPHA3 +#undef SF_BETA0 +#undef SF_BETA1 +#undef SF_BETA2 +#undef SF_BETA3 +} + +/* Acknowledgement (without error) */ +static gps_mask_t handle_0x06(struct gps_device_t *session) +{ + unsigned char *buf = session->packet.outbuffer + 3; + u_int8_t cmd_id = getub(buf, 3); + u_int8_t port = getub(buf, 4); + session->driver.navcom.physical_port = port; /* This tells us which serial port was used last */ + gpsd_report(LOG_PROG, + "Navcom: received packet type 0x06 (Acknowledgement (without error))\n"); + /*@ -type @*/ + gpsd_report(LOG_IO, + "Navcom: acknowledged command id 0x%02x on port %c\n", + cmd_id, (port==0?'A':(port==1?'B':'?'))); + /*@ +type @*/ + return 0; /* Nothing updated */ +} + +/* Negative Acknowledge */ +static gps_mask_t handle_0x15(struct gps_device_t *session) +{ + size_t n; + unsigned char *buf = session->packet.outbuffer + 3; + size_t msg_len = (size_t)getleuw(buf, 1); + /*@ -type @*/ + u_int8_t port, cmd_id = getub(buf, 3); + gpsd_report(LOG_PROG, + "Navcom: received packet type 0x15 (Negative Acknowledge)\n"); + for (n=4; n<(msg_len-2); n+=2) { + u_int8_t err_id = getub(buf, n); + u_int8_t err_desc = getub(buf, n+1); + gpsd_report(LOG_IO, + "Navcom: error id = 0x%02x, error description = 0x%02x\n", + err_id, err_desc); + } + port = getub(buf, n); + gpsd_report(LOG_IO, + "Navcom: negative acknowledge was for command id 0x%02x on port %c\n", + cmd_id, (port==0?'A':(port==1?'B':'?'))); + /*@ -type @*/ + return 0; /* Nothing updated */ +} + +/* PVT Block */ +static gps_mask_t handle_0xb1(struct gps_device_t *session) +{ + unsigned int n; + unsigned char *buf = session->packet.outbuffer + 3; + uint16_t week; + uint32_t tow; + uint32_t sats_used; + int32_t lat, lon; + /* Resolution of lat/lon values (2^-11) */ +#define LL_RES (0.00048828125) + uint8_t lat_fraction, lon_fraction; + /* Resolution of lat/lon fractions (2^-15) */ +#define LL_FRAC_RES (0.000030517578125) + uint8_t nav_mode; + int32_t ellips_height, altitude; + /* Resolution of height and altitude values (2.0^-10) */ +#define EL_RES (0.0009765625) + double vel_north, vel_east, vel_up; + /* Resolution of velocity values (2.0^-10) */ +#define VEL_RES (0.0009765625) + double track; + uint8_t fom, gdop, pdop, hdop, vdop, tdop, tfom; + /* This value means "undefined" */ +#define DOP_UNDEFINED (255) + + int16_t ant_height_adj; + int32_t set_delta_up; + /* Resolution of delta north, east, and up, + and ant. height adjustment values (1mm) */ +#define D_RES (0.001) + +#ifdef __UNUSED__ + /* Other values provided by the PVT block which we + may want to provide in the future. At the present + moment, the gpsd protocol does not have a mechanism + to make this available to the user */ + uint8_t dgps_conf; + uint16_t max_dgps_age; + uint8_t ext_nav_mode; + int32_t set_delta_north, set_delta_east; + uint8_t nav_failure_code; +#endif /* __UNUSED__ */ + + /* Timestamp */ + week = (uint16_t)getleuw(buf, 3); + tow = (uint32_t)getleul(buf, 5); + session->gpsdata.fix.time = session->gpsdata.sentence_time = gpstime_to_unix((int)week, tow/1000.0) - session->context->leap_seconds; + + /* Satellites used */ + sats_used = (uint32_t)getleul(buf, 9); + session->gpsdata.satellites_used = 0; + for (n = 0; n < 31; n++) { + if ((sats_used & (0x01 << n)) != 0) + session->gpsdata.used[session->gpsdata.satellites_used++] = (int)(n+1); + } + + /* Get latitude, longitude */ + lat = getlesl(buf, 13); + lon = getlesl(buf, 17); + lat_fraction = (uint8_t)(getub(buf, 21) >> 4); + lon_fraction = (uint8_t)(getub(buf, 21) & 0x0f); + + session->gpsdata.fix.latitude = (double)(lat * LL_RES + lat_fraction * LL_FRAC_RES ) / 3600; + session->gpsdata.fix.longitude = (double)(lon * LL_RES + lon_fraction * LL_FRAC_RES ) / 3600; + + /* Nav mode */ + nav_mode = (uint8_t)getub(buf, 22); + if (-nav_mode & 0x80) { + session->gpsdata.status = STATUS_NO_FIX; + session->gpsdata.fix.mode = MODE_NO_FIX; + } else { + session->gpsdata.fix.mode = (nav_mode & 0x40 ? MODE_3D : MODE_2D); + session->gpsdata.status = (nav_mode & 0x03 ? STATUS_DGPS_FIX : STATUS_FIX); + } + + /* Height Data */ + ellips_height = getlesl(buf, 23); + altitude = getlesl(buf, 27); + + ant_height_adj = getlesw(buf, 51); + set_delta_up = getlesl(buf, 79); + + session->gpsdata.fix.altitude = (double)(altitude * EL_RES) + + (ant_height_adj * D_RES) + (set_delta_up * D_RES); + session->gpsdata.separation = (double)(ellips_height - altitude)*EL_RES + + (ant_height_adj * D_RES) + (set_delta_up * D_RES); + + /* Speed Data */ + vel_north = (double)getlesl24(buf, 31); + vel_east = (double)getlesl24(buf, 34); + /* vel_up = getlesl24(buf, 37); */ + vel_up = (double)getlesl24(buf, 37); + + track = atan2(vel_east, vel_north); + if (track < 0) + track += 2 * GPS_PI; + session->gpsdata.fix.track = track * RAD_2_DEG; + /*@ -evalorder @*/ + session->gpsdata.fix.speed = sqrt(pow(vel_east,2) + pow(vel_north,2)) * VEL_RES; + /*@ +evalorder @*/ + session->gpsdata.fix.climb = vel_up * VEL_RES; + + /* Quality indicators */ + /*@ -type @*/ + fom = getub(buf, 40); + gdop = getub(buf, 41); + pdop = getub(buf, 42); + hdop = getub(buf, 43); + vdop = getub(buf, 44); + tdop = getub(buf, 45); + tfom = getub(buf, 46); + /*@ +type @*/ + + /* splint apparently gets confused about C promotion rules. */ + /* "Assignment of arbitrary unsigned integral type to double" on these */ +#ifndef S_SPLINT_S + session->gpsdata.fix.eph = fom/100.0*1.96/*Two sigma*/; + /* FIXME - Which units is tfom in (spec doesn't say) and + which units does gpsd require? (docs don't say) */ + session->gpsdata.fix.ept = tfom*1.96/*Two sigma*/; + /* FIXME This cannot possibly be right */ + /* I cannot find where to get VRMS from in the Navcom output, though, + and this value seems to agree with the output from other software */ + session->gpsdata.fix.epv = (double)fom/(double)hdop*(double)vdop/100.0*1.96/*Two sigma*/; + + if (gdop == DOP_UNDEFINED) + session->gpsdata.gdop = NAN; + else + session->gpsdata.gdop = gdop/10.0; + if (pdop == DOP_UNDEFINED) + session->gpsdata.pdop = NAN; + else + session->gpsdata.pdop = pdop/10.0; + if (hdop == DOP_UNDEFINED) + session->gpsdata.hdop = NAN; + else + session->gpsdata.hdop = hdop/10.0; + if (vdop == DOP_UNDEFINED) + session->gpsdata.vdop = NAN; + else + session->gpsdata.vdop = vdop/10.0; + if (tdop == DOP_UNDEFINED) + session->gpsdata.tdop = NAN; + else + session->gpsdata.tdop = tdop/10.0; +#endif /* S_SPLINT_S */ + + gpsd_report(LOG_PROG, "Navcom: received packet type 0xb1 (PVT Report)\n"); + gpsd_report(LOG_IO, "Navcom: navigation mode %s (0x%02x) - %s - %s\n", + (-nav_mode&0x80?"invalid":"valid"), nav_mode, + (nav_mode&0x40?"3D":"2D"), (nav_mode&0x03?"DGPS":"GPS")); + gpsd_report(LOG_IO, "Navcom: latitude = %f, longitude = %f, altitude = %f, geoid = %f\n", + session->gpsdata.fix.latitude, session->gpsdata.fix.longitude, + session->gpsdata.fix.altitude, session->gpsdata.separation); + gpsd_report(LOG_IO, + "Navcom: velocities: north = %f, east = %f, up = %f (track = %f, speed = %f)\n", + vel_north*VEL_RES, vel_east*VEL_RES, vel_up*VEL_RES, + session->gpsdata.fix.track, session->gpsdata.fix.speed); + gpsd_report(LOG_IO, + "Navcom: hrms = %f, vrms = %f, gdop = %f, pdop = %f, " + "hdop = %f, vdop = %f, tdop = %f\n", + session->gpsdata.fix.eph, session->gpsdata.fix.epv, + session->gpsdata.gdop, session->gpsdata.pdop, + session->gpsdata.hdop, session->gpsdata.vdop, + session->gpsdata.tdop); +#undef D_RES +#undef LL_RES +#undef LL_FRAC_RES +#undef EL_RES +#undef VEL_RES +#undef DOP_UNDEFINED + + return LATLON_SET | ALTITUDE_SET | CLIMB_SET | SPEED_SET | TRACK_SET + | TIME_SET | STATUS_SET | MODE_SET | USED_SET | HERR_SET | VERR_SET + | TIMERR_SET | DOP_SET | CYCLE_START_SET; +} + +/* Packed Ephemeris Data */ +static gps_mask_t handle_0x81(struct gps_device_t *session) +{ + /* Scale factors for everything */ + /* 2^-31 */ +#define SF_TGD (.000000000465661287307739257812) + /* 2^4 */ +#define SF_TOC (16) + /* 2^-55 */ +#define SF_AF2 (.000000000000000027755575615628) + /* 2^-43 */ +#define SF_AF1 (.000000000000113686837721616029) + /* 2^-31 */ +#define SF_AF0 (.000000000465661287307739257812) + /* 2^-5 */ +#define SF_CRS (.031250000000000000000000000000) + /* 2^-43 */ +#define SF_DELTA_N (.000000000000113686837721616029) + /* 2^-31 */ +#define SF_M0 (.000000000465661287307739257812) + /* 2^-29 */ +#define SF_CUC (.000000001862645149230957031250) + /* 2^-33 */ +#define SF_E (.000000000116415321826934814453) + /* 2^-29 */ +#define SF_CUS (.000000001862645149230957031250) + /* 2^-19 */ +#define SF_SQRT_A (.000001907348632812500000000000) + /* 2^4 */ +#define SF_TOE (16) + /* 2^-29 */ +#define SF_CIC (.000000001862645149230957031250) + /* 2^-31 */ +#define SF_OMEGA0 (.000000000465661287307739257812) + /* 2^-29 */ +#define SF_CIS (.000000001862645149230957031250) + /* 2^-31 */ +#define SF_I0 (.000000000465661287307739257812) + /* 2^-5 */ +#define SF_CRC (.031250000000000000000000000000) + /* 2^-31 */ +#define SF_OMEGA (.000000000465661287307739257812) + /* 2^-43 */ +#define SF_OMEGADOT (.000000000000113686837721616029) + /* 2^-43 */ +#define SF_IDOT (.000000000000113686837721616029) + + unsigned char *buf = session->packet.outbuffer + 3; + u_int8_t prn = getub(buf, 3); + u_int16_t week = getleuw(buf, 4); + u_int32_t tow = getleul(buf, 6); + u_int16_t iodc = getleuw(buf, 10); + /* And now the fun starts... everything that follows is + raw GPS data minus parity */ + /* Subframe 1, words 3 to 10 minus parity */ + u_int16_t wn = (getleuw_be(buf, 12)&0xffc0)>>6; + u_int8_t cl2 = (getub(buf, 13)&0x30)>>4; + u_int8_t ura = getub(buf, 13)&0x0f; + u_int8_t svh = (getub(buf, 14)&0xfc)>>2; + /* We already have IODC from earlier in the message, so + we do not decode again */ +/* u_int16_t iodc = (getub(buf, 14)&0x03)<<8;*/ + u_int8_t l2pd = (getub(buf, 15)&0x80)>>7; + int8_t tgd = getsb(buf, 26); +/* iodc |= getub(buf, 27);*/ + u_int16_t toc = getleuw_be(buf, 28); + int8_t af2 = getsb(buf, 30); + int16_t af1 = getlesw_be(buf, 31); + /*@ -shiftimplementation @*/ + int32_t af0 = getlesl24_be(buf, 33)>>2; + /*@ +shiftimplementation @*/ + /* Subframe 2, words 3 to 10 minus parity */ + u_int8_t iode = getub(buf, 36); + int16_t crs = getlesw_be(buf, 37); + int16_t delta_n = getlesw_be(buf, 39); + int32_t m0 = getlesl_be(buf, 41); + int16_t cuc = getlesw_be(buf, 45); + u_int32_t e = getleul_be(buf, 47); + int16_t cus = getlesw_be(buf, 51); + u_int32_t sqrt_a = getleul_be(buf, 53); + u_int16_t toe = getleuw_be(buf, 57); + /* NOTE - Fit interval & AODO not collected */ + /* Subframe 3, words 3 to 10 minus parity */ + int16_t cic = getlesw_be(buf, 60); + int32_t Omega0 = getlesl_be(buf, 62); + int16_t cis = getlesw_be(buf, 66); + int32_t i0 = getlesl_be(buf, 68); + int16_t crc = getlesw_be(buf, 72); + int32_t omega = getlesl_be(buf, 74); + int32_t Omegadot = getlesl24_be(buf, 78); + /*@ -predboolothers @*/ + /* Question: What is the proper way of shifting a signed int 2 bits to + * the right, preserving sign? Answer: integer division by 4. */ + int16_t idot = (int16_t)(((getlesw_be(buf, 82)&0xfffc)/4)|(getub(buf, 82)&80?0xc000:0x0000)); + /*@ +predboolothers @*/ + char time_str[24]; + (void)unix_to_iso8601(gpstime_to_unix((int)wn, (double)(toc*SF_TOC)), time_str, sizeof(time_str)); + + gpsd_report(LOG_PROG, + "Navcom: received packet type 0x81 (Packed Ephemeris Data)\n"); + gpsd_report(LOG_IO, + "Navcom: PRN: %u, Epoch: %u (%s), SV clock bias/drift/drift rate: %#19.12E/%#19.12E/%#19.12E\n", + prn, toc*SF_TOC, time_str, ((double)af0)*SF_AF0, ((double)af1)*SF_AF1, ((double)af2)*SF_AF2); + gpsd_report(LOG_IO, + "Navcom: IODE (!AODE): %u Crs: %19.12e, Delta n: %19.12e, M0: %19.12e\n", + iode, (double)crs*SF_CRS, (double)delta_n*SF_DELTA_N*GPS_PI, (double)m0*SF_M0*GPS_PI); + gpsd_report(LOG_IO, + "Navcom: Cuc: %19.12e, Eccentricity: %19.12e, Cus: %19.12e, A^1/2: %19.12e\n", + (double)cuc*SF_CUC, (double)e*SF_E, (double)cus*SF_CUS, (double)sqrt_a*SF_SQRT_A); + gpsd_report(LOG_IO, + "Navcom: TOE: %u, Cic: %19.12e, Omega %19.12e, Cis: %19.12e\n", + toe*SF_TOE, (double)cic*SF_CIC, (double)Omega0*SF_OMEGA0*GPS_PI, + (double)cis*SF_CIS); + gpsd_report(LOG_IO, + "Navcom: i0: %19.12e, Crc: %19.12e, omega: %19.12e, Omega dot: %19.12e\n", + (double)i0*SF_I0*GPS_PI, (double)crc*SF_CRC, (double)omega*SF_OMEGA*GPS_PI, + (double)Omegadot*SF_OMEGADOT*GPS_PI); + gpsd_report(LOG_IO, + "Navcom: IDOT: %19.12e, Codes on L2: 0x%x, GPS Week: %u, L2 P data flag: %x\n", + (double)idot*SF_IDOT*GPS_PI, cl2, week-(week%1024)+wn, l2pd); + gpsd_report(LOG_IO, + "Navcom: SV accuracy: 0x%x, SV health: 0x%x, TGD: %f, IODC (!AODC): %u\n", + ura, svh, (double)tgd*SF_TGD, iodc); + gpsd_report(LOG_IO, + "Navcom: Transmission time: %u\n", + tow); + +#undef SF_TGD +#undef SF_TOC +#undef SF_AF2 +#undef SF_AF1 +#undef SF_AF0 +#undef SF_CRS +#undef SF_DELTA_N +#undef SF_M0 +#undef SF_CUC +#undef SF_E +#undef SF_CUS +#undef SF_SQRT_A +#undef SF_TOE +#undef SF_CIC +#undef SF_OMEGA0 +#undef SF_CIS +#undef SF_I0 +#undef SF_CRC +#undef SF_OMEGA +#undef SF_OMEGADOT +#undef SF_IDOT + + return 0; +} + +/* Channel Status */ +static gps_mask_t handle_0x86(struct gps_device_t *session) +{ + size_t n, i; + u_int8_t prn, tracking_status, ele, ca_snr, p2_snr, log_channel, hw_channel, s; + u_int16_t azm, dgps_age; + unsigned char *buf = session->packet.outbuffer + 3; + size_t msg_len = (size_t)getleuw(buf, 1); + u_int16_t week = getleuw(buf, 3); + u_int32_t tow = getleul(buf, 5); + u_int8_t eng_status = getub(buf, 9); + u_int16_t sol_status = getleuw(buf, 10); + u_int8_t sats_visible = getub(buf, 12); + u_int8_t sats_tracked = getub(buf, 13); + u_int8_t sats_used = getub(buf, 14); + u_int8_t pdop = getub(buf, 15); + + /* Timestamp and PDOP */ + session->gpsdata.sentence_time = gpstime_to_unix((int)week, tow/1000.0) + - session->context->leap_seconds; + session->gpsdata.pdop = (int)pdop / 10.0; + + /* Satellite count */ + session->gpsdata.satellites = (int)sats_visible; + session->gpsdata.satellites_used = (int)sats_used; + + /* Fix mode */ + switch(sol_status & 0x05) + { + case 0x05: + session->gpsdata.status = STATUS_DGPS_FIX; + break; + case 0x01: + session->gpsdata.status = STATUS_FIX; + break; + default: + session->gpsdata.status = STATUS_NO_FIX; + } + + /*@ -predboolothers @*/ + gpsd_report(LOG_PROG, + "Navcom: received packet type 0x86 (Channel Status) " + "- satellites: visible = %u, tracked = %u, used = %u\n", + sats_visible, sats_tracked, sats_used); + gpsd_report(LOG_IO, + "Navcom: engine status = 0x%x, almanac = %s, time = 0x%x, pos = 0x%x\n", + eng_status&0x07, (eng_status&0x08?"valid":"invalid"), + eng_status&0x30>>4, eng_status&0xc0>>6); + /*@ +predboolothers @*/ + + /* Satellite details */ + i = 0; + for(n = 17; n < msg_len; n += 14) { + if(i >= MAXCHANNELS) { + gpsd_report(LOG_ERROR, + "Navcom: packet type 0x86: too many satellites!\n"); + gpsd_zero_satellites(&session->gpsdata); + return ERROR_SET; + } + prn = getub(buf, n); + tracking_status = getub(buf, n+1); + log_channel = getub(buf, n+2); + ele = getub(buf, n+5); + azm = getleuw(buf, n+6); + ca_snr = getub(buf, n+8); + p2_snr = getub(buf, n+10); + dgps_age = getleuw(buf, n+11); + hw_channel = getub(buf, n+13); + s = (unsigned char)0; + /*@ -predboolothers +charint @*/ + /* NOTE - In theory, I think one would check for hw channel number to + see if one is dealing with a GPS or other satellite, but the + channel numbers reported bear no resemblance to what the spec + says should be. So I check for the fact that if all three + values below are zero, one is not interested on this satellite */ + if (!(ele == 0 && azm == 0 && dgps_age == 0)) { + session->gpsdata.PRN[i] = (int)prn; + session->gpsdata.elevation[i] = (int)ele; + session->gpsdata.azimuth[i] = (int)azm; + /*@i1@*/s = session->gpsdata.ss[i++] = (p2_snr ? p2_snr : ca_snr) / 4; + } + gpsd_report(LOG_IO, + "Navcom: prn = %3u, ele = %02u, azm = %03u, snr = %d (%s), " + "dgps age = %.1fs, log ch = %d, hw ch = 0x%02x\n", + prn, ele, azm, s, (p2_snr?"P2":"C/A"), + (double)dgps_age*0.1, log_channel&0x3f, hw_channel); + gpsd_report(LOG_IO, + "Navcom: sol. valid = %c, clock = %s, pos. = %s, " + "height = %s, err. code = 0x%x\n", + (sol_status&0x01?'Y':'N'), (sol_status&0x02?"stable":"unstable"), + (sol_status&0x04?"dgps":"unaided"), (sol_status&0x08?"solved":"constrained"), + (sol_status&0x01?0x00:sol_status&0x0f00>>8)); + /*@ +predboolothers -charint @*/ + } + + return PDOP_SET | SATELLITE_SET | STATUS_SET; +} + +/* Raw Meas. Data Block */ +static gps_mask_t handle_0xb0(struct gps_device_t *session) +{ + /* L1 wavelength (299792458m/s / 1575420000Hz) */ +#define LAMBDA_L1 (.190293672798364880476317426464) + size_t n; + unsigned char *buf = session->packet.outbuffer + 3; + size_t msg_len = (size_t)getleuw(buf, 1); + u_int16_t week = getleuw(buf, 3); + u_int32_t tow = getleul(buf, 5); + u_int8_t tm_slew_acc = getub(buf, 9); + u_int8_t status = getub(buf, 10); + + char time_str[24]; + (void)unix_to_iso8601(gpstime_to_unix((int)week, (double)tow/1000.0), time_str, sizeof(time_str)); + + gpsd_report(LOG_PROG, + "Navcom: received packet type 0xb0 (Raw Meas. Data Block)\n"); + /*@ -predboolothers @*/ + gpsd_report(LOG_IO, + "Navcom: Epoch = %s, time slew accumulator = %u (1/1023mS), status = 0x%02x " + "(%sclock %s - %u blocks follow)\n", + time_str, + tm_slew_acc, status, (status&0x80?"channel time set - ":""), + (status&0x40?"stable":"not stable"), status&0x0f); + /*@ +predboolothers @*/ + for (n=11; n> sv status = 0x%02x (PRN %u - C/A & L1 %s - P1 %s - P2 & L2 %s)\n", + sv_status, (sv_status&0x1f), (sv_status&0x80?"valid":"invalid"), + (sv_status&0x40?"valid":"invalid"), (sv_status&0x20?"valid":"invalid")); + gpsd_report(LOG_IO+1, + "Navcom: >>> ch status = 0x%02x (Logical channel: %u - CA C/No: %u dBHz) " + "sL1: %u, sL2: %u\n", + ch_status, ch_status&0x0f, ((ch_status&0xf0)>>4)+35, l1_slips, l2_slips); + gpsd_report(LOG_IO+1, + "Navcom: >>> C1: %14.3f, L1: %14.3f, L2: %14.3f, P1: %14.3f, P2: %14.3f\n", + c1, l1, l2, p1, p2); + /*@ +predboolothers -charint @*/ + } +#undef LAMBDA_L1 + return 0; /* Raw measurements not yet implemented in gpsd */ +} + +/* Pseudorange Noise Statistics */ +static gps_mask_t handle_0xb5(struct gps_device_t *session) +{ + if(sizeof(double) == 8) { + union long_double l_d; + unsigned char *buf = session->packet.outbuffer + 3; + u_int16_t week = getleuw(buf, 3); + u_int32_t tow = getleul(buf, 5); + double rms = getled(buf, 9); +#ifdef __UNUSED__ + /* Reason why it's unused is these figures do not agree + with those obtained from the PVT report (handle_0xb1). + The figures from 0xb1 do agree with the values reported + by Navcom's PC utility */ + double ellips_maj = getled(buf, 17); + double ellips_min = getled(buf, 25); + double ellips_azm = getled(buf, 33); + double lat_sd = getled(buf, 41); + double lon_sd = getled(buf, 49); + double alt_sd = getled(buf, 57); + double hrms = sqrt(pow(lat_sd, 2) + pow(lon_sd, 2)); +#endif /* __UNUSED__ */ + session->gpsdata.epe = rms*1.96; +#ifdef __UNUSED__ + session->gpsdata.fix.eph = hrms*1.96; + session->gpsdata.fix.epv = alt_sd*1.96; +#endif /* __UNUSED__ */ + session->gpsdata.sentence_time = gpstime_to_unix((int)week, tow/1000.0) + - session->context->leap_seconds; + gpsd_report(LOG_PROG, + "Navcom: received packet type 0xb5 (Pseudorange Noise Statistics)\n"); + gpsd_report(LOG_IO, + "Navcom: epe = %f\n", session->gpsdata.epe); + return TIME_SET | PERR_SET; + } else { + /* Ignore this message block */ + if (!session->driver.navcom.warned) { + gpsd_report(LOG_WARN, + "Navcom: received packet type 0xb5 (Pseudorange Noise Statistics) ignored " + " - sizeof(double) == 64 bits required\n"); + session->driver.navcom.warned = true; + } + return 0; /* Block ignored - wrong sizeof(double) */ + } +} + +/* LBM DSP Status Block */ +static gps_mask_t handle_0xd3(struct gps_device_t *session UNUSED) +{ + /* This block contains status information about the + unit's L-band (Inmarsat) module. There is nothing + interesting in it for our purposes so we do not deal + with it. This callback is purely to a) stop + "unrecognised packet" messages appearing in the log + and b) explain what it is for the curious */ + return 0; /* Nothing updated */ +} + +/* Identification Block */ +static gps_mask_t handle_0xae(struct gps_device_t *session) +{ + char *engconfstr, *asicstr; + unsigned char *buf = session->packet.outbuffer + 3; + size_t msg_len = (size_t)getleuw(buf, 1); + u_int8_t engconf = getub(buf, 3); + u_int8_t asic = getub(buf, 4); + u_int8_t swvermaj = getub(buf, 5); + u_int8_t swvermin = getub(buf, 6); + u_int16_t dcser = getleuw(buf, 7); + u_int8_t dcclass = getub(buf, 9); + u_int16_t rfcser = getleuw(buf, 10); + u_int8_t rfcclass= getub(buf, 12); + /*@ -stringliteralnoroomfinalnull -type @*/ + u_int8_t softtm[17] = "\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0"; + u_int8_t bootstr[17] = "\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0"; + u_int8_t ioptm[17] = "\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0"; + /*@ +stringliteralnoroomfinalnull +type @*/ + u_int8_t iopvermaj = (u_int8_t)0x00; + u_int8_t iopvermin = (u_int8_t)0x00; + u_int8_t picver = (u_int8_t)0x00; + u_int8_t slsbn = (u_int8_t)0x00; + u_int8_t iopsbn = (u_int8_t)0x00; + memcpy(softtm, &buf[13], 16); + memcpy(bootstr, &buf[29], 16); + if (msg_len == 0x0037) { /* No IOP */ + slsbn = getub(buf, 53); + } else { /* IOP Present */ + iopvermaj = getub(buf, 53); + iopvermin = getub(buf, 54); + memcpy(ioptm, &buf[55], 16); + picver = getub(buf, 71); + slsbn = getub(buf, 72); + iopsbn = getub(buf, 73); + } + + switch(engconf) + { + case 0x00: + engconfstr = "Unknown/Undefined"; + break; + case 0x01: + engconfstr = "NCT 2000 S"; + break; + case 0x02: + engconfstr = "NCT 2000 D"; + break; + case 0x03: + engconfstr = "Startfire Single"; + break; + case 0x04: + engconfstr = "Starfire Dual"; + break; + case 0x05: + engconfstr = "Pole Mount RTK (Internal Radio)"; + break; + case 0x06: + engconfstr = "Pole Mount GIS (LBM)"; + break; + case 0x07: + engconfstr = "Black Box RTK (Internal Radio)"; + break; + case 0x08: + engconfstr = "Black Box GIS (LBM)"; + break; + case 0x80: + engconfstr = "R100"; + break; + case 0x81: + engconfstr = "R200"; + break; + case 0x82: + engconfstr = "R210"; + break; + case 0x83: + engconfstr = "R300"; + break; + case 0x84: + engconfstr = "R310"; + break; + default: + engconfstr = "?"; + } + + switch(asic) + { + case 0x01: + asicstr = "A-ASIC"; + break; + case 0x02: + asicstr = "B-ASIC"; + break; + case 0x03: + asicstr = "C-ASIC"; + break; + case 0x04: + asicstr = "M-ASIC"; + break; + default: + asicstr = "?"; + } + + gpsd_report(LOG_PROG, "Navcom: received packet type 0xae (Identification Block)\n"); + if(msg_len == 0x0037) { + gpsd_report(LOG_INF, "Navcom: ID Data: " + "%s %s Ver. %u.%u.%u, DC S/N: %u.%u, RF S/N: %u.%u, " + "Build ID: %s, Boot software: %s\n", + engconfstr, asicstr, swvermaj, swvermin, slsbn, dcser, dcclass, + rfcser, rfcclass, softtm, bootstr); + } else { + gpsd_report(LOG_INF, "Navcom: ID Data: " + "%s %s Ver. %u.%u.%u, DC S/N: %u.%u, RF S/N: %u.%u, " + "Build ID: %s, Boot software: %s, " + "IOP Ver.: %u.%u.%u, PIC: %u, IOP Build ID: %s\n", + engconfstr, asicstr, swvermaj, swvermin, slsbn, dcser, dcclass, + rfcser, rfcclass, softtm, bootstr, iopvermaj, iopvermin, iopsbn, + picver, ioptm); + } + + /*@ -formattype @*/ + (void)snprintf(session->subtype, sizeof(session->subtype), + "%s %s Ver. %u.%u.%u S/N %u.%u %u.%u", + engconfstr, asicstr, swvermaj, swvermin, slsbn, dcser, dcclass, + rfcser, rfcclass); + /*@ +formattype @*/ + return DEVICEID_SET; +} + +/* Clock Drift and Offset */ +static gps_mask_t handle_0xef(struct gps_device_t *session) +{ + unsigned char *buf = session->packet.outbuffer + 3; + u_int16_t week = getleuw(buf, 3); + u_int32_t tow = getleul(buf, 5); + int8_t osc_temp = getsb(buf, 9); + u_int8_t nav_status = getub(buf, 10); + union long_double l_d; + double nav_clock_offset; + union int_float i_f; + float nav_clock_drift; + float osc_filter_drift_est; + int32_t time_slew = (int32_t)getlesl(buf, 27); + if (sizeof(double) == 8) { + nav_clock_offset = getled(buf, 11); + } else { + nav_clock_offset = NAN; + } + if (sizeof(float) == 4) { + nav_clock_drift = getlef(buf, 19); + osc_filter_drift_est = getlef(buf, 23); + } else { + nav_clock_drift = NAN; + osc_filter_drift_est = NAN; + } + + session->gpsdata.sentence_time = gpstime_to_unix((int)week, tow/1000.0) + - session->context->leap_seconds; + + gpsd_report(LOG_PROG, + "Navcom: received packet type 0xef (Clock Drift and Offset)\n"); + gpsd_report(LOG_IO, + "Navcom: oscillator temp. = %d, nav. status = 0x%02x, " + "nav. clock offset = %f, nav. clock drift = %f, " + "osc. filter drift est. = %f, acc.time slew value = %d\n", + osc_temp, nav_status, nav_clock_offset, nav_clock_drift, + osc_filter_drift_est, time_slew); + return TIME_SET; +} + + +/*@ +charint @*/ +gps_mask_t navcom_parse(struct gps_device_t *session, unsigned char *buf, size_t len) +{ + unsigned char cmd_id; + unsigned char *payload; + unsigned int msg_len; + + if (len == 0) + return 0; + + cmd_id = getub(buf, 3); + payload = &buf[6]; + msg_len = getleuw(buf, 4); + + /*@ -usedef -compdef @*/ + gpsd_report(LOG_RAW, "Navcom: packet type 0x%02x, length %d: %s\n", + cmd_id, msg_len, gpsd_hexdump_wrapper(buf, len, LOG_RAW)); + /*@ +usedef +compdef @*/ + + (void)snprintf(session->gpsdata.tag, sizeof(session->gpsdata.tag), + "0x%02x",cmd_id); + + switch (cmd_id) + { + case 0x06: + return handle_0x06(session); + case 0x15: + return handle_0x15(session); + case 0x81: + return handle_0x81(session); + case 0x83: + return handle_0x83(session); + case 0x86: + return handle_0x86(session); + case 0xae: + return handle_0xae(session); + case 0xb0: + return handle_0xb0(session); + case 0xb1: + return handle_0xb1(session); + case 0xb5: + return handle_0xb5(session); + case 0xd3: + return handle_0xd3(session); + case 0xef: + return handle_0xef(session); + default: + gpsd_report(LOG_PROG, + "Navcom: received packet type 0x%02x, length %d - unknown or unimplemented\n", + cmd_id, msg_len); + return 0; + } +} +/*@ -charint @*/ + +static gps_mask_t navcom_parse_input(struct gps_device_t *session) +{ + gps_mask_t st; + + if (session->packet.type == NAVCOM_PACKET){ + st = navcom_parse(session, session->packet.outbuffer, session->packet.outbuflen); + session->gpsdata.driver_mode = MODE_BINARY; /* binary */ + return st; +#ifdef NMEA_ENABLE + } else if (session->packet.type == NMEA_PACKET) { + st = nmea_parse((char *)session->packet.outbuffer, session); + session->gpsdata.driver_mode = MODE_NMEA; /* NMEA */ + return st; +#endif /* NMEA_ENABLE */ + } else + return 0; +} + + +/* this is everything we export */ +const struct gps_type_t navcom_binary = +{ + .type_name = "Navcom binary", /* full name of type */ + .packet_type = NAVCOM_PACKET, /* lexer packet type */ + .trigger = "\x02\x99\x66", /* packet leader */ + .channels = NAVCOM_CHANNELS, /* 12 L1 + 12 L2 + 2 Inmarsat L-Band */ + .control_send = navcom_control_send, /* no control sender yet */ + .probe_wakeup = navcom_ping, /* wakeup to be done before hunt */ + .probe_detect = NULL, /* no probe */ + .probe_subtype = navcom_probe_subtype, /* subtype probing */ +#ifdef ALLOW_RECONFIGURE + .configurator = NULL, /* no reconfigure */ +#endif /* ALLOW_RECONFIGURE */ + .get_packet = generic_get, /* use generic one */ + .parse_packet = navcom_parse_input, /* parse message packets */ + .rtcm_writer = pass_rtcm, /* send RTCM data straight */ + .speed_switcher = navcom_speed, /* we do change baud rates */ + .mode_switcher = NULL, /* there is not a mode switcher */ + .rate_switcher = NULL, /* no sample-rate switcher */ + .cycle_chars = -1, /* ignore, no rate switch */ +#ifdef ALLOW_RECONFIGURE + .revert = NULL, /* no reversion code */ +#endif /* ALLOW_RECONFIGURE */ + .wrapup = NULL, /* ignore, no wrapup */ + .cycle = 1, /* updates every second */ +}; + +#endif /* defined(NAVCOM_ENABLE) && defined(BINARY_ENABLE) */ diff --git a/driver_nmea.c b/driver_nmea.c new file mode 100644 index 00000000..a8a2b556 --- /dev/null +++ b/driver_nmea.c @@ -0,0 +1,936 @@ +/* $Id$ */ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "gpsd_config.h" +#include "gpsd.h" +#include "timebase.h" + +#ifdef MKT3301_ENABLE +extern gps_mask_t processMKT3301(int c UNUSED, char *field[], struct gps_device_t *session); +#endif /* MKT3301_ENABLE */ + +#ifdef NMEA_ENABLE +/************************************************************************** + * + * Parser helpers begin here + * + **************************************************************************/ + +static void do_lat_lon(char *field[], struct gps_data_t *out) +/* process a pair of latitude/longitude fields starting at field index BEGIN */ +{ + double lat, lon, d, m; + char str[20], *p; + int updated = 0; + + if (*(p = field[0]) != '\0') { + strncpy(str, p, 20); + (void)sscanf(p, "%lf", &lat); + m = 100.0 * modf(lat / 100.0, &d); + lat = d + m / 60.0; + p = field[1]; + if (*p == 'S') + lat = -lat; + if (out->fix.latitude != lat) + out->fix.latitude = lat; + updated++; + } + if (*(p = field[2]) != '\0') { + strncpy(str, p, 20); + (void)sscanf(p, "%lf", &lon); + m = 100.0 * modf(lon / 100.0, &d); + lon = d + m / 60.0; + + p = field[3]; + if (*p == 'W') + lon = -lon; + if (out->fix.longitude != lon) + out->fix.longitude = lon; + updated++; + } +} + +/************************************************************************** + * + * Scary timestamp fudging begins here + * + * Four sentences, GGA and GLL and RMC and ZDA, contain timestamps. + * GGA/GLL/RMC timestamps look like hhmmss.ss, with the trailing .ss part + * optional. RMC has a date field, in the format ddmmyy. ZDA has + * separate fields for day/month/year, with a 4-digit year. This + * means that for RMC we must supply a century and for GGA and GLL we + * must supply a century, year, and day. We get the missing data from + * a previous RMC or ZDA; century in RMC is supplied by a constant if + * there has been no previous ZDA. + * + **************************************************************************/ + +#define DD(s) ((int)((s)[0]-'0')*10+(int)((s)[1]-'0')) + +static void merge_ddmmyy(char *ddmmyy, struct gps_device_t *session) +/* sentence supplied ddmmyy, but no century part */ +{ + if (session->driver.nmea.date.tm_year == 0) + session->driver.nmea.date.tm_year = (CENTURY_BASE + DD(ddmmyy+4)) - 1900; + session->driver.nmea.date.tm_mon = DD(ddmmyy+2)-1; + session->driver.nmea.date.tm_mday = DD(ddmmyy); +} + +static void merge_hhmmss(char *hhmmss, struct gps_device_t *session) +/* update from a UTC time */ +{ + int old_hour = session->driver.nmea.date.tm_hour; + + session->driver.nmea.date.tm_hour = DD(hhmmss); + if (session->driver.nmea.date.tm_hour < old_hour) /* midnight wrap */ + session->driver.nmea.date.tm_mday++; + session->driver.nmea.date.tm_min = DD(hhmmss+2); + session->driver.nmea.date.tm_sec = DD(hhmmss+4); + session->driver.nmea.subseconds = atof(hhmmss+4) - session->driver.nmea.date.tm_sec; +} + +#undef DD + +/************************************************************************** + * + * Compare GPS timestamps for equality. Depends on the fact that the + * timestamp granularity of GPS is 1/100th of a second. Use this to avoid + * naive float comparisons. + * + **************************************************************************/ + +#define GPS_TIME_EQUAL(a, b) (fabs((a) - (b)) < 0.01) + +/************************************************************************** + * + * NMEA sentence handling begins here + * + **************************************************************************/ + +static gps_mask_t processGPRMC(int count, char *field[], struct gps_device_t *session) +/* Recommend Minimum Course Specific GPS/TRANSIT Data */ +{ + /* + RMC,225446.33,A,4916.45,N,12311.12,W,000.5,054.7,191194,020.3,E,A*68 + 1 225446.33 Time of fix 22:54:46 UTC + 2 A Status of Fix: A = Autonomous, valid; + D = Differential, valid; V = invalid + 3,4 4916.45,N Latitude 49 deg. 16.45 min North + 5,6 12311.12,W Longitude 123 deg. 11.12 min West + 7 000.5 Speed over ground, Knots + 8 054.7 Course Made Good, True north + 9 181194 Date of fix 18 November 1994 + 10,11 020.3,E Magnetic variation 20.3 deg East + 12 A FAA mode indicator (NMEA 2.3 and later) + A=autonomous, D=differential, E=Estimated, + N=not valid, S=Simulator, M=Manual input mode + *68 mandatory nmea_checksum + + * SiRF chipsets don't return either Mode Indicator or magnetic variation. + */ + gps_mask_t mask = 0; + + if (strcmp(field[2], "V")==0) { + /* copes with Magellan EC-10X, see below */ + if (session->gpsdata.status != STATUS_NO_FIX) { + session->gpsdata.status = STATUS_NO_FIX; + mask |= STATUS_SET; + } + if (session->gpsdata.fix.mode >= MODE_2D) { + session->gpsdata.fix.mode = MODE_NO_FIX; + mask |= MODE_SET; + } + /* set something nz, so it won't look like an unknown sentence */ + mask |= ONLINE_SET; + } else if (strcmp(field[2], "A")==0) { + if (count > 9) { + merge_hhmmss(field[1], session); + merge_ddmmyy(field[9], session); + mask |= TIME_SET; + session->gpsdata.fix.time = (double)mkgmtime(&session->driver.nmea.date)+session->driver.nmea.subseconds; + if (!GPS_TIME_EQUAL(session->gpsdata.sentence_time, session->gpsdata.fix.time)) { + mask |= CYCLE_START_SET; + gpsd_report(LOG_PROG, "GPRMC starts a reporting cycle.\n"); + } + session->gpsdata.sentence_time = session->gpsdata.fix.time; + } + do_lat_lon(&field[3], &session->gpsdata); + mask |= LATLON_SET; + session->gpsdata.fix.speed = atof(field[7]) * KNOTS_TO_MPS; + session->gpsdata.fix.track = atof(field[8]); + mask |= (TRACK_SET | SPEED_SET); + /* + * This copes with GPSes like the Magellan EC-10X that *only* emit + * GPRMC. In this case we set mode and status here so the client + * code that relies on them won't mistakenly believe it has never + * received a fix. + */ + if (session->gpsdata.status == STATUS_NO_FIX) { + session->gpsdata.status = STATUS_FIX; /* could be DGPS_FIX, we can't tell */ + mask |= STATUS_SET; + } + if (session->gpsdata.fix.mode < MODE_2D) { + session->gpsdata.fix.mode = MODE_2D; + mask |= MODE_SET; + } + } + + gpsd_report(LOG_PROG, "GPRMC sets mode %d\n", session->gpsdata.fix.mode); + return mask; +} + +static gps_mask_t processGPGLL(int count, char *field[], struct gps_device_t *session) +/* Geographic position - Latitude, Longitude */ +{ + /* Introduced in NMEA 3.0. + + $GPGLL,4916.45,N,12311.12,W,225444,A,A*5C + + 1,2: 4916.46,N Latitude 49 deg. 16.45 min. North + 3,4: 12311.12,W Longitude 123 deg. 11.12 min. West + 5: 225444 Fix taken at 22:54:44 UTC + 6: A Data valid + 7: A Autonomous mode + 8: *5C Mandatory NMEA checksum + + 1,2 Latitude, N (North) or S (South) + 3,4 Longitude, E (East) or W (West) + 5 UTC of position + 6 A=Active, V=Void + 7 Mode Indicator + A = Autonomous mode + D = Differential Mode + E = Estimated (dead-reckoning) mode + M = Manual Input Mode + S = Simulated Mode + N = Data Not Valid + + I found a note at + indicating that the Garmin 65 does not return time and status. + SiRF chipsets don't return the Mode Indicator. + This code copes gracefully with both quirks. + + Unless you care about the FAA indicator, this sentence supplies nothing + that GPRMC doesn't already. But at least one Garmin GPS -- the 48 + actually ships updates in GPLL that aren't redundant. + */ + char *status = field[7]; + gps_mask_t mask = ERROR_SET; + + if (strcmp(field[6], "A")==0 && (count < 8 || *status != 'N')) { + int newstatus = session->gpsdata.status; + + mask = 0; + merge_hhmmss(field[5], session); + if (session->driver.nmea.date.tm_year == 0) + gpsd_report(LOG_WARN, "can't use GGL time until after ZDA or RMC has supplied a year.\n"); + else { + mask = TIME_SET; + session->gpsdata.fix.time = (double)mkgmtime(&session->driver.nmea.date)+session->driver.nmea.subseconds; + if (!GPS_TIME_EQUAL(session->gpsdata.sentence_time, session->gpsdata.fix.time)) { + mask |= CYCLE_START_SET; + gpsd_report(LOG_PROG, "GPGLL starts a reporting cycle.\n"); + } + session->gpsdata.sentence_time = session->gpsdata.fix.time; + } + do_lat_lon(&field[1], &session->gpsdata); + mask |= LATLON_SET; + if (count >= 8 && *status == 'D') + newstatus = STATUS_DGPS_FIX; /* differential */ + else + newstatus = STATUS_FIX; + /* + * This is a bit dodgy. Technically we shouldn't set the mode + * bit until we see GSA. But it may be later in the cycle, + * some devices like the FV-18 don't send it by default, and + * elsewhere in the code we want to be able to test for the + * presence of a valid fix with mode > MODE_NO_FIX. + */ + if (session->gpsdata.fix.mode < MODE_2D) { + session->gpsdata.fix.mode = MODE_2D; + mask |= MODE_SET; + } + session->gpsdata.status = newstatus; + mask |= STATUS_SET; + gpsd_report(LOG_PROG, "GPGLL sets status %d\n", session->gpsdata.status); + } + + return mask; +} + +static gps_mask_t processGPGGA(int c UNUSED, char *field[], struct gps_device_t *session) +/* Global Positioning System Fix Data */ +{ + /* + GGA,123519,4807.038,N,01131.324,E,1,08,0.9,545.4,M,46.9,M, , *42 + 1 123519 Fix taken at 12:35:19 UTC + 2,3 4807.038,N Latitude 48 deg 07.038' N + 4,5 01131.324,E Longitude 11 deg 31.324' E + 6 1 Fix quality: 0 = invalid, 1 = GPS, 2 = DGPS, + 3=PPS (Precise Position Service), + 4=RTK (Real Time Kinematic) with fixed integers, + 5=Float RTK, 6=Estimated, 7=Manual, 8=Simulator + 7 08 Number of satellites being tracked + 8 0.9 Horizontal dilution of position + 9,10 545.4,M Altitude, Metres above mean sea level + 11,12 46.9,M Height of geoid (mean sea level) above WGS84 + ellipsoid, in Meters + (empty field) time in seconds since last DGPS update + (empty field) DGPS station ID number (0000-1023) + */ + gps_mask_t mask; + + session->gpsdata.status = atoi(field[6]); + mask = STATUS_SET; + if (session->gpsdata.status > STATUS_NO_FIX) { + char *altitude; + double oldfixtime = session->gpsdata.fix.time; + + merge_hhmmss(field[1], session); + if (session->driver.nmea.date.tm_year == 0) + gpsd_report(LOG_WARN, "can't use GGA time until after ZDA or RMC has supplied a year.\n"); + else { + mask |= TIME_SET; + session->gpsdata.fix.time = (double)mkgmtime(&session->driver.nmea.date)+session->driver.nmea.subseconds; + if (!GPS_TIME_EQUAL(session->gpsdata.sentence_time, session->gpsdata.fix.time)) { + mask |= CYCLE_START_SET; + gpsd_report(LOG_PROG, "GPGGA starts a reporting cycle.\n"); + } + session->gpsdata.sentence_time = session->gpsdata.fix.time; + } + do_lat_lon(&field[2], &session->gpsdata); + mask |= LATLON_SET; + session->gpsdata.satellites_used = atoi(field[7]); + altitude = field[9]; + /* + * SiRF chipsets up to version 2.2 report a null altitude field. + * See . + * If we see this, force mode to 2D at most. + */ + if (altitude[0] == '\0') { + if (session->gpsdata.fix.mode == MODE_3D) { + session->gpsdata.fix.mode = session->gpsdata.status ? MODE_2D : MODE_NO_FIX; + mask |= MODE_SET; + } + } else { + double oldaltitude = session->gpsdata.fix.altitude; + + session->gpsdata.fix.altitude = atof(altitude); + mask |= ALTITUDE_SET; + /* + * This is a bit dodgy. Technically we shouldn't set the mode + * bit until we see GSA. But it may be later in the cycle, + * some devices like the FV-18 don't send it by default, and + * elsewhere in the code we want to be able to test for the + * presence of a valid fix with mode > MODE_NO_FIX. + */ + if (session->gpsdata.fix.mode < MODE_3D) { + session->gpsdata.fix.mode = MODE_3D; + mask |= MODE_SET; + } + + /* + * Compute climb/sink in the simplest possible way. + * This substitutes for the climb report provided by + * SiRF and Garmin chips, which might have some smoothing + * going on. + */ + if (isnan(oldaltitude)!=0 || session->gpsdata.fix.time==oldfixtime) + session->gpsdata.fix.climb = 0; + else { + session->gpsdata.fix.climb = (session->gpsdata.fix.altitude-oldaltitude)/(session->gpsdata.fix.time-oldfixtime); + } + mask |= CLIMB_SET; + } + if (strlen(field[11]) > 0) { + session->gpsdata.separation = atof(field[11]); + } else { + session->gpsdata.separation = wgs84_separation(session->gpsdata.fix.latitude,session->gpsdata.fix.longitude); + } + } + gpsd_report(LOG_PROG, "GPGGA sets status %d and mode %d (%s)\n", session->gpsdata.status, session->gpsdata.fix.mode, ((mask&MODE_SET)!=0) ? "changed" : "unchanged"); + return mask; +} + +static gps_mask_t processGPGSA(int count, char *field[], struct gps_device_t *session) +/* GPS DOP and Active Satellites */ +{ + /* + eg1. $GPGSA,A,3,,,,,,16,18,,22,24,,,3.6,2.1,2.2*3C + eg2. $GPGSA,A,3,19,28,14,18,27,22,31,39,,,,,1.7,1.0,1.3*35 + 1 = Mode: + M=Manual, forced to operate in 2D or 3D + A=Automatic, 3D/2D + 2 = Mode: 1=Fix not available, 2=2D, 3=3D + 3-14 = PRNs of satellites used in position fix (null for unused fields) + 15 = PDOP + 16 = HDOP + 17 = VDOP + */ + gps_mask_t mask; + int i; + + /* + * One chipset called the i.Trek M3 issues GPGSA lines that look like + * this: "$GPGSA,A,1,,,,*32" when it has no fix. This is broken + * in at least two ways: it's got the wrong number of fields, and + * it claims to be a valid sentence (A flag) when it isn't. + * Alarmingly, it's possible this error may be generic to SiRFstarIII. + */ + if (count < 17) + return ONLINE_SET; + + session->gpsdata.fix.mode = atoi(field[2]); + /* + * The first arm of this conditional ignores dead-reckoning + * fixes from an Antaris chipset. which returns E in field 2 + * for a dead-reckoning estimate. Fix by Andreas Stricker. + */ + if (session->gpsdata.fix.mode == 0 && field[2][0] == 'E') + mask = 0; + else + mask = MODE_SET; + gpsd_report(LOG_PROG, "GPGSA sets mode %d\n", session->gpsdata.fix.mode); + session->gpsdata.pdop = atof(field[15]); + session->gpsdata.hdop = atof(field[16]); + session->gpsdata.vdop = atof(field[17]); + session->gpsdata.satellites_used = 0; + memset(session->gpsdata.used,0,sizeof(session->gpsdata.used)); + /* the magic 6 here counts the tag, two mode fields, and the DOP fields */ + for (i = 0; i < count - 6; i++) { + int prn = atoi(field[i+3]); + if (prn > 0) + session->gpsdata.used[session->gpsdata.satellites_used++] = prn; + } + mask |= USED_SET; + if (strlen(field[count-3]) > 0) + mask |= HDOP_SET; + if (strlen(field[count-2]) > 0) + mask |= VDOP_SET; + if (strlen(field[count-1]) > 0) + mask |= HDOP_SET; + return mask; +} + +static gps_mask_t processGPGSV(int count, char *field[], struct gps_device_t *session) +/* GPS Satellites in View */ +{ + /* + GSV,2,1,08,01,40,083,46,02,17,308,41,12,07,344,39,14,22,228,45*75 + 2 Number of sentences for full data + 1 Sentence 1 of 2 + 08 Total number of satellites in view + 01 Satellite PRN number + 40 Elevation, degrees + 083 Azimuth, degrees + 46 Signal-to-noise ratio in decibels + + There my be up to three GSV sentences in a data packet + */ + int n, fldnum; + if (count <= 3) { + gpsd_zero_satellites(&session->gpsdata); + session->gpsdata.satellites = 0; + return ERROR_SET; + } + if (count % 4 != 0){ + gpsd_report(LOG_WARN, "malformed GPGSV - fieldcount %d %% 4 != 0\n", count); + gpsd_zero_satellites(&session->gpsdata); + session->gpsdata.satellites = 0; + return ERROR_SET; + } + + session->driver.nmea.await = atoi(field[1]); + if (sscanf(field[2], "%d", &session->driver.nmea.part) < 1) { + gpsd_zero_satellites(&session->gpsdata); + return ERROR_SET; + } else if (session->driver.nmea.part == 1) + gpsd_zero_satellites(&session->gpsdata); + + for (fldnum = 4; fldnum < count; ) { + if (session->gpsdata.satellites >= MAXCHANNELS) { + gpsd_report(LOG_ERROR, "internal error - too many satellites!\n"); + gpsd_zero_satellites(&session->gpsdata); + break; + } + session->gpsdata.PRN[session->gpsdata.satellites] = atoi(field[fldnum++]); + session->gpsdata.elevation[session->gpsdata.satellites] = atoi(field[fldnum++]); + session->gpsdata.azimuth[session->gpsdata.satellites] = atoi(field[fldnum++]); + session->gpsdata.ss[session->gpsdata.satellites] = atoi(field[fldnum++]); + /* + * Incrementing this unconditionally falls afoul of chipsets like + * the Motorola Oncore GT+ that emit empty fields at the end of the + * last sentence in a GPGSV set if the number of satellites is not + * a multiple of 4. + */ + if (session->gpsdata.PRN[session->gpsdata.satellites] != 0) + session->gpsdata.satellites++; + } + if (session->driver.nmea.part == session->driver.nmea.await && atoi(field[3]) != session->gpsdata.satellites) + gpsd_report(LOG_WARN, "GPGSV field 3 value of %d != actual count %d\n", + atoi(field[3]), session->gpsdata.satellites); + + /* not valid data until we've seen a complete set of parts */ + if (session->driver.nmea.part < session->driver.nmea.await) { + gpsd_report(LOG_PROG, "Partial satellite data (%d of %d).\n", session->driver.nmea.part, session->driver.nmea.await); + return ERROR_SET; + } + /* + * This sanity check catches an odd behavior of SiRFstarII receivers. + * When they can't see any satellites at all (like, inside a + * building) they sometimes cough up a hairball in the form of a + * GSV packet with all the azimuth entries 0 (but nonzero + * elevations). This behavior was observed under SiRF firmware + * revision 231.000.000_A2. + */ + for (n = 0; n < session->gpsdata.satellites; n++) + if (session->gpsdata.azimuth[n] != 0) + goto sane; + gpsd_report(LOG_WARN, "Satellite data no good (%d of %d).\n", session->driver.nmea.part, session->driver.nmea.await); + gpsd_zero_satellites(&session->gpsdata); + return ERROR_SET; + sane: + gpsd_report(LOG_PROG, "Satellite data OK (%d of %d).\n", session->driver.nmea.part, session->driver.nmea.await); + return SATELLITE_SET; + } + +static gps_mask_t processPGRME(int c UNUSED, char *field[], struct gps_device_t *session) +/* Garmin Estimated Position Error */ +{ + /* + $PGRME,15.0,M,45.0,M,25.0,M*22 + 1 = horizontal error estimate + 2 = units + 3 = vertical error estimate + 4 = units + 5 = spherical error estimate + 6 = units + * + * Garmin won't say, but the general belief is that these are 50% CEP. + * We follow the advice at . + * If this assumption changes here, it should also change in garmin.c + * where we scale error estimates from Garmin binary packets, and + * in libgpsd_core.c where we generate $PGRME. + */ + if ((strcmp(field[2], "M")!=0) || + (strcmp(field[4], "M")!=0) || + (strcmp(field[6], "M")!=0)){ + session->gpsdata.fix.eph = + session->gpsdata.fix.epv = + session->gpsdata.epe = 100; + return ERROR_SET; + } + + session->gpsdata.fix.eph = atof(field[1]) * (GPSD_CONFIDENCE/CEP50_SIGMA); + session->gpsdata.fix.epv = atof(field[3]) * (GPSD_CONFIDENCE/CEP50_SIGMA); + session->gpsdata.epe = atof(field[5]) * (GPSD_CONFIDENCE/CEP50_SIGMA); + + return HERR_SET | VERR_SET | PERR_SET; +} + +static gps_mask_t processGPZDA(int c UNUSED, char *field[], struct gps_device_t *session) +/* Time & Date */ +{ + gps_mask_t mask = TIME_SET; + /* + $GPZDA,160012.71,11,03,2004,-1,00*7D + 1) UTC time (hours, minutes, seconds, may have fractional subsecond) + 2) Day, 01 to 31 + 3) Month, 01 to 12 + 4) Year (4 digits) + 5) Local zone description, 00 to +- 13 hours + 6) Local zone minutes description, apply same sign as local hours + 7) Checksum + */ + merge_hhmmss(field[1], session); + session->driver.nmea.date.tm_year = atoi(field[4]) - 1900; + session->driver.nmea.date.tm_mon = atoi(field[3])-1; + session->driver.nmea.date.tm_mday = atoi(field[2]); + session->gpsdata.fix.time = (double)mkgmtime(&session->driver.nmea.date)+session->driver.nmea.subseconds; + if (!GPS_TIME_EQUAL(session->gpsdata.sentence_time, session->gpsdata.fix.time)) { + mask |= CYCLE_START_SET; + gpsd_report(LOG_PROG, "GPZDA starts a reporting cycle.\n"); + } + session->gpsdata.sentence_time = session->gpsdata. fix.time; + return mask; +} + +#ifdef TNT_ENABLE +static gps_mask_t processTNTHTM(int c UNUSED, char *field[], struct gps_device_t *session) +{ + /* + * Proprietary sentence for True North Technologies Magnetic Compass. + * This may also apply to some Honeywell units since they may have been + * designed by True North. + + HTM,x.x,a,x.x,a,x.x,a,x.x,x.x*hh + Fields in order: + 1. True heading in degrees + 2. magnetometer status character: + C = magnetometer calibration alarm + L = low alarm + M = low warning + N = normal + O = high warning + P = high alarm + V = magnetometer voltage level alarm + 3. pitch angle + 4. pitch status character - see field 2 above + 5. roll angle + 6. roll status character - see field 2 above + 7. dip angle + 8. relative magnitude horizontal component of earth's magnetic field + *hh mandatory nmea_checksum + */ + gps_mask_t mask; + mask = ONLINE_SET; + + //gpsd_zero_satellites(&session->gpsdata); + + /* + * Heading maps to track. + * Pitch maps to climb. + * Roll maps to speed. + * Dip maps to altitude. + */ + session->gpsdata.fix.time = timestamp(); + session->gpsdata.fix.track = atof(field[1]); + session->gpsdata.headingStatus = *field[2]; + session->gpsdata.fix.climb = atof(field[3]); + session->gpsdata.pitchStatus = *field[4]; + session->gpsdata.fix.speed = atof(field[5]); + session->gpsdata.rollStatus = *field[6]; + session->gpsdata.fix.altitude = atof(field[7]); + session->gpsdata.horzField = atof(field[8]); + session->gpsdata.fix.mode = MODE_3D; + mask |= (STATUS_SET | MODE_SET | TRACK_SET | SPEED_SET | CLIMB_SET | ALTITUDE_SET); + session->gpsdata.status = STATUS_FIX; /* could be DGPS_FIX */ + + gpsd_report(LOG_RAW, "Heading %lf %c.\n", session->gpsdata.fix.track, session->gpsdata.headingStatus); + return mask; +} +#endif /* TNT_ENABLE */ + +#ifdef OCEANSERVER_ENABLE +static gps_mask_t processOHPR(int c UNUSED, char *field[], struct gps_device_t *session) +{ + /* + * Proprietary sentence for OceanServer Magnetic Compass. + + OHPR,x.x,x.x,x.x,x.x,x.x,x.x,x.x,x.x,x.x,x.x,x.x,x.x,x.x,x.x,x.x,x.x,x.x,x.x*hh + Fields in order: + 1. Azimuth + 2. Pitch Angle + 3. Roll Angle + 4. Temperature + 5. Depth (feet) + 6. Magnetic Vector Length + 7-9. 3 axis Magnetic Field readings x,y,z + 10. Acceleration Vector Length + 11-13. 3 axis Acceleration Readings x,y,z + 14. Reserved + 15-16. 2 axis Gyro Output, X,y + 17. Reserved + 18. Reserved + *hh mandatory nmea_checksum + */ + gps_mask_t mask; + mask = ONLINE_SET; + + //gpsd_zero_satellites(&session->gpsdata); + + /* + * Heading maps to track. + * Pitch maps to climb. + * Roll maps to speed. + * Depth maps to altitude. + */ + session->gpsdata.fix.time = timestamp(); + session->gpsdata.fix.track = atof(field[1]); + session->gpsdata.fix.climb = atof(field[2]); + session->gpsdata.fix.speed = atof(field[3]); + session->gpsdata.temperature = atof(field[4]); + session->gpsdata.fix.altitude = atof(field[5]); + session->gpsdata.magnetic_length = atof(field[6]); + session->gpsdata.magnetic_field_x = atof(field[7]); + session->gpsdata.magnetic_field_y = atof(field[8]); + session->gpsdata.magnetic_field_z = atof(field[9]); + session->gpsdata.acceleration_length = atof(field[10]); + session->gpsdata.acceleration_field_x = atof(field[11]); + session->gpsdata.acceleration_field_y = atof(field[12]); + session->gpsdata.acceleration_field_z = atof(field[13]); + session->gpsdata.gyro_output_x = atof(field[15]); + session->gpsdata.gyro_output_y = atof(field[16]); + session->gpsdata.fix.mode = MODE_3D; + mask |= (STATUS_SET | MODE_SET | TRACK_SET | SPEED_SET | CLIMB_SET | ALTITUDE_SET); + session->gpsdata.status = STATUS_FIX; /* could be DGPS_FIX */ + + gpsd_report(LOG_RAW, "Heading %lf.\n", session->gpsdata.fix.track); + return mask; +} +#endif /* OCEANSERVER_ENABLE */ + +#ifdef ASHTECH_ENABLE +static gps_mask_t processPASHR(int c UNUSED, char *field[], struct gps_device_t *session) +{ + gps_mask_t mask; + mask = ONLINE_SET; + + if (0 == strcmp("RID", field[1])){ /* Receiver ID */ + (void)snprintf(session->subtype, sizeof(session->subtype)-1, + "%s ver %s", field[2], field[3]); + return 0; + } else if (0 == strcmp("POS", field[1])){ /* 3D Position */ + mask |= MODE_SET | STATUS_SET | CYCLE_START_SET; + if (0 == strlen(field[2])){ + /* empty first field means no 3D fix is available */ + session->gpsdata.status = STATUS_NO_FIX; + session->gpsdata.fix.mode = MODE_NO_FIX; + return mask; + } + + /* if we make it this far, we at least have a 3D fix */ + session->gpsdata.fix.mode = MODE_3D; + if (1 == atoi(field[2])) + session->gpsdata.status = STATUS_DGPS_FIX; + else + session->gpsdata.status = STATUS_FIX; + + session->gpsdata.satellites_used = atoi(field[3]); + merge_hhmmss(field[4], session); + do_lat_lon(&field[5], &session->gpsdata); + session->gpsdata.fix.altitude = atof(field[9]); + session->gpsdata.fix.track = atof(field[11]); + session->gpsdata.fix.speed = atof(field[12]) / MPS_TO_KPH; + session->gpsdata.fix.climb = atof(field[13]); + session->gpsdata.pdop = atof(field[14]); + session->gpsdata.hdop = atof(field[15]); + session->gpsdata.vdop = atof(field[16]); + session->gpsdata.tdop = atof(field[17]); + mask |= (TIME_SET | LATLON_SET | ALTITUDE_SET); + mask |= (SPEED_SET | TRACK_SET | CLIMB_SET); + mask |= (PDOP_SET | HDOP_SET | VDOP_SET | TDOP_SET); + } else if (0 == strcmp("SAT", field[1])){ /* Satellite Status */ + int i, n, p, u; + n = session->gpsdata.satellites = atoi(field[2]); + u = 0; + for (i = 0; i < n; i++){ + session->gpsdata.PRN[i] = p = atoi(field[3+i*5+0]); + session->gpsdata.azimuth[i] = atoi(field[3+i*5+1]); + session->gpsdata.elevation[i] = atoi(field[3+i*5+2]); + session->gpsdata.ss[i] = atoi(field[3+i*5+3]); + if (field[3+i*5+4][0] == 'U') + session->gpsdata.used[u++] = p; + } + session->gpsdata.satellites_used = u; + mask |= SATELLITE_SET | USED_SET; + } + return mask; +} +#endif /* ASHTECH_ENABLE */ + +#ifdef __UNUSED__ +static short nmea_checksum(char *sentence, unsigned char *correct_sum) +/* is the checksum on the specified sentence good? */ +{ + unsigned char sum = '\0'; + char c, *p = sentence, csum[3]; + + while ((c = *p++) != '*' && c != '\0') + sum ^= c; + if (correct_sum) + *correct_sum = sum; + (void)snprintf(csum, sizeof(csum), "%02X", sum); + return(csum[0]==toupper(p[0])) && (csum[1]==toupper(p[1])); +} +#endif /* __ UNUSED__ */ + +/************************************************************************** + * + * Entry points begin here + * + **************************************************************************/ + +/*@ -mayaliasunique @*/ +gps_mask_t nmea_parse(char *sentence, struct gps_device_t *session) +/* parse an NMEA sentence, unpack it into a session structure */ +{ + typedef gps_mask_t (*nmea_decoder)(int count, char *f[], struct gps_device_t *session); + static struct { + char *name; + int nf; /* minimum number of fields required to parse */ + nmea_decoder decoder; + } nmea_phrase[] = { + /*@ -nullassign @*/ + {"RMC", 8, processGPRMC}, + {"GGA", 13, processGPGGA}, + {"GLL", 7, processGPGLL}, + {"GSA", 17, processGPGSA}, + {"GSV", 0, processGPGSV}, + {"VTG", 0, NULL}, /* ignore Velocity Track made Good */ + {"ZDA", 7, processGPZDA}, + {"PGRMC", 0, NULL}, /* ignore Garmin Sensor Config */ + {"PGRME", 7, processPGRME}, + {"PGRMI", 0, NULL}, /* ignore Garmin Sensor Init */ + {"PGRMO", 0, NULL}, /* ignore Garmin Sentence Enable */ +#ifdef TNT_ENABLE + {"PTNTHTM", 9, processTNTHTM}, +#endif /* TNT_ENABLE */ +#ifdef ASHTECH_ENABLE + {"PASHR", 3, processPASHR}, /* general handler for Ashtech */ +#endif /* ASHTECH_ENABLE */ +#ifdef OCEANSERVER_ENABLE + {"OHPR", 18, processOHPR}, +#endif /* OCEANSERVER_ENABLE */ + /*@ +nullassign @*/ + }; + + int count; + gps_mask_t retval = 0; + unsigned int i; + char *p, *s, *e; +#ifndef USE_OLD_SPLIT + volatile char *t; +#endif +#ifdef __UNUSED__ + unsigned char sum; + + if (!nmea_checksum(sentence+1, &sum)) { + gpsd_report(LOG_ERROR, "Bad NMEA checksum: '%s' should be %02X\n", + sentence, sum); + return 0; + } +#endif /* __ UNUSED__ */ + + /* + * We've had reports that on the Garmin GPS-10 the device sometimes + * (1:1000 or so) sends garbage packets that have a valid checksum + * but are like 2 successive NMEA packets merged together in one + * with some fields lost. Usually these are much longer than the + * legal limit for NMEA, so we can cope by just tossing out overlong + * packets. This may be a generic bug of all Garmin chipsets. + */ + if (strlen(sentence) > NMEA_MAX) { + gpsd_report(LOG_WARN, "Overlong packet rejected.\n"); + return ONLINE_SET; + } + +#ifdef BREAK_REGRESSIONS + /* trim trailing CR/LF */ + for (i = 0; i < strlen(sentence); i++) + if ((sentence[i] == '\r') || (sentence[i] == '\n')){ + sentence[i] = '\0'; + break; + } +#endif + /*@ -usedef @*//* splint 3.1.1 seems to have a bug here */ + /* make an editable copy of the sentence */ + strncpy((char *)session->driver.nmea.fieldcopy, sentence, NMEA_MAX); + /* discard the checksum part */ + for (p = (char *)session->driver.nmea.fieldcopy; (*p != '*') && (*p >= ' '); ) ++p; + if (*p == '*') + *p++ = ','; /* otherwise we drop the last field */ + *p = '\0'; + e = p; + /* split sentence copy on commas, filling the field array */ +#ifdef USE_OLD_SPLIT + for (count = 0, p = (char *)session->driver.nmea.fieldcopy; p != NULL && *p != '\0'; ++count, p = strchr(p, ',')) { + *p = '\0'; + session->driver.nmea.field[count] = ++p; + } +#else + count = 0; + t = p; /* end of sentence */ + p = (char *)session->driver.nmea.fieldcopy + 1; /* beginning of tag, 'G' not '$' */ + /* while there is a search string and we haven't run off the buffer... */ + while((p != NULL) && (p <= t)){ + session->driver.nmea.field[count] = p; /* we have a field. record it */ + /*@ -compdef @*/ + if ((p = strchr(p, ',')) != NULL){ /* search for the next delimiter */ + *p = '\0'; /* replace it with a NUL */ + count++; /* bump the counters and continue */ + p++; + } + /*@ +compdef @*/ + } +#endif + /* point remaining fields at empty string, just in case */ + for (i = (unsigned int)count; + i < (unsigned)(sizeof(session->driver.nmea.field)/sizeof(session->driver.nmea.field[0])); + i++) + session->driver.nmea.field[i] = e; + /* dispatch on field zero, the sentence tag */ + for (i = 0; i < (unsigned)(sizeof(nmea_phrase)/sizeof(nmea_phrase[0])); ++i) { + s = session->driver.nmea.field[0]; + if (strlen(nmea_phrase[i].name) == 3) + s += 2; /* skip talker ID */ + if (strcmp(nmea_phrase[i].name, s) == 0) { + if (nmea_phrase[i].decoder!=NULL && (count >= nmea_phrase[i].nf)) { + retval = (nmea_phrase[i].decoder)(count, session->driver.nmea.field, session); + strncpy(session->gpsdata.tag, nmea_phrase[i].name, MAXTAGLEN); + session->gpsdata.sentence_length = strlen(sentence); + } else + retval = ONLINE_SET; /* unknown sentence */ + break; + } + } +#ifdef MKT3301_ENABLE + if (strncmp("PMTK", session->driver.nmea.field[0], 4) == 0) /* general handler for MKT3301 vendor specifics */ + retval = processMKT3301(count, session->driver.nmea.field, session); +#endif /* MKT3301_ENABLE */ + /*@ +usedef @*/ + return retval; +} +/*@ +mayaliasunique @*/ +#endif /* NMEA_ENABLE */ + +void nmea_add_checksum(char *sentence) +/* add NMEA checksum to a possibly *-terminated sentence */ +{ + unsigned char sum = '\0'; + char c, *p = sentence; + + if (*p == '$') { + p++; + } else { + gpsd_report(LOG_ERROR, "Bad NMEA sentence: '%s'\n", sentence); + } + while ( ((c = *p) != '*') && (c != '\0')) { + sum ^= c; + p++; + } + *p++ = '*'; + (void)snprintf(p, 5, "%02X\r\n", (unsigned)sum); +} + +ssize_t nmea_write(struct gps_device_t *session, char *buf, size_t len UNUSED) +/* ship a command to the GPS, adding * and correct checksum */ +{ + (void)strlcpy(session->msgbuf, buf, sizeof(session->msgbuf)); + if (session->msgbuf[0] == '$') { + (void)strlcat(session->msgbuf, "*", sizeof(session->msgbuf)); + nmea_add_checksum(session->msgbuf); + } else + (void)strlcat(session->msgbuf, "\r\n", sizeof(session->msgbuf)); + session->msgbuflen = strlen(session->msgbuf); + return gpsd_write(session, session->msgbuf, session->msgbuflen); +} + +ssize_t nmea_send(struct gps_device_t *session, const char *fmt, ... ) +{ + char buf[BUFSIZ]; + va_list ap; + + va_start(ap, fmt) ; + (void)vsnprintf(buf, sizeof(buf)-5, fmt, ap); + va_end(ap); + return nmea_write(session, buf, strlen(buf)); +} diff --git a/driver_rtcm2.c b/driver_rtcm2.c new file mode 100644 index 00000000..c6683962 --- /dev/null +++ b/driver_rtcm2.c @@ -0,0 +1,739 @@ +/* $Id$ */ +/***************************************************************************** + +This is a decoder for RTCM-104 2.x, an obscure and complicated serial +protocol used for broadcasting pseudorange corrections from +differential-GPS reference stations. The applicable +standard is + +RTCM RECOMMENDED STANDARDS FOR DIFFERENTIAL NAVSTAR GPS SERVICE, +RTCM PAPER 194-93/SC 104-STD + +Ordering instructions are accessible from +under "Publications". This describes version 2.1 of the RTCM specification. +RTCM-104 was later incrementally revised up to a 2.3 level before being +completely redesigned as level 3.0. + +Also applicable is ITU-R M.823: "Technical characteristics of +differential transmissions for global navigation satellite systems +from maritime radio beacons in the frequency band 283.5 - 315 kHz in +region 1 and 285 - 325 kHz in regions 2 & 3." + +The RTCM 2.x protocol uses as a transport layer the GPS satellite downlink +protocol described in IS-GPS-200, the Navstar GPS Interface +Specification. This code relies on the lower-level packet-assembly +code for that protocol in isgps.c. + +The lower layer's job is done when it has assembled a message of up to +33 words of clean parity-checked data. At this point this upper layer +takes over. struct rtcm2_msg_t is overlaid on the buffer and the bitfields +are used to extract pieces of it. Those pieces are copied and (where +necessary) reassembled into a struct rtcm2_t. + +This code and the contents of isgps.c are evolved from code by Wolfgang +Rupprecht. Wolfgang's decoder was loosely based on one written by +John Sager in 1999 (in particular the dump function emits a close +descendant of Sager's dump format). Here are John Sager's original +notes: + +The RTCM decoder prints a legible representation of the input data. +The RTCM SC-104 specification is copyrighted, so I cannot +quote it - in fact, I have never read it! Most of the information +used to develop the decoder came from publication ITU-R M.823. +This is a specification of the data transmitted from LF DGPS +beacons in the 300kHz band. M.823 contains most of those parts of +RTCM SC-104 directly relevant to the air interface (there +are one or two annoying and vital omissions!). Information +about the serial interface format was gleaned from studying +the output of a beacon receiver test program made available on +Starlink's website. + +*****************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include /* for round() */ + +#include "gpsd_config.h" +#include "gpsd.h" +#include "driver_rtcm2.h" + +#ifdef RTCM104V2_ENABLE + +#define PREAMBLE_PATTERN 0x66 + +static unsigned int tx_speed[] = { 25, 50, 100, 110, 150, 200, 250, 300 }; + +#define DIMENSION(a) (unsigned)(sizeof(a)/sizeof(a[0])) + +void rtcm2_unpack(/*@out@*/struct rtcm2_t *tp, char *buf) +/* break out the raw bits into the content fields */ +{ + int len; + unsigned int n, w; + struct rtcm2_msg_t *msg = (struct rtcm2_msg_t *)buf; + + tp->type = msg->w1.msgtype; + tp->length = msg->w2.frmlen; + tp->zcount = msg->w2.zcnt * ZCOUNT_SCALE; + tp->refstaid = msg->w1.refstaid; + tp->seqnum = msg->w2.sqnum; + tp->stathlth = msg->w2.stathlth; + + len = (int)tp->length; + n = 0; + switch (tp->type) { + case 1: + case 9: + { + struct b_correction_t *m = &msg->msg_type.type1.corrections[0]; + + while (len >= 0) { + if (len >= 2) { + tp->msg_data.ranges.sat[n].ident = m->w3.satident1; + tp->msg_data.ranges.sat[n].udre = m->w3.udre1; + tp->msg_data.ranges.sat[n].issuedata = m->w4.issuedata1; + tp->msg_data.ranges.sat[n].rangerr = m->w3.pc1 * + (m->w3.scale1 ? PCLARGE : PCSMALL); + tp->msg_data.ranges.sat[n].rangerate = m->w4.rangerate1 * + (m->w3.scale1 ? RRLARGE : RRSMALL); + n++; + } + if (len >= 4) { + tp->msg_data.ranges.sat[n].ident = m->w4.satident2; + tp->msg_data.ranges.sat[n].udre = m->w4.udre2; + tp->msg_data.ranges.sat[n].issuedata = m->w6.issuedata2; + tp->msg_data.ranges.sat[n].rangerr = m->w5.pc2 * + (m->w4.scale2 ? PCLARGE : PCSMALL); + tp->msg_data.ranges.sat[n].rangerate = m->w5.rangerate2 * + (m->w4.scale2 ? RRLARGE : RRSMALL); + n++; + } + if (len >= 5) { + tp->msg_data.ranges.sat[n].ident = m->w6.satident3; + tp->msg_data.ranges.sat[n].udre = m->w6.udre3; + tp->msg_data.ranges.sat[n].issuedata = m->w7.issuedata3; + /*@ -shiftimplementation @*/ + tp->msg_data.ranges.sat[n].rangerr = ((m->w6.pc3_h<<8)|(m->w7.pc3_l)) * + (m->w6.scale3 ? PCLARGE : PCSMALL); + tp->msg_data.ranges.sat[n].rangerate = m->w7.rangerate3 * + (m->w6.scale3 ? RRLARGE : RRSMALL); + /*@ +shiftimplementation @*/ + n++; + } + len -= 5; + m++; + } + tp->msg_data.ranges.nentries = n; + } + break; + case 3: + { + struct rtcm2_msg3 *m = &msg->msg_type.type3; + + if ((tp->msg_data.ecef.valid = len >= 4)) { + tp->msg_data.ecef.x = ((m->w3.x_h<<8)|(m->w4.x_l))*XYZ_SCALE; + tp->msg_data.ecef.y = ((m->w4.y_h<<16)|(m->w5.y_l))*XYZ_SCALE; + tp->msg_data.ecef.z = ((m->w5.z_h<<24)|(m->w6.z_l))*XYZ_SCALE; + } + } + break; + case 4: + if ((tp->msg_data.reference.valid = len >= 2)){ + struct rtcm2_msg4 *m = &msg->msg_type.type4; + + tp->msg_data.reference.system = + (m->w3.dgnss==0) ? gps : + ((m->w3.dgnss==1) ? glonass : unknown); + tp->msg_data.reference.sense = (m->w3.dat != 0) ? global : local; + if (m->w3.datum_alpha_char1){ + tp->msg_data.reference.datum[n++] = (char)(m->w3.datum_alpha_char1); + } + if (m->w3.datum_alpha_char2){ + tp->msg_data.reference.datum[n++] = (char)(m->w3.datum_alpha_char2); + } + if (m->w4.datum_sub_div_char1){ + tp->msg_data.reference.datum[n++] = (char)(m->w4.datum_sub_div_char1); + } + if (m->w4.datum_sub_div_char2){ + tp->msg_data.reference.datum[n++] = (char)(m->w4.datum_sub_div_char2); + } + if (m->w4.datum_sub_div_char3){ + tp->msg_data.reference.datum[n++] = (char)(m->w4.datum_sub_div_char3); + } + tp->msg_data.reference.datum[n++] = '\0'; + if (len >= 4) { + tp->msg_data.reference.dx = m->w5.dx * DXYZ_SCALE; + tp->msg_data.reference.dy = ((m->w5.dy_h << 8) | m->w6.dy_l) * DXYZ_SCALE; + tp->msg_data.reference.dz = m->w6.dz * DXYZ_SCALE; + } else + tp->msg_data.reference.sense = invalid; + } + break; + case 5: + for (n = 0; n < (unsigned)len; n++) { + struct consat_t *csp = &tp->msg_data.conhealth.sat[n]; + struct b_health_t *m = &msg->msg_type.type5.health[n]; + + csp->ident = m->sat_id; + csp->iodl = m->issue_of_data_link!=0; + csp->health = m->data_health; + /*@i@*/csp->snr = (m->cn0?(m->cn0+CNR_OFFSET):SNR_BAD); + csp->health_en = m->health_enable; + csp->new_data = m->new_nav_data!=0; + csp->los_warning = m->loss_warn!=0; + csp->tou = m->time_unhealthy*TU_SCALE; + } + tp->msg_data.conhealth.nentries = n; + break; + case 7: + for (w = 0; w < (unsigned)len; w++) { + struct station_t *np = &tp->msg_data.almanac.station[n]; + struct b_station_t *mp = &msg->msg_type.type7.almanac[w]; + + np->latitude = mp->w3.lat * LA_SCALE; + /*@i@*/np->longitude = ((mp->w3.lon_h << 8) | mp->w4.lon_l) * LO_SCALE; + np->range = mp->w4.range; + np->frequency = (((mp->w4.freq_h << 6) | mp->w5.freq_l) * FREQ_SCALE) + FREQ_OFFSET; + np->health = mp->w5.health; + np->station_id = mp->w5.station_id, + np->bitrate = tx_speed[mp->w5.bit_rate]; + n++; + } + tp->msg_data.almanac.nentries = (unsigned)(len/3); + break; + case 16: + /*@ -boolops @*/ + for (w = 0; w < (unsigned)len; w++){ + if (!msg->msg_type.type16.txt[w].byte1) { + break; + } + tp->msg_data.message[n++] = (char)(msg->msg_type.type16.txt[w].byte1); + if (!msg->msg_type.type16.txt[w].byte2) { + break; + } + tp->msg_data.message[n++] = (char)(msg->msg_type.type16.txt[w].byte2); + if (!msg->msg_type.type16.txt[w].byte3) { + break; + } + tp->msg_data.message[n++] = (char)(msg->msg_type.type16.txt[w].byte3); + } + /*@ +boolops @*/ + tp->msg_data.message[n++] = '\0'; + break; + + default: + memcpy(tp->msg_data.words, msg->msg_type.rtcm2_msgunk, (RTCM2_WORDS_MAX-2)*sizeof(isgps30bits_t)); + break; + } +} + +bool rtcm2_repack(struct rtcm2_t *tp, isgps30bits_t *buf) +/* repack the content fields into the raw bits */ +{ + int len, sval; + unsigned int n, w, uval; + struct rtcm2_msg_t *msg = (struct rtcm2_msg_t *)buf; + struct rtcm2_msghw1 *wp = (struct rtcm2_msghw1 *)buf; + + msg->w1.msgtype = tp->type; + msg->w2.frmlen = tp->length; + msg->w2.zcnt = (unsigned) round(tp->zcount / ZCOUNT_SCALE); + msg->w1.refstaid = tp->refstaid; + msg->w2.sqnum = tp->seqnum; + msg->w2.stathlth = tp->stathlth; + + len = (int)tp->length; + n = 0; + switch (tp->type) { + case 1: /* S */ + case 9: + { + struct b_correction_t *m = &msg->msg_type.type1.corrections[0]; + + while (len >= 0) { + if (len >= 2) { + struct rangesat_t *ssp = &tp->msg_data.ranges.sat[n]; + m->w3.satident1 = ssp->ident; + m->w3.udre1 = ssp->udre; + m->w4.issuedata1 = ssp->issuedata; + m->w3.scale1 = (unsigned)((ssp->rangerr > MAXPCSMALL) || + (ssp->rangerr < (-MAXPCSMALL)) || + (ssp->rangerate > MAXRRSMALL) || + (ssp->rangerate < (-MAXRRSMALL))); + m->w3.pc1 = (int) round(ssp->rangerr / (m->w3.scale1 ? PCLARGE : PCSMALL)); + m->w4.rangerate1 = (int) round(ssp->rangerate / (m->w3.scale1 ? RRLARGE : RRSMALL)); + n++; + } + if (len >= 4) { + struct rangesat_t *ssp = &tp->msg_data.ranges.sat[n]; + m->w4.satident2 = ssp->ident; + m->w4.udre2 = ssp->udre; + m->w6.issuedata2 = ssp->issuedata; + m->w4.scale2 = (unsigned)((ssp->rangerr > MAXPCSMALL) || + (ssp->rangerr < (-MAXPCSMALL)) || + (ssp->rangerate > MAXRRSMALL) || + (ssp->rangerate < (-MAXRRSMALL))); + m->w5.pc2 = (int) round(ssp->rangerr / (m->w4.scale2 ? PCLARGE : PCSMALL)); + m->w5.rangerate2 = (int) round(ssp->rangerate / (m->w4.scale2 ? RRLARGE : RRSMALL)); + n++; + } + if (len >= 5) { + struct rangesat_t *ssp = &tp->msg_data.ranges.sat[n]; + m->w6.satident3 = ssp->ident; + m->w6.udre3 = ssp->udre; + m->w7.issuedata3 = ssp->issuedata; + m->w6.scale3 = (unsigned)((ssp->rangerr > MAXPCSMALL) || + (ssp->rangerr < (-MAXPCSMALL)) || + (ssp->rangerate > MAXRRSMALL) || + (ssp->rangerate < (-MAXRRSMALL))); + sval = (int) round(ssp->rangerr / (m->w6.scale3 ? PCLARGE : PCSMALL)); + /*@ -shiftimplementation @*/ + m->w6.pc3_h = sval >> 8; + /*@ +shiftimplementation @*/ + m->w7.pc3_l = (unsigned)sval & 0xff; + m->w7.rangerate3 = (int) round(ssp->rangerate / (m->w6.scale3 ? RRLARGE : RRSMALL)); + n++; + } + len -= 5; + m++; + } + tp->msg_data.ranges.nentries = n; + } + break; + case 3: /* R */ + if (tp->msg_data.ecef.valid) { + struct rtcm2_msg3 *m = &msg->msg_type.type3; + unsigned x = (unsigned) round(tp->msg_data.ecef.x / XYZ_SCALE); + unsigned y = (unsigned) round(tp->msg_data.ecef.y / XYZ_SCALE); + unsigned z = (unsigned) round(tp->msg_data.ecef.z / XYZ_SCALE); + + m->w4.x_l = x & 0xff; + m->w3.x_h = x >> 8; + m->w5.y_l = y & 0xffff; + m->w4.y_h = y >> 16; + m->w6.z_l = z & 0xffffff; + m->w5.z_h = z >> 24; + } + break; + case 4: /* D */ + if (tp->msg_data.reference.valid) { + struct rtcm2_msg4 *m = &msg->msg_type.type4; + + m->w3.dgnss = tp->msg_data.reference.system; + m->w3.dat = (unsigned)(tp->msg_data.reference.sense == global); + /*@ -predboolothers -type @*/ + if (tp->msg_data.reference.datum[0]) + m->w3.datum_alpha_char1 = tp->msg_data.reference.datum[0]; + else + m->w3.datum_alpha_char1 = 0; + if (tp->msg_data.reference.datum[1]) + m->w3.datum_alpha_char2 = tp->msg_data.reference.datum[1]; + else + m->w3.datum_alpha_char2 = 0; + if (tp->msg_data.reference.datum[2]) + m->w4.datum_sub_div_char1 = tp->msg_data.reference.datum[2]; + else + m->w4.datum_sub_div_char1 = 0; + if (tp->msg_data.reference.datum[3]) + m->w4.datum_sub_div_char2 = tp->msg_data.reference.datum[3]; + else + m->w4.datum_sub_div_char2 = 0; + if (tp->msg_data.reference.datum[4]) + m->w4.datum_sub_div_char3 = tp->msg_data.reference.datum[4]; + else + m->w4.datum_sub_div_char3 = 0; + /*@ +predboolothers +type @*/ + if (tp->msg_data.reference.system != unknown) { + m->w5.dx = (uint)round(tp->msg_data.reference.dx / DXYZ_SCALE); + uval = (uint)round(tp->msg_data.reference.dy / DXYZ_SCALE); + m->w5.dy_h = uval >> 8; + m->w6.dy_l = uval & 0xff; + m->w6.dz = (uint)round(tp->msg_data.reference.dz / DXYZ_SCALE); + } + } + break; + case 5: /* C */ + for (n = 0; n < (unsigned)len; n++) { + struct consat_t *csp = &tp->msg_data.conhealth.sat[n]; + struct b_health_t *m = &msg->msg_type.type5.health[n]; + + m->sat_id = csp->ident; + m->issue_of_data_link = (unsigned)csp->iodl; + m->data_health = csp->health; + m->cn0 = (csp->snr == SNR_BAD) ? 0 : (unsigned)csp->snr-CNR_OFFSET; + m->health_enable = csp->health_en; + m->new_nav_data = (unsigned)csp->new_data; + m->loss_warn = (unsigned)csp->los_warning; + m->time_unhealthy = (unsigned)(csp->tou / TU_SCALE); + } + break; + case 7: /* A */ + for (w = 0; w < (RTCM2_WORDS_MAX - 2)/ 3; w++) { + struct station_t *np = &tp->msg_data.almanac.station[n++]; + struct b_station_t *mp = &msg->msg_type.type7.almanac[w]; + + mp->w3.lat = (int) round(np->latitude / LA_SCALE); + sval = (int) round(np->longitude / LO_SCALE); + /*@ -shiftimplementation @*/ + mp->w3.lon_h = sval >> 8; + /*@ +shiftimplementation @*/ + mp->w4.lon_l = (unsigned)sval & 0xff; + mp->w4.range = np->range; + uval = (unsigned) round(((np->frequency-FREQ_OFFSET) / FREQ_SCALE)); + mp->w4.freq_h = uval >> 6; + mp->w5.freq_l = uval & 0x3f; + mp->w5.health = np->health; + mp->w5.station_id = np->station_id; + mp->w5.bit_rate = 0; + for (uval = 0; uval < (unsigned)(sizeof(tx_speed)/sizeof(tx_speed[0])); uval++) + if (tx_speed[uval] == np->bitrate) { + mp->w5.bit_rate = uval; + break; + } + if (mp->w5.bit_rate == 0) + return false; + } + tp->msg_data.almanac.nentries = n; + break; + case 16: /* T */ + /*@ -boolops @*/ + for (w = 0; w < RTCM2_WORDS_MAX - 2; w++){ + if (!tp->msg_data.message[n]) { + break; + } + msg->msg_type.type16.txt[w].byte1 = (unsigned)tp->msg_data.message[n++]; + if (!tp->msg_data.message[n]) { + break; + } + msg->msg_type.type16.txt[w].byte2 = (unsigned)tp->msg_data.message[n++]; + if (!tp->msg_data.message[n]) { + break; + } + msg->msg_type.type16.txt[w].byte3 = (unsigned)tp->msg_data.message[n++]; + } + msg->w2.frmlen = w+1; + /*@ +boolops @*/ + break; + + default: /* U */ + memcpy(msg->msg_type.rtcm2_msgunk, tp->msg_data.words, (RTCM2_WORDS_MAX-2)*sizeof(isgps30bits_t)); + break; + } + + /* compute parity for each word in the message */ + for (w = 0; w < tp->length; w++) + wp[w].parity = isgps_parity(buf[w]); + + /* FIXME: must do inversion here */ + return true; +} + +static bool preamble_match(isgps30bits_t *w) +{ + return (((struct rtcm2_msghw1 *)w)->preamble == PREAMBLE_PATTERN); +} + +static bool length_check(struct gps_packet_t *lexer) +{ + return lexer->isgps.bufindex >= 2 + && lexer->isgps.bufindex >= ((struct rtcm2_msg_t *)lexer->isgps.buf)->w2.frmlen + 2u; +} + +enum isgpsstat_t rtcm2_decode(struct gps_packet_t *lexer, unsigned int c) +{ + return isgps_decode(lexer, + preamble_match, + length_check, + RTCM2_WORDS_MAX, + c); +} + +void rtcm2_dump(struct rtcm2_t *rtcm, /*@out@*/char buf[], size_t buflen) +/* dump the contents of a parsed RTCM104 message */ +{ + unsigned int n; + + (void)snprintf(buf, buflen, "H\t%u\t%u\t%0.1f\t%u\t%u\t%u\n", + rtcm->type, + rtcm->refstaid, + rtcm->zcount, + rtcm->seqnum, + rtcm->length, + rtcm->stathlth); + + switch (rtcm->type) { + case 1: + case 9: + for (n = 0; n < rtcm->msg_data.ranges.nentries; n++) { + struct rangesat_t *rsp = &rtcm->msg_data.ranges.sat[n]; + (void)snprintf(buf + strlen(buf), buflen - strlen(buf), + "S\t%u\t%u\t%u\t%0.1f\t%0.3f\t%0.3f\n", + rsp->ident, + rsp->udre, + rsp->issuedata, + rtcm->zcount, + rsp->rangerr, + rsp->rangerate); + } + break; + + case 3: + if (rtcm->msg_data.ecef.valid) + (void)snprintf(buf + strlen(buf), buflen - strlen(buf), + "R\t%.2f\t%.2f\t%.2f\n", + rtcm->msg_data.ecef.x, + rtcm->msg_data.ecef.y, + rtcm->msg_data.ecef.z); + break; + + case 4: + if (rtcm->msg_data.reference.valid) + (void)snprintf(buf + strlen(buf), buflen - strlen(buf), + "D\t%s\t%1d\t%s\t%.1f\t%.1f\t%.1f\n", + (rtcm->msg_data.reference.system==gps) ? "GPS" + : ((rtcm->msg_data.reference.system==glonass) ? "GLONASS" + : "UNKNOWN"), + rtcm->msg_data.reference.sense, + rtcm->msg_data.reference.datum, + rtcm->msg_data.reference.dx, + rtcm->msg_data.reference.dy, + rtcm->msg_data.reference.dz); + break; + + case 5: + for (n = 0; n < rtcm->msg_data.conhealth.nentries; n++) { + struct consat_t *csp = &rtcm->msg_data.conhealth.sat[n]; + (void)snprintf(buf + strlen(buf), buflen - strlen(buf), + "C\t%2u\t%1u\t%1u\t%2d\t%1u\t%1u\t%1u\t%2u\n", + csp->ident, + (unsigned)csp->iodl, + (unsigned)csp->health, + csp->snr, + (unsigned)csp->health_en, + (unsigned)csp->new_data, + (unsigned)csp->los_warning, + csp->tou); + } + break; + + case 6: /* NOP msg */ + (void)strlcat(buf, "N\n", buflen); + break; + + case 7: + for (n = 0; n < rtcm->msg_data.almanac.nentries; n++) { + struct station_t *ssp = &rtcm->msg_data.almanac.station[n]; + (void)snprintf(buf + strlen(buf), buflen - strlen(buf), + "A\t%.4f\t%.4f\t%u\t%.1f\t%u\t%u\t%u\n", + ssp->latitude, + ssp->longitude, + ssp->range, + ssp->frequency, + ssp->health, + ssp->station_id, + ssp->bitrate); + } + break; + case 16: + (void)snprintf(buf + strlen(buf), buflen - strlen(buf), + "T\t\"%s\"\n", rtcm->msg_data.message); + break; + + default: + for (n = 0; n < rtcm->length; n++) + (void)snprintf(buf + strlen(buf), buflen - strlen(buf), + "U\t0x%08x\n", rtcm->msg_data.words[n]); + break; + } + + (void)strlcat(buf, ".\n", buflen); +} + +int rtcm2_undump(/*@out@*/struct rtcm2_t *rtcmp, char *buf) +/* merge a line of data into an RTCM structure, return 0 if done */ +{ + int fldcount, v; + unsigned n; + char buf2[BUFSIZ]; /* stdio.h says BUFSIZ=1024. True everywhere? */ + + /*@ -usedef @*/ + switch (rtcmp->type) { + case 0: + fldcount = sscanf(buf, "H\t%u\t%u\t%lf\t%u\t%u\t%u\n", + &rtcmp->type, + &rtcmp->refstaid, + &rtcmp->zcount, + &rtcmp->seqnum, + &rtcmp->length, + &rtcmp->stathlth); + if (fldcount != 6) + return -1; + else + return 1; + //break; + + case 1: + case 9: + { + struct rangesat_t *rsp = &rtcmp->msg_data.ranges.sat[rtcmp->msg_data.ranges.nentries++]; + /* we ignore the third (zcount) field, it's in the parent */ + fldcount = sscanf(buf, + "S\t%u\t%u\t%u\t%*f\t%lf\t%lf\n", + &rsp->ident, + &rsp->udre, + &rsp->issuedata, + &rsp->rangerr, + &rsp->rangerate); + if (fldcount != 5 || (rtcmp->type != 1 && rtcmp->type != 9)) + return (int)(-rtcmp->type-1); + else if (rtcmp->msg_data.ranges.nentries != rtcmp->length*3/5) + return (int)(rtcmp->type+1); + else + return 0; + } + //break; + + case 3: + fldcount = sscanf(buf, + "R\t%lf\t%lf\t%lf\n", + &rtcmp->msg_data.ecef.x, + &rtcmp->msg_data.ecef.y, + &rtcmp->msg_data.ecef.z); + if (fldcount != 3 || rtcmp->type != 3) + return -4; + else { + rtcmp->msg_data.ecef.valid = true; + return 0; + } + //break; + + case 4: + fldcount = sscanf(buf, + "D\t%1023s\t%1d\t%5s\t%lf\t%lf\t%lf\n", + buf2, + &v, + (char *)&rtcmp->msg_data.reference.datum, + &rtcmp->msg_data.reference.dx, + &rtcmp->msg_data.reference.dy, + &rtcmp->msg_data.reference.dz); + if (fldcount != 6 || rtcmp->type != 4) + return -5; + else { + if (strcmp(buf2, "GPS") == 0) + rtcmp->msg_data.reference.system = gps; + else if (strcmp(buf2, "GLONASS") == 0) + rtcmp->msg_data.reference.system = glonass; + else + rtcmp->msg_data.reference.system = unknown; + rtcmp->msg_data.reference.sense = (v == 1) ? global : ((v == 0) ? local : invalid); + rtcmp->msg_data.reference.valid = true; + return 0; + } + //break; + + case 5: + { + struct consat_t *csp = &rtcmp->msg_data.conhealth.sat[rtcmp->msg_data.conhealth.nentries++]; + unsigned int iodl, new_data, los_warning; + + fldcount = sscanf(buf, + "C\t%2u\t%1u\t%1u\t%2d\t%1u\t%1u\t%1u\t%2u\n", + &csp->ident, + &iodl, + &csp->health, + &csp->snr, + &csp->health_en, + &new_data, + &los_warning, + &csp->tou); + csp->iodl = iodl > 0; + csp->new_data = new_data > 0; + csp->los_warning = los_warning > 0; + if (fldcount != 8 || rtcmp->type != 5) + return -6; + else if (rtcmp->msg_data.conhealth.nentries < rtcmp->length) + return 6; + else + return 0; + } + //break; + + case 6: /* NOP msg */ + if (buf[0] != 'N') + return -7; + else + return 0; + //break; + + case 7: + { + struct station_t *ssp = &rtcmp->msg_data.almanac.station[rtcmp->msg_data.almanac.nentries++]; + fldcount = sscanf(buf, + "A\t%lf\t%lf\t%u\t%lf\t%u\t%u\t%u\n", + &ssp->latitude, + &ssp->longitude, + &ssp->range, + &ssp->frequency, + &ssp->health, + &ssp->station_id, + &ssp->bitrate); + if (fldcount != 7 || rtcmp->type != 7) + return 8; + else if (rtcmp->msg_data.almanac.nentries < rtcmp->length/3) + return 8; + else + return 0; + } + //break; + + case 16: + fldcount = sscanf(buf, "T\t\"%[^\"]\"\n", rtcmp->msg_data.message); + if (fldcount != 1) + return 16; + else + return 0; + //break; + + default: + for (n = 0; n < DIMENSION(rtcmp->msg_data.words); n++) + if (rtcmp->msg_data.words[n] == 0) + break; + if (n >= DIMENSION(rtcmp->msg_data.words)) + return 0; + else { + unsigned int u; + fldcount = sscanf(buf, "U\t0x%08x\n", &u); + if (fldcount != 1) + return (int)(-rtcmp->type-1); + else { + rtcmp->msg_data.words[n] = (isgps30bits_t)u; + if (n == rtcmp->length-1) + return 0; + else + return (int)(rtcmp->type+1); + } + } + //break; + } + /*@ +usedef @*/ +} + +#ifdef __UNUSED__ +void rtcm2_output_magnavox(isgps30bits_t *ip, FILE *fp) +/* ship an RTCM message in the format emitted by Magnavox DGPS receivers */ +{ + static uint sqnum = 0; + + ((struct rtcm2_msg_t *) ip)->w2.sqnum = sqnum++; + sqnum &= 0x7; + + isgps_output_magnavox(ip, ((struct rtcm2_msg_t *) ip)->w2.frmlen + 2, fp); +} +#endif /* __UNUSED__ */ + +#endif /* RTCM104V2_ENABLE */ diff --git a/driver_rtcm2.h b/driver_rtcm2.h new file mode 100644 index 00000000..fe2fdfe4 --- /dev/null +++ b/driver_rtcm2.h @@ -0,0 +1,497 @@ +/* $Id$ */ +#ifndef _GPSD_RTCM2_H_ +#define _GPSD_RTCM2_H_ + +/***************************************************************************** + +This is a decoder for RTCM-104 2.x, an obscure and complicated serial +protocol used for broadcasting pseudorange corrections from +differential-GPS reference stations. The applicable +standard is + +RTCM RECOMMENDED STANDARDS FOR DIFFERENTIAL NAVSTAR GPS SERVICE, +RTCM PAPER 194-93/SC 104-STD + +Ordering instructions are accessible from +under "Publications". This describes version 2.1 of the RTCM specification. +RTCM-104 was later incrementally revised up to a 2.3 level before being +completely redesigned as level 3.0. + +Also applicable is ITU-R M.823: "Technical characteristics of +differential transmissions for global navigation satellite systems +from maritime radio beacons in the frequency band 283.5 - 315 kHz in +region 1 and 285 - 325 kHz in regions 2 & 3." + +The RTCM 2.x protocol uses as a transport layer the GPS satellite +downlink protocol described in IS-GPS-200, the Navstar GPS Interface +Specification. This code relies on the lower-level packet-assembly +code for that protocol in isgps.c. + +The lower layer's job is done when it has assembled a message of up to +33 words of clean parity-checked data. At this point this upper layer +takes over. struct rtcm2_msg_t is overlaid on the buffer and the bitfields +are used to extract pieces of it. Those pieces are copied and (where +necessary) reassembled into a struct rtcm2_t. + +This code and the contents of isgps.c are evolved from code by Wolfgang +Rupprecht. Wolfgang's decoder was loosely based on one written by +John Sager in 1999 (in particular the dump function emits a close +descendant of Sager's dump format). Here are John Sager's original +notes: + +The RTCM decoder prints a legible representation of the input data. +The RTCM SC-104 specification is copyrighted, so I cannot +quote it - in fact, I have never read it! Most of the information +used to develop the decoder came from publication ITU-R M.823. +This is a specification of the data transmitted from LF DGPS +beacons in the 300kHz band. M.823 contains most of those parts of +RTCM SC-104 directly relevant to the air interface (there +are one or two annoying and vital omissions!). Information +about the serial interface format was gleaned from studying +the output of a beacon receiver test program made available on +Starlink's website. + +*****************************************************************************/ + +/* + * Structures for interpreting words in an RTCM-104 2.x message (after + * parity checking and removing inversion). Note, these structures + * are overlayed on the raw data in order to decode them into + * bitfields; this will fail horribly if your C compiler ever + * introduces padding between or before bit fields, or between + * 8-bit-aligned bitfields and character arrays. + * + * (In practice, the only class of machines on which this is likely + * to fail are word-aligned architectures without barrel shifters. + * Very few of these are left in 2008.) + * + * The RTCM 2.1 standard is less explicit than it should be about signed-integer + * representations. Two's complement is specified for prc and rrc (msg1wX), + * but not everywhere. + */ + +#define ZCOUNT_SCALE 0.6 /* sec */ +#define PCSMALL 0.02 /* meters */ +#define PCLARGE 0.32 /* meters */ +#define RRSMALL 0.002 /* meters/sec */ +#define RRLARGE 0.032 /* meters/sec */ + +#define MAXPCSMALL (0x7FFF * PCSMALL) /* 16-bits signed */ +#define MAXRRSMALL (0x7F * RRSMALL) /* 8-bits signed */ + +#define XYZ_SCALE 0.01 /* meters */ +#define DXYZ_SCALE 0.1 /* meters */ +#define LA_SCALE (90.0/32767.0) /* degrees */ +#define LO_SCALE (180.0/32767.0) /* degrees */ +#define FREQ_SCALE 0.1 /* kHz */ +#define FREQ_OFFSET 190.0 /* kHz */ +#define CNR_OFFSET 24 /* dB */ +#define TU_SCALE 5 /* minutes */ + +#pragma pack(1) + +#ifndef WORDS_BIGENDIAN /* little-endian, like x86 */ + +struct rtcm2_msg_t { + struct rtcm2_msghw1 { /* header word 1 */ + uint parity:6; + uint refstaid:10; /* reference station ID */ + uint msgtype:6; /* RTCM message type */ + uint preamble:8; /* fixed at 01100110 */ + uint _pad:2; + } w1; + + struct rtcm2_msghw2 { /* header word 2 */ + uint parity:6; + uint stathlth:3; /* station health */ + uint frmlen:5; + uint sqnum:3; + uint zcnt:13; + uint _pad:2; + } w2; + + union { + /* msg 1 - differential gps corrections */ + struct rtcm2_msg1 { + struct b_correction_t { + struct { /* msg 1 word 3 */ + uint parity:6; + int pc1:16; + uint satident1:5; /* satellite ID */ + uint udre1:2; + uint scale1:1; + uint _pad:2; + } w3; + + struct { /* msg 1 word 4 */ + uint parity:6; + uint satident2:5; /* satellite ID */ + uint udre2:2; + uint scale2:1; + uint issuedata1:8; + int rangerate1:8; + uint _pad:2; + } w4; + + struct { /* msg 1 word 5 */ + uint parity:6; + int rangerate2:8; + int pc2:16; + uint _pad:2; + } w5; + + struct { /* msg 1 word 6 */ + uint parity:6; + int pc3_h:8; + uint satident3:5; /* satellite ID */ + uint udre3:2; + uint scale3:1; + uint issuedata2:8; + uint _pad:2; + } w6; + + struct { /* msg 1 word 7 */ + uint parity:6; + uint issuedata3:8; + int rangerate3:8; + uint pc3_l:8; /* NOTE: uint for low byte */ + uint _pad:2; + } w7; + } corrections[(RTCM2_WORDS_MAX - 2) / 5]; + } type1; + + /* msg 3 - reference station parameters */ + struct rtcm2_msg3 { + struct { + uint parity:6; + uint x_h:24; + uint _pad:2; + } w3; + struct { + uint parity:6; + uint y_h:16; + uint x_l:8; + uint _pad:2; + } w4; + struct { + uint parity:6; + uint z_h:8; + uint y_l:16; + uint _pad:2; + } w5; + + struct { + uint parity:6; + uint z_l:24; + uint _pad:2; + } w6; + } type3; + + /* msg 4 - reference station datum */ + struct rtcm2_msg4 { + struct { + uint parity:6; + uint datum_alpha_char2:8; + uint datum_alpha_char1:8; + uint spare:4; + uint dat:1; + uint dgnss:3; + uint _pad:2; + } w3; + struct { + uint parity:6; + uint datum_sub_div_char2:8; + uint datum_sub_div_char1:8; + uint datum_sub_div_char3:8; + uint _pad:2; + } w4; + struct { + uint parity:6; + uint dy_h:8; + uint dx:16; + uint _pad:2; + } w5; + struct { + uint parity:6; + uint dz:24; + uint dy_l:8; + uint _pad:2; + } w6; + } type4; + + /* msg 5 - constellation health */ + struct rtcm2_msg5 { + struct b_health_t { + uint parity:6; + uint unassigned:2; + uint time_unhealthy:4; + uint loss_warn:1; + uint new_nav_data:1; + uint health_enable:1; + uint cn0:5; + uint data_health:3; + uint issue_of_data_link:1; + uint sat_id:5; + uint reserved:1; + uint _pad:2; + } health[MAXHEALTH]; + } type5; + + /* msg 6 - null message */ + + /* msg 7 - beacon almanac */ + struct rtcm2_msg7 { + struct b_station_t { + struct { + uint parity:6; + int lon_h:8; + int lat:16; + uint _pad:2; + } w3; + struct { + uint parity:6; + uint freq_h:6; + uint range:10; + uint lon_l:8; + uint _pad:2; + } w4; + struct { + uint parity:6; + uint encoding:1; + uint sync_type:1; + uint mod_mode:1; + uint bit_rate:3; + /* + * ITU-R M.823-2 page 9 and RTCM-SC104 v2.1 pages + * 4-21 and 4-22 are in conflict over the next two + * field sizes. ITU says 9+3, RTCM says 10+2. + * The latter correctly decodes the USCG station + * id's so I'll use that one here. -wsr + */ + uint station_id:10; + uint health:2; + uint freq_l:6; + uint _pad:2; + } w5; + } almanac[(RTCM2_WORDS_MAX - 2)/3]; + } type7; + + /* msg 16 - text msg */ + struct rtcm2_msg16 { + struct { + uint parity:6; + uint byte3:8; + uint byte2:8; + uint byte1:8; + uint _pad:2; + } txt[RTCM2_WORDS_MAX-2]; + } type16; + + /* unknown message */ + isgps30bits_t rtcm2_msgunk[RTCM2_WORDS_MAX-2]; + } msg_type; +}; + +#endif /* LITTLE_ENDIAN */ + +#ifdef WORDS_BIGENDIAN +/* This struct was generated from the above using invert-bitfields.pl */ +#ifndef S_SPLINT_S /* splint thinks it's a duplicate definition */ + +struct rtcm2_msg_t { + struct rtcm2_msghw1 { /* header word 1 */ + uint _pad:2; + uint preamble:8; /* fixed at 01100110 */ + uint msgtype:6; /* RTCM message type */ + uint refstaid:10; /* reference station ID */ + uint parity:6; + } w1; + + struct rtcm2_msghw2 { /* header word 2 */ + uint _pad:2; + uint zcnt:13; + uint sqnum:3; + uint frmlen:5; + uint stathlth:3; /* station health */ + uint parity:6; + } w2; + + union { + /* msg 1 - differential gps corrections */ + struct rtcm2_msg1 { + struct b_correction_t { + struct { /* msg 1 word 3 */ + uint _pad:2; + uint scale1:1; + uint udre1:2; + uint satident1:5; /* satellite ID */ + int pc1:16; + uint parity:6; + } w3; + + struct { /* msg 1 word 4 */ + uint _pad:2; + int rangerate1:8; + uint issuedata1:8; + uint scale2:1; + uint udre2:2; + uint satident2:5; /* satellite ID */ + uint parity:6; + } w4; + + struct { /* msg 1 word 5 */ + uint _pad:2; + int pc2:16; + int rangerate2:8; + uint parity:6; + } w5; + + struct { /* msg 1 word 6 */ + uint _pad:2; + uint issuedata2:8; + uint scale3:1; + uint udre3:2; + uint satident3:5; /* satellite ID */ + int pc3_h:8; + uint parity:6; + } w6; + + struct { /* msg 1 word 7 */ + uint _pad:2; + uint pc3_l:8; /* NOTE: uint for low byte */ + int rangerate3:8; + uint issuedata3:8; + uint parity:6; + } w7; + } corrections[(RTCM2_WORDS_MAX - 2) / 5]; + } type1; + + /* msg 3 - reference station parameters */ + struct rtcm2_msg3 { + struct { + uint _pad:2; + uint x_h:24; + uint parity:6; + } w3; + struct { + uint _pad:2; + uint x_l:8; + uint y_h:16; + uint parity:6; + } w4; + struct { + uint _pad:2; + uint y_l:16; + uint z_h:8; + uint parity:6; + } w5; + + struct { + uint _pad:2; + uint z_l:24; + uint parity:6; + } w6; + } type3; + + /* msg 4 - reference station datum */ + struct rtcm2_msg4 { + struct { + uint _pad:2; + uint dgnss:3; + uint dat:1; + uint spare:4; + uint datum_alpha_char1:8; + uint datum_alpha_char2:8; + uint parity:6; + } w3; + struct { + uint _pad:2; + uint datum_sub_div_char3:8; + uint datum_sub_div_char1:8; + uint datum_sub_div_char2:8; + uint parity:6; + } w4; + struct { + uint _pad:2; + uint dx:16; + uint dy_h:8; + uint parity:6; + } w5; + struct { + uint _pad:2; + uint dy_l:8; + uint dz:24; + uint parity:6; + } w6; + } type4; + + /* msg 5 - constellation health */ + struct rtcm2_msg5 { + struct b_health_t { + uint _pad:2; + uint reserved:1; + uint sat_id:5; + uint issue_of_data_link:1; + uint data_health:3; + uint cn0:5; + uint health_enable:1; + uint new_nav_data:1; + uint loss_warn:1; + uint time_unhealthy:4; + uint unassigned:2; + uint parity:6; + } health[MAXHEALTH]; + } type5; + + /* msg 6 - null message */ + + /* msg 7 - beacon almanac */ + struct rtcm2_msg7 { + struct b_station_t { + struct { + uint _pad:2; + int lat:16; + int lon_h:8; + uint parity:6; + } w3; + struct { + uint _pad:2; + uint lon_l:8; + uint range:10; + uint freq_h:6; + uint parity:6; + } w4; + struct { + uint _pad:2; + uint freq_l:6; + uint health:2; + uint station_id:10; + /* see comments in LE struct above. */ + uint bit_rate:3; + uint mod_mode:1; + uint sync_type:1; + uint encoding:1; + uint parity:6; + } w5; + } almanac[(RTCM2_WORDS_MAX - 2)/3]; + } type7; + + /* msg 16 - text msg */ + struct rtcm2_msg16 { + struct { + uint _pad:2; + uint byte1:8; + uint byte2:8; + uint byte3:8; + uint parity:6; + } txt[RTCM2_WORDS_MAX-2]; + } type16; + + /* unknown message */ + isgps30bits_t rtcm2_msgunk[RTCM2_WORDS_MAX-2]; + } msg_type; +}; + +#endif /* S_SPLINT_S */ +#endif /* BIG ENDIAN */ +#endif /* _GPSD_RTCM2_H_ */ diff --git a/driver_rtcm3.c b/driver_rtcm3.c new file mode 100644 index 00000000..807816de --- /dev/null +++ b/driver_rtcm3.c @@ -0,0 +1,473 @@ +/***************************************************************************** + +This is a decoder for RTCM-104 3.x, a serial protocol used for +broadcasting pseudorange corrections from differential-GPS reference +stations. The applicable specification is RTCM 10403.1: RTCM Paper +177-2006-SC104-STD. This obsolesces the esrlier RTCM-104 2.x +specifications. The specification document is proprietary; ordering +instructions are accessible from +under "Publications". + +Unike the RTCM 2.x protocol, RTCM3.x does not use the strange +sliding-bit-window IS-GPS-200 protocol as a transport layer, but is a +self-contained byte-oriented packet protocol. Packet recognition is +handled in the GPSD packet-getter state machine; this code is +concerned with unpacking the packets into well-behaved C structures, +coping with odd field lengths and fields that may overlap byte +boudaries. These report structures live in gps.h. + +Note that the unpacking this module does is probably useful only for +RTCM reporting and diagnostic tools. It is not necessary when +passing RTCM corrections to a GPS, which normally should just be +passed an entire correction packet for processing by their internal +firmware. + +*****************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include /* for ntohl(3) and friends */ + +#include "gpsd_config.h" +#include "gpsd.h" +#include "bits.h" + +#ifdef RTCM104V3_ENABLE + +/* scaling constants for RTCM3 real number types */ +#define PSEUDORANGE_RESOLUTION 0.2 /* DF011 */ +#define PSEUDORANGE_DIFF_RESOLUTION 0.0005 /* DF012 */ +#define CARRIER_NOISE_RATIO_UNITS 0.25 /* DF015 */ +#define ANTENNA_POSITION_RESOLUTION 0.0001 /* DF025-027 */ +#define ANTENNA_DEGREE_RESOLUTION 25e-6 /* DF062 */ +#define GPS_EPOCH_TIME_RESOLUTION 0.1 /* DF065 */ +#define PHASE_CORRECTION_RESOLUTION 0.5 /* DF069-070 */ + +/* Other magic values */ +#define INVALID_PSEUDORANGE 0x80000 /* DF012 */ + +/*@ -type @*/ /* re-enable when we're ready to take this live */ + +void rtcm3_unpack(/*@out@*/struct rtcm3_t *rtcm, char *buf) +/* break out the raw bits into the scaled report-structure fields */ +{ + unsigned int bitcount = 0; + unsigned i; + signed long temp; + + /*@ -evalorder -sefparams @*/ +#define ugrab(width) (bitcount += width, ubits(buf, bitcount-width, width)) +#define sgrab(width) (bitcount += width, sbits(buf, bitcount-width, width)) + assert(ugrab(8) == 0xD3); + assert(ugrab(6) == 0x00); + + rtcm->length = (uint)ugrab(10); + rtcm->type = (uint)ugrab(12); + + switch(rtcm->type) { + case 1001: + rtcm->rtcmtypes.rtcm3_1001.header.msgnum = (uint)ugrab(12); + rtcm->rtcmtypes.rtcm3_1001.header.station_id = (uint)ugrab(12); + rtcm->rtcmtypes.rtcm3_1001.header.tow = (time_t)ugrab(30); + rtcm->rtcmtypes.rtcm3_1001.header.sync = (bool)ugrab(1); + rtcm->rtcmtypes.rtcm3_1001.header.satcount = (ushort)ugrab(5); + rtcm->rtcmtypes.rtcm3_1001.header.smoothing = (bool)ugrab(1); + rtcm->rtcmtypes.rtcm3_1001.header.interval = (ushort)ugrab(3); + for (i = 0; i < rtcm->rtcmtypes.rtcm3_1001.header.satcount; i++) { + rtcm->rtcmtypes.rtcm3_1001.rtk_data[i].ident = (ushort)ugrab(6); + rtcm->rtcmtypes.rtcm3_1001.rtk_data[i].L1.indicator = (unsigned char)ugrab(1); + temp = (long)sgrab(24); + if (temp == INVALID_PSEUDORANGE) + rtcm->rtcmtypes.rtcm3_1001.rtk_data[i].L1.pseudorange = 0; + else + rtcm->rtcmtypes.rtcm3_1001.rtk_data[i].L1.pseudorange = temp * PSEUDORANGE_RESOLUTION; + if (temp == INVALID_PSEUDORANGE) + rtcm->rtcmtypes.rtcm3_1001.rtk_data[i].L1.rangediff = 0; + else + rtcm->rtcmtypes.rtcm3_1001.rtk_data[i].L1.rangediff = temp * PSEUDORANGE_DIFF_RESOLUTION; + rtcm->rtcmtypes.rtcm3_1001.rtk_data[i].L1.locktime = (unsigned char)sgrab(7); + } + break; + + case 1002: + rtcm->rtcmtypes.rtcm3_1002.header.msgnum = (uint)ugrab(12); + rtcm->rtcmtypes.rtcm3_1002.header.station_id = (uint)ugrab(12); + rtcm->rtcmtypes.rtcm3_1002.header.tow = (time_t)ugrab(30); + rtcm->rtcmtypes.rtcm3_1002.header.sync = (bool)ugrab(1); + rtcm->rtcmtypes.rtcm3_1002.header.satcount = (ushort)ugrab(5); + rtcm->rtcmtypes.rtcm3_1002.header.smoothing = (bool)ugrab(1); + rtcm->rtcmtypes.rtcm3_1002.header.interval = (ushort)ugrab(3); + for (i = 0; i < rtcm->rtcmtypes.rtcm3_1002.header.satcount; i++) { + rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].ident = (ushort)ugrab(6); + rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.indicator = (unsigned char)ugrab(1); + temp = (long)sgrab(24); + if (temp == INVALID_PSEUDORANGE) + rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.pseudorange = 0; + else + rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.pseudorange = temp * PSEUDORANGE_RESOLUTION; + if (temp == INVALID_PSEUDORANGE) + rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.rangediff = 0; + else + rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.rangediff = temp * PSEUDORANGE_DIFF_RESOLUTION; + rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.locktime = (unsigned char)sgrab(7); + rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.ambiguity = (bool)ugrab(8); + rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.CNR = (bool)ugrab(8) * CARRIER_NOISE_RATIO_UNITS; + } + break; + + case 1003: + rtcm->rtcmtypes.rtcm3_1003.header.msgnum = (uint)ugrab(12); + rtcm->rtcmtypes.rtcm3_1003.header.station_id = (uint)ugrab(12); + rtcm->rtcmtypes.rtcm3_1003.header.tow = (time_t)ugrab(30); + rtcm->rtcmtypes.rtcm3_1003.header.sync = (bool)ugrab(1); + rtcm->rtcmtypes.rtcm3_1003.header.satcount = (ushort)ugrab(5); + rtcm->rtcmtypes.rtcm3_1003.header.smoothing = (bool)ugrab(1); + rtcm->rtcmtypes.rtcm3_1003.header.interval = (ushort)ugrab(3); + for (i = 0; i < rtcm->rtcmtypes.rtcm3_1003.header.satcount; i++) { + rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].ident = (ushort)ugrab(6); + rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L1.indicator = (unsigned char)ugrab(1); + temp = (long)sgrab(24); + if (temp == INVALID_PSEUDORANGE) + rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L1.pseudorange = 0; + else + rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L1.pseudorange = temp * PSEUDORANGE_RESOLUTION; + if (temp == INVALID_PSEUDORANGE) + rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L1.rangediff = 0; + else + rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L1.rangediff = temp * PSEUDORANGE_DIFF_RESOLUTION; + rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L1.locktime = (unsigned char)sgrab(7); + rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L2.indicator = (unsigned char)ugrab(2); + temp = (long)sgrab(24); + if (temp == INVALID_PSEUDORANGE) + rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L2.pseudorange = 0; + else + rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L2.pseudorange = temp * PSEUDORANGE_RESOLUTION; + if (temp == INVALID_PSEUDORANGE) + rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L2.rangediff = 0; + else + rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L2.rangediff = temp * PSEUDORANGE_DIFF_RESOLUTION; + rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L2.locktime = (unsigned char)sgrab(7); + } + break; + + case 1004: + rtcm->rtcmtypes.rtcm3_1004.header.msgnum = (uint)ugrab(12); + rtcm->rtcmtypes.rtcm3_1004.header.station_id = (uint)ugrab(12); + rtcm->rtcmtypes.rtcm3_1004.header.tow = (time_t)ugrab(30); + rtcm->rtcmtypes.rtcm3_1004.header.sync = (bool)ugrab(1); + rtcm->rtcmtypes.rtcm3_1004.header.satcount = (ushort)ugrab(5); + rtcm->rtcmtypes.rtcm3_1004.header.smoothing = (bool)ugrab(1); + rtcm->rtcmtypes.rtcm3_1004.header.interval = (ushort)ugrab(3); + for (i = 0; i < rtcm->rtcmtypes.rtcm3_1004.header.satcount; i++) { + rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].ident = (ushort)ugrab(6); + rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L1.indicator = (bool)ugrab(1); + temp = (long)sgrab(24); + if (temp == INVALID_PSEUDORANGE) + rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L1.pseudorange = 0; + else + rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L1.pseudorange = temp * PSEUDORANGE_RESOLUTION; + if (temp == INVALID_PSEUDORANGE) + rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L1.rangediff = 0; + else + rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L1.rangediff = temp * PSEUDORANGE_DIFF_RESOLUTION; + rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L1.locktime = (unsigned char)sgrab(7); + rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L1.ambiguity = (bool)ugrab(8); + rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L1.CNR = (bool)ugrab(8) * CARRIER_NOISE_RATIO_UNITS; + rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.indicator = (unsigned char)ugrab(2); + temp = (long)sgrab(24); + if (temp == INVALID_PSEUDORANGE) + rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.pseudorange = 0; + else + rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.pseudorange = temp * PSEUDORANGE_RESOLUTION; + if (temp == INVALID_PSEUDORANGE) + rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.rangediff = 0; + else + rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.rangediff = temp * PSEUDORANGE_DIFF_RESOLUTION; + rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.locktime = (unsigned char)sgrab(7); + rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.ambiguity = (bool)ugrab(8); + rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.CNR = (bool)ugrab(8) * CARRIER_NOISE_RATIO_UNITS; + } + break; + + case 1005: + rtcm->rtcmtypes.rtcm3_1005.station_id = (unsigned short)ugrab(12); + ugrab(6); /* reserved */ + if ((bool)ugrab(1)) + rtcm->rtcmtypes.rtcm3_1005.system = gps; + else if ((bool)ugrab(1)) + rtcm->rtcmtypes.rtcm3_1005.system = glonass; + else if ((bool)ugrab(1)) + rtcm->rtcmtypes.rtcm3_1005.system = galileo; + rtcm->rtcmtypes.rtcm3_1005.reference_station = (bool)ugrab(1); + rtcm->rtcmtypes.rtcm3_1005.ecef_x = sgrab(38) * ANTENNA_POSITION_RESOLUTION; + rtcm->rtcmtypes.rtcm3_1005.single_receiver = ugrab(1); + ugrab(1); + rtcm->rtcmtypes.rtcm3_1005.ecef_y = sgrab(38) * ANTENNA_POSITION_RESOLUTION; + ugrab(2); + rtcm->rtcmtypes.rtcm3_1005.ecef_z = sgrab(38) * ANTENNA_POSITION_RESOLUTION; + break; + + case 1006: + rtcm->rtcmtypes.rtcm3_1006.station_id = (unsigned short)ugrab(12); + ugrab(6); /* reserved */ + if ((bool)ugrab(1)) + rtcm->rtcmtypes.rtcm3_1006.system = gps; + else if ((bool)ugrab(1)) + rtcm->rtcmtypes.rtcm3_1006.system = glonass; + else if ((bool)ugrab(1)) + rtcm->rtcmtypes.rtcm3_1006.system = galileo; + rtcm->rtcmtypes.rtcm3_1006.reference_station = (bool)ugrab(1); + rtcm->rtcmtypes.rtcm3_1006.ecef_x = sgrab(38) * ANTENNA_POSITION_RESOLUTION; + rtcm->rtcmtypes.rtcm3_1006.single_receiver = ugrab(1); + ugrab(1); + rtcm->rtcmtypes.rtcm3_1006.ecef_y = sgrab(38) * ANTENNA_POSITION_RESOLUTION; + ugrab(2); + rtcm->rtcmtypes.rtcm3_1006.ecef_z = sgrab(38) * ANTENNA_POSITION_RESOLUTION; + rtcm->rtcmtypes.rtcm3_1006.height = ugrab(16) * ANTENNA_POSITION_RESOLUTION; + break; + + case 1007: + break; + + case 1008: + break; + + case 1009: + break; + + case 1010: + break; + + case 1011: + break; + + case 1012: + break; + + case 1013: + break; + + case 1014: + break; + + case 1015: + break; + + case 1016: + break; + + case 1017: + break; + + case 1018: + break; + + case 1019: + break; + + case 1020: + break; + + case 1029: + break; + } +#undef sgrab +#undef ugrab + /*@ +evalorder +sefparams @*/ +} + +void rtcm3_dump(struct rtcm3_t *rtcm, FILE *fp) +/* dump the contents of a parsed RTCM104 message */ +{ + int i; + + char *systems[] = {"GPS", "Glonass", "Galileo", "unknown"}; + + (void)fprintf(fp, "%u (%u):\n", rtcm->type, rtcm->length); + +#define BOOL(c) (c!=0 ? 't' : 'f') +#define CODE(x) (unsigned int)(x) +#define INT(x) (unsigned int)(x) + switch(rtcm->type) { + case 1001: + (void)fprintf(fp, + " #%u station_id=%u, tow=%d sync=%c smoothing=%c interval=%u satcount=%u", + rtcm->rtcmtypes.rtcm3_1001.header.msgnum, + rtcm->rtcmtypes.rtcm3_1001.header.station_id, + (int)rtcm->rtcmtypes.rtcm3_1001.header.tow, + BOOL(rtcm->rtcmtypes.rtcm3_1001.header.sync), + BOOL(rtcm->rtcmtypes.rtcm3_1001.header.smoothing), + rtcm->rtcmtypes.rtcm3_1001.header.interval, + rtcm->rtcmtypes.rtcm3_1001.header.satcount); + for (i = 0; i < rtcm->rtcmtypes.rtcm3_1001.header.satcount; i++) { + (void)fprintf(fp, + " ident=%u\n L1: ind=%u prange=%8.1f delta=%6.4f lockt=%u\n", + rtcm->rtcmtypes.rtcm3_1001.rtk_data[i].ident, + CODE(rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L1.indicator), + rtcm->rtcmtypes.rtcm3_1001.rtk_data[i].L1.pseudorange, + rtcm->rtcmtypes.rtcm3_1001.rtk_data[i].L1.rangediff, + INT(rtcm->rtcmtypes.rtcm3_1001.rtk_data[i].L1.locktime)); + } + break; + + case 1002: + (void)fprintf(fp, + " #%u station_id=%u, tow=%d sync=%c smoothing=%c interval=%u satcount=%u", + rtcm->rtcmtypes.rtcm3_1002.header.msgnum, + rtcm->rtcmtypes.rtcm3_1002.header.station_id, + (int)rtcm->rtcmtypes.rtcm3_1002.header.tow, + BOOL(rtcm->rtcmtypes.rtcm3_1002.header.sync), + BOOL(rtcm->rtcmtypes.rtcm3_1002.header.smoothing), + rtcm->rtcmtypes.rtcm3_1002.header.interval, + rtcm->rtcmtypes.rtcm3_1002.header.satcount); + for (i = 0; i < rtcm->rtcmtypes.rtcm3_1002.header.satcount; i++) { + (void)fprintf(fp, + " ident=%u\n L1: ind=%u prange=%8.1f delta=%6.4f lockt=%u amb=%u CNR=%.2f\n", + rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].ident, + CODE(rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L1.indicator), + rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.pseudorange, + rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.rangediff, + INT(rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.locktime), + INT(rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.ambiguity), + rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.CNR); + } + break; + + case 1003: + (void)fprintf(fp, + " #%u station_id=%u, tow=%d sync=%c smoothing=%c interval=%u satcount=%u", + rtcm->rtcmtypes.rtcm3_1003.header.msgnum, + rtcm->rtcmtypes.rtcm3_1003.header.station_id, + (int)rtcm->rtcmtypes.rtcm3_1003.header.tow, + BOOL(rtcm->rtcmtypes.rtcm3_1003.header.sync), + BOOL(rtcm->rtcmtypes.rtcm3_1003.header.smoothing), + rtcm->rtcmtypes.rtcm3_1003.header.interval, + rtcm->rtcmtypes.rtcm3_1003.header.satcount); + for (i = 0; i < rtcm->rtcmtypes.rtcm3_1003.header.satcount; i++) { + (void)fprintf(fp, + " ident=%u\n L1: ind=%u prange=%8.1f delta=%6.4f lockt=%u\n L2: ind=%u prange=%8.1f delta=%6.4f lockt=%u\n", + rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].ident, + CODE(rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L1.indicator), + rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L1.pseudorange, + rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L1.rangediff, + INT(rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L1.locktime), + CODE(rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L2.indicator), + rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L2.pseudorange, + rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L2.rangediff, + INT(rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L2.locktime)); + } + break; + + case 1004: + (void)fprintf(fp, + " #%u station_id=%u, tow=%d sync=%c smoothing=%c interval=%u satcount=%u\n", + rtcm->rtcmtypes.rtcm3_1004.header.msgnum, + rtcm->rtcmtypes.rtcm3_1004.header.station_id, + (int)rtcm->rtcmtypes.rtcm3_1004.header.tow, + BOOL(rtcm->rtcmtypes.rtcm3_1004.header.sync), + BOOL(rtcm->rtcmtypes.rtcm3_1004.header.smoothing), + rtcm->rtcmtypes.rtcm3_1004.header.interval, + rtcm->rtcmtypes.rtcm3_1004.header.satcount); + for (i = 0; i < rtcm->rtcmtypes.rtcm3_1004.header.satcount; i++) { + (void)fprintf(fp, + " ident=%u\n L1: ind=%u prange=%8.1f delta=%6.4f lockt=%u amb=%u CNR=%.2f\n L2: ind=%u prange=%8.1f delta=%6.4f lockt=%u amb=%u CNR=%.2f\n", + rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].ident, + CODE(rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L1.indicator), + rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L1.pseudorange, + rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L1.rangediff, + INT(rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L1.locktime), + INT(rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.ambiguity), + rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.CNR, + CODE(rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.indicator), + rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.pseudorange, + rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.rangediff, + INT(rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.locktime), + INT(rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.ambiguity), + rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.CNR); + } + break; + + case 1005: + (void)fprintf(fp, + " station_id=%u, %s refstation=%c sro=%c x=%f y=%f z=%f\n", + rtcm->rtcmtypes.rtcm3_1005.station_id, + systems[rtcm->rtcmtypes.rtcm3_1005.system], + BOOL(rtcm->rtcmtypes.rtcm3_1005.reference_station), + BOOL(rtcm->rtcmtypes.rtcm3_1005.single_receiver), + rtcm->rtcmtypes.rtcm3_1005.ecef_x, + rtcm->rtcmtypes.rtcm3_1005.ecef_y, + rtcm->rtcmtypes.rtcm3_1005.ecef_z); + break; + + case 1006: + (void)fprintf(fp, + " station_id=%u, %s refstation=%c sro=%c x=%f y=%f z=%f a=%f\n", + rtcm->rtcmtypes.rtcm3_1006.station_id, + systems[rtcm->rtcmtypes.rtcm3_1006.system], + BOOL(rtcm->rtcmtypes.rtcm3_1006.reference_station), + BOOL(rtcm->rtcmtypes.rtcm3_1006.single_receiver), + rtcm->rtcmtypes.rtcm3_1006.ecef_x, + rtcm->rtcmtypes.rtcm3_1006.ecef_y, + rtcm->rtcmtypes.rtcm3_1006.ecef_z, + rtcm->rtcmtypes.rtcm3_1006.height); + break; + + case 1007: + break; + + case 1008: + break; + + case 1009: + break; + + case 1010: + break; + + case 1011: + break; + + case 1012: + break; + + case 1013: + break; + + case 1014: + break; + + case 1015: + break; + + case 1016: + break; + + case 1017: + break; + + case 1018: + break; + + case 1019: + break; + + case 1020: + break; + + case 1029: + (void)fprintf(fp, " Unknown content\n"); + break; + } +#undef CODE +#undef BOOL +#undef INT +} + +/*@ +type @*/ + +#endif /* RTCM104V3_ENABLE */ diff --git a/driver_sirf.c b/driver_sirf.c new file mode 100644 index 00000000..dfd184c8 --- /dev/null +++ b/driver_sirf.c @@ -0,0 +1,1037 @@ +/* $Id$ */ +/* + * This is the gpsd driver for SiRF GPSes operating in binary mode. + * It also handles uBlox, a SiRF derivative. + * + * The advantage: Reports climb/sink rate (raw-mode clients won't see this). + * The disadvantages: Doesn't return PDOP or VDOP, just HDOP. + * + * Chris Kuethe, our SiRF expert, tells us: + * + * "I don't see any indication in any of my material that PDOP, GDOP + * or VDOP are output. There are quantities called Estimated + * {Horizontal Position, Vertical Position, Time, Horizonal Velocity} + * Error, but those are apparently only valid when SiRFDRive is + * active." + * + * "(SiRFdrive is their Dead Reckoning augmented firmware. It + * allows you to feed odometer ticks, gyro and possibly + * accelerometer inputs to the chip to allow it to continue + * to navigate in the absence of satellite information, and + * to improve fixes when you do have satellites.)" + * + * "[When we need RINEX data, we can get it from] SiRF Message #5. + * If it's no longer implemented on your receiver, messages + * 7, 28, 29 and 30 will give you the same information." + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "gpsd_config.h" +#include "gpsd.h" +#include "bits.h" +#if defined(SIRF_ENABLE) && defined(BINARY_ENABLE) + +#define HI(n) ((n) >> 8) +#define LO(n) ((n) & 0xff) + +#ifdef ALLOW_RECONFIGURE +/*@ +charint @*/ +static unsigned char enablesubframe[] = {0xa0, 0xa2, 0x00, 0x19, + 0x80, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x0C, + 0x10, + 0x00, 0x00, 0xb0, 0xb3}; + +static unsigned char disablesubframe[] = {0xa0, 0xa2, 0x00, 0x19, + 0x80, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x0C, + 0x00, + 0x00, 0x00, 0xb0, 0xb3}; + +static unsigned char modecontrol[] = {0xa0, 0xa2, 0x00, 0x0e, + 0x88, + 0x00, 0x00, /* pad bytes */ + 0x00, /* degraded mode off */ + 0x00, 0x00, /* pad bytes */ + 0x00, 0x00, /* altitude */ + 0x00, /* altitude hold auto */ + 0x00, /* use last computed alt */ + 0x00, /* reserved */ + 0x00, /* disable degraded mode */ + 0x00, /* disable dead reckoning */ + 0x01, /* enable track smoothing */ + 0x00, 0x00, 0xb0, 0xb3}; + +static unsigned char enablemid52[] = { + 0xa0, 0xa2, 0x00, 0x08, + 0xa6, 0x00, 0x34, 0x01, 0x00, 0x00, 0x00, 0x00, + 0x00, 0xdb, 0xb0, 0xb3}; +/*@ -charint @*/ +#endif /* ALLOW_RECONFIGURE */ + + +static gps_mask_t sirf_msg_debug(unsigned char *, size_t ); +static gps_mask_t sirf_msg_errors(unsigned char *, size_t ); + +static gps_mask_t sirf_msg_swversion(struct gps_device_t *, unsigned char *, size_t ); +static gps_mask_t sirf_msg_navdata(struct gps_device_t *, unsigned char *, size_t ); +static gps_mask_t sirf_msg_svinfo(struct gps_device_t *, unsigned char *, size_t ); +static gps_mask_t sirf_msg_navsol(struct gps_device_t *, unsigned char *, size_t ); +static gps_mask_t sirf_msg_geodetic(struct gps_device_t *, unsigned char *, size_t ); +static gps_mask_t sirf_msg_sysparam(struct gps_device_t *, unsigned char *, size_t ); +static gps_mask_t sirf_msg_ublox(struct gps_device_t *, unsigned char *, size_t ); +static gps_mask_t sirf_msg_ppstime(struct gps_device_t *, unsigned char *, size_t ); + + +bool sirf_write(int fd, unsigned char *msg) { + unsigned int crc; + size_t i, len; + bool ok; + + len = (size_t)((msg[2] << 8) | msg[3]); + + /* calculate CRC */ + crc = 0; + for (i = 0; i < len; i++) + crc += (int)msg[4 + i]; + crc &= 0x7fff; + + /* enter CRC after payload */ + msg[len + 4] = (unsigned char)((crc & 0xff00) >> 8); + msg[len + 5] = (unsigned char)( crc & 0x00ff); + + gpsd_report(LOG_IO, "Writing SiRF control type %02x:%s\n", msg[4], + gpsd_hexdump_wrapper(msg, len+8, LOG_IO)); + ok = (write(fd, msg, len+8) == (ssize_t)(len+8)); + (void)tcdrain(fd); + return(ok); +} + +static ssize_t sirf_control_send(struct gps_device_t *session, char *msg, size_t len) { + /*@ +charint +matchanyintegral -initallelements -mayaliasunique @*/ + session->msgbuf[0] = 0xa0; + session->msgbuf[1] = 0xa2; + session->msgbuf[2] = (len >> 8) & 0xff; + session->msgbuf[3] = len & 0xff; + memcpy(session->msgbuf+4, msg, len); + session->msgbuf[len + 6] = 0xb0; + session->msgbuf[len + 7] = 0xb3; + session->msgbuflen = len + 8; + + return sirf_write(session->gpsdata.gps_fd, + (unsigned char *)session->msgbuf) ? session->msgbuflen : -1; + /*@ -charint -matchanyintegral +initallelements +mayaliasunique @*/ +} + +static bool sirf_speed(int ttyfd, speed_t speed, char parity, int stopbits) +/* change speed in binary mode */ +{ + /*@ +charint @*/ + static unsigned char msg[] = {0xa0, 0xa2, 0x00, 0x09, + 0x86, /* byte 4: main serial port */ + 0x00, 0x00, 0x12, 0xc0, /* bytes 5-8: 4800 bps */ + 0x08, /* byte 9: 8 data bits */ + 0x01, /* byte 10: 1 stop bit */ + 0x00, /* byte 11: no parity */ + 0x00, /* byte 12: reserved pad */ + 0x00, 0x00, 0xb0, 0xb3}; + /*@ -charint @*/ + + switch (parity) { + case 'E': + case 2: + parity = (char)2; + break; + case 'O': + case 1: + parity = (char)1; + break; + case 'N': + case 0: + default: + parity = (char)0; + break; + } + msg[7] = (unsigned char)HI(speed); + msg[8] = (unsigned char)LO(speed); + msg[10] = (unsigned char)stopbits; + msg[11] = (unsigned char)parity; + return (sirf_write(ttyfd, msg)); +} + +static bool sirf_to_nmea(int ttyfd, speed_t speed) +/* switch from binary to NMEA at specified baud */ +{ + /*@ +charint @*/ + static unsigned char msg[] = {0xa0, 0xa2, 0x00, 0x18, + 0x81, 0x02, + 0x01, 0x01, /* GGA */ + 0x00, 0x00, /* suppress GLL */ + 0x01, 0x01, /* GSA */ + 0x05, 0x01, /* GSV */ + 0x01, 0x01, /* RMC */ + 0x00, 0x00, /* suppress VTG */ + 0x00, 0x01, 0x00, 0x01, + 0x00, 0x01, 0x00, 0x01, + 0x12, 0xc0, /* 4800 bps */ + 0x00, 0x00, 0xb0, 0xb3}; + /*@ -charint @*/ + + msg[26] = (unsigned char)HI(speed); + msg[27] = (unsigned char)LO(speed); + return (sirf_write(ttyfd, msg)); +} + +static void sirfbin_mode(struct gps_device_t *session, int mode) +{ + if (mode == MODE_NMEA) { + (void)sirf_to_nmea(session->gpsdata.gps_fd,session->gpsdata.baudrate); + } else { + session->back_to_nmea = false; + } +} + +static ssize_t sirf_get(struct gps_device_t *session) +{ + ssize_t len = generic_get(session); + + if (session->packet.type == SIRF_PACKET) { + session->gpsdata.driver_mode = MODE_BINARY; + } else if (session->packet.type == NMEA_PACKET) { + session->gpsdata.driver_mode = MODE_NMEA; + (void)gpsd_switch_driver(session, "Generic NMEA"); + } else { + /* should never happen */ + gpsd_report(LOG_PROG, "Unexpected packet type %d\n", + session->packet.type); + (void)gpsd_switch_driver(session, "Generic NMEA"); + } + + return len; +} + +static gps_mask_t sirf_msg_debug(unsigned char *buf, size_t len) +{ + char msgbuf[MAX_PACKET_LENGTH*3 + 2]; + int i; + + bzero(msgbuf, (int)sizeof(msgbuf)); + + /*@ +charint @*/ + if (0xe1 == buf[0]) { /* Development statistics messages */ + for (i = 2; i < (int)len; i++) + (void)snprintf(msgbuf+strlen(msgbuf), + sizeof(msgbuf)-strlen(msgbuf), + "%c", buf[i]^0xff); + gpsd_report(LOG_PROG, "DEV 0xe1: %s\n", msgbuf); + } else if (0xff == (unsigned char)buf[0]) { /* Debug messages */ + for (i = 1; i < (int)len; i++) + if (isprint(buf[i])) + (void)snprintf(msgbuf+strlen(msgbuf), + sizeof(msgbuf)-strlen(msgbuf), + "%c", buf[i]); + else + (void)snprintf(msgbuf+strlen(msgbuf), + sizeof(msgbuf)-strlen(msgbuf), + "\\x%02x", (unsigned int)buf[i]); + gpsd_report(LOG_PROG, "DBG 0xff: %s\n", msgbuf); + } + /*@ -charint @*/ + return 0; +} + +static gps_mask_t sirf_msg_errors(unsigned char *buf, size_t len UNUSED) +{ + switch (getbeuw(buf, 1)) { + case 2: + gpsd_report(LOG_PROG, "EID 0x0a type 2: Subframe %u error on PRN %u\n", getbeul(buf, 9), getbeul(buf, 5)); + break; + + case 4107: + gpsd_report(LOG_PROG, "EID 0x0a type 4107: neither KF nor LSQ fix.\n"); + break; + + default: + gpsd_report(LOG_PROG, "EID 0x0a: Error ID type %d\n", getbeuw(buf, 1)); + break; + } + return 0; +} + +static gps_mask_t sirf_msg_swversion(struct gps_device_t *session, unsigned char *buf, size_t len) +{ + double fv; + + if (len < 20) + return 0; + + gpsd_report(LOG_INF, "FV 0x06: Firmware version: %s\n", buf+1); + (void)strlcpy(session->subtype, (char *)buf+1, sizeof(session->subtype)); + fv = atof((char *)(buf+1)); + if (fv < 231) { + session->driver.sirf.driverstate |= SIRF_LT_231; + if (fv > 200) + sirfbin_mode(session, 0); + } else if (fv < 232) { + session->driver.sirf.driverstate |= SIRF_EQ_231; + } else { +#ifdef ALLOW_RECONFIGURE + gpsd_report(LOG_PROG, "Enabling PPS message...\n"); + (void)sirf_write(session->gpsdata.gps_fd, enablemid52); +#endif /* ALLOW_RECONFIGURE */ + session->driver.sirf.driverstate |= SIRF_GE_232; + session->context->valid |= LEAP_SECOND_VALID; + } + if (strstr((char *)(buf+1), "ES")) + gpsd_report(LOG_INF, "Firmware has XTrac capability\n"); + gpsd_report(LOG_PROG, "Driver state flags are: %0x\n", session->driver.sirf.driverstate); +#ifdef NTPSHM_ENABLE + session->driver.sirf.time_seen = 0; +#endif /* NTPSHM_ENABLE */ +#ifdef ALLOW_RECONFIGURE + if (session->gpsdata.baudrate >= 38400){ + gpsd_report(LOG_PROG, "Enabling subframe transmission...\n"); + (void)sirf_write(session->gpsdata.gps_fd, enablesubframe); + } +#endif /* ALLOW_RECONFIGURE */ + return DEVICEID_SET; +} + +static gps_mask_t sirf_msg_navdata(struct gps_device_t *session, unsigned char *buf, size_t len) +{ + unsigned int words[10]; + + if (len != 43) + return 0; + + //unsigned int chan = (unsigned int)getub(buf, 1); + //unsigned int svid = (unsigned int)getub(buf, 2); + words[0] = (unsigned int)getbeul(buf, 3); + words[1] = (unsigned int)getbeul(buf, 7); + words[2] = (unsigned int)getbeul(buf, 11); + words[3] = (unsigned int)getbeul(buf, 15); + words[4] = (unsigned int)getbeul(buf, 19); + words[5] = (unsigned int)getbeul(buf, 23); + words[6] = (unsigned int)getbeul(buf, 27); + words[7] = (unsigned int)getbeul(buf, 31); + words[8] = (unsigned int)getbeul(buf, 35); + words[9] = (unsigned int)getbeul(buf, 39); + gpsd_report(LOG_PROG, "50B 0x08\n"); + gpsd_interpret_subframe(session, words); + +#ifdef ALLOW_RECONFIGURE + if (session->gpsdata.baudrate < 38400){ + gpsd_report(LOG_PROG, "Disabling subframe transmission...\n"); + (void)sirf_write(session->gpsdata.gps_fd, disablesubframe); + } +#endif /* ALLOW_RECONFIGURE */ + return 0; +} + +static gps_mask_t sirf_msg_svinfo(struct gps_device_t *session, unsigned char *buf, size_t len) +{ + int st, i, j, cn; + + if (len != 188) + return 0; + + gpsd_zero_satellites(&session->gpsdata); + session->gpsdata.sentence_time + = gpstime_to_unix(getbesw(buf, 1), getbeul(buf, 3)*1e-2) - session->context->leap_seconds; + for (i = st = 0; i < SIRF_CHANNELS; i++) { + int off = 8 + 15 * i; + bool good; + session->gpsdata.PRN[st] = (int)getub(buf, off); + session->gpsdata.azimuth[st] = (int)(((unsigned)getub(buf, off+1)*3)/2.0); + session->gpsdata.elevation[st] = (int)((unsigned)getub(buf, off+2)/2.0); + cn = 0; + for (j = 0; j < 10; j++) + cn += (int)getub(buf, off+5+j); + + session->gpsdata.ss[st] = cn/10; + good = session->gpsdata.PRN[st]!=0 && + session->gpsdata.azimuth[st]!=0 && + session->gpsdata.elevation[st]!=0; +#ifdef __UNUSED__ + gpsd_report(LOG_PROG, "PRN=%2d El=%3.2f Az=%3.2f ss=%3d stat=%04x %c\n", + getub(buf, off), + getub(buf, off+2)/2.0, + (getub(buf, off+1)*3)/2.0, + cn/10, + getbeuw(buf, off+3), + good ? '*' : ' '); +#endif /* UNUSED */ + if (good!=0) + st += 1; + } + session->gpsdata.satellites = st; +#ifdef NTPSHM_ENABLE + if (st > 3) { + if ((session->driver.sirf.time_seen & TIME_SEEN_GPS_1)==0) + gpsd_report(LOG_PROG, "valid time in message 0x04, seen=0x%02x\n", + session->driver.sirf.time_seen); + session->driver.sirf.time_seen |= TIME_SEEN_GPS_1; + if (session->context->enable_ntpshm && IS_HIGHEST_BIT(session->driver.sirf.time_seen,TIME_SEEN_GPS_1)) + (void)ntpshm_put(session,session->gpsdata.sentence_time+0.8); + } +#endif /* NTPSHM_ENABLE */ + /* + * The freaking brain-dead SiRF chip doesn't obey its own + * rate-control command for 04, at least at firmware rev. 231, + * so we have to do our own rate-limiting here... + */ + gpsd_report(LOG_PROG, "MTD 0x04: %d satellites\n", st); + if ((session->driver.sirf.satcounter++ % 5) != 0) + return 0; + else + return TIME_SET | SATELLITE_SET; +} + +static gps_mask_t sirf_msg_navsol(struct gps_device_t *session, unsigned char *buf, size_t len) +{ + int i; + unsigned short navtype; + gps_mask_t mask = 0; + + if (len != 41) + return 0; + + session->gpsdata.satellites_used = (int)getub(buf, 28); + memset(session->gpsdata.used,0,sizeof(session->gpsdata.used)); + for (i = 0; i < SIRF_CHANNELS; i++) + session->gpsdata.used[i] = (int)getub(buf, 29+i); + if ((session->driver.sirf.driverstate & (SIRF_GE_232 | UBLOX))==0) { + /* position/velocity is bytes 1-18 */ + ecef_to_wgs84fix(&session->gpsdata, + getbesl(buf, 1)*1.0, getbesl(buf, 5)*1.0, getbesl(buf, 9)*1.0, + getbesw(buf, 13)/8.0, getbesw(buf, 15)/8.0, getbesw(buf, 17)/8.0); + /* fix status is byte 19 */ + navtype = (unsigned short)getub(buf, 19); + session->gpsdata.status = STATUS_NO_FIX; + session->gpsdata.fix.mode = MODE_NO_FIX; + if ((navtype & 0x80) != 0) + session->gpsdata.status = STATUS_DGPS_FIX; + else if ((navtype & 0x07) > 0 && (navtype & 0x07) < 7) + session->gpsdata.status = STATUS_FIX; + if ((navtype & 0x07) == 4 || (navtype & 0x07) == 6) + session->gpsdata.fix.mode = MODE_3D; + else if (session->gpsdata.status != 0) + session->gpsdata.fix.mode = MODE_2D; + if (session->gpsdata.fix.mode == MODE_3D) + mask |= ALTITUDE_SET | CLIMB_SET; + gpsd_report(LOG_PROG, "MND 0x02: Navtype = 0x%0x, Status = %d, mode = %d\n", + navtype,session->gpsdata.status,session->gpsdata.fix.mode); + /* byte 20 is HDOP, see below */ + /* byte 21 is "mode 2", not clear how to interpret that */ + session->gpsdata.fix.time = session->gpsdata.sentence_time = + gpstime_to_unix(getbesw(buf, 22), getbeul(buf, 24)*1e-2) - + session->context->leap_seconds; +#ifdef NTPSHM_ENABLE + if (session->gpsdata.fix.mode > MODE_NO_FIX) { + if ((session->driver.sirf.time_seen & TIME_SEEN_GPS_2) == 0) + gpsd_report(LOG_PROG, "valid time in message 0x02, seen=0x%02x\n", + session->driver.sirf.time_seen); + session->driver.sirf.time_seen |= TIME_SEEN_GPS_2; + if (session->context->enable_ntpshm && IS_HIGHEST_BIT(session->driver.sirf.time_seen,TIME_SEEN_GPS_2)) + (void)ntpshm_put(session, session->gpsdata.fix.time + 0.8); + } +#endif /* NTPSHM_ENABLE */ + /* fix quality data */ + session->gpsdata.hdop = (double)getub(buf, 20)/5.0; + mask |= TIME_SET | LATLON_SET | TRACK_SET | SPEED_SET | STATUS_SET | MODE_SET | HDOP_SET | USED_SET | CYCLE_START_SET; + } + return mask; +} + +static gps_mask_t sirf_msg_geodetic(struct gps_device_t *session, unsigned char *buf, size_t len) +{ + unsigned short navtype; + gps_mask_t mask = 0; + + if (len != 91) + return 0; + + session->gpsdata.sentence_length = 91; + (void)strlcpy(session->gpsdata.tag, "GND",MAXTAGLEN+1); + + navtype = (unsigned short)getbeuw(buf, 3); + session->gpsdata.status = STATUS_NO_FIX; + session->gpsdata.fix.mode = MODE_NO_FIX; + if (navtype & 0x80) + session->gpsdata.status = STATUS_DGPS_FIX; + else if ((navtype & 0x07) > 0 && (navtype & 0x07) < 7) + session->gpsdata.status = STATUS_FIX; + session->gpsdata.fix.mode = MODE_NO_FIX; + if ((navtype & 0x07) == 4 || (navtype & 0x07) == 6) + session->gpsdata.fix.mode = MODE_3D; + else if (session->gpsdata.status) + session->gpsdata.fix.mode = MODE_2D; + gpsd_report(LOG_PROG, "GND 0x29: Navtype = 0x%0x, Status = %d, mode = %d\n", + navtype, session->gpsdata.status, session->gpsdata.fix.mode); + mask |= STATUS_SET | MODE_SET; + + session->gpsdata.fix.latitude = getbesl(buf, 23)*1e-7; + session->gpsdata.fix.longitude = getbesl(buf, 27)*1e-7; + if (session->gpsdata.fix.latitude!=0 && session->gpsdata.fix.latitude!=0) + mask |= LATLON_SET; + + if ((session->gpsdata.fix.eph = getbesl(buf, 50)*1e-2) > 0) + mask |= HERR_SET; + if ((session->gpsdata.fix.epv = getbesl(buf, 54)*1e-2) > 0) + mask |= VERR_SET; + if ((session->gpsdata.fix.eps = getbesw(buf, 62)*1e-2) > 0) + mask |= SPEEDERR_SET; + + /* HDOP should be available at byte 89, but in 231 it's zero. */ + if ((session->gpsdata.hdop = (unsigned int)getub(buf, 89) * 0.2) > 0) + mask |= HDOP_SET; + + if (session->driver.sirf.driverstate & SIRF_GE_232) { + struct tm unpacked_date; + double subseconds; + /* + * Many versions of the SiRF protocol manual don't document + * this sentence at all. Those that do may incorrectly + * describe UTC Day, Hour, and Minute as 2-byte quantities, + * not 1-byte. Chris Kuethe, our SiRF expert, tells us: + * + * "The Geodetic Navigation packet (0x29) was not fully + * implemented in firmware prior to version 2.3.2. So for + * anyone running 231.000.000 or earlier (including ES, + * SiRFDRive, XTrac trains) you won't get UTC time. I don't + * know what's broken in firmwares before 2.3.1..." + * + * To work around the incomplete implementation of this + * packet in 231, we used to assume that only the altitude field + * from this packet is valid. But even this doesn't necessarily + * seem to be the case. Instead, we do our own computation + * of geoid separation now. + * + * UTC is left all zeros in 231 and older firmware versions, + * and misdocumented in the Protocol Reference (version 1.4). + * Documented: Real: + * UTC year 2 2 + * UTC month 1 1 + * UTC day 2 1 + * UTC hour 2 1 + * UTC minute 2 1 + * UTC second 2 2 + * 11 8 + */ + unpacked_date.tm_year = (int)getbeuw(buf, 11)-1900; + unpacked_date.tm_mon = (int)getub(buf, 13)-1; + unpacked_date.tm_mday = (int)getub(buf, 14); + unpacked_date.tm_hour = (int)getub(buf, 15); + unpacked_date.tm_min = (int)getub(buf, 16); + unpacked_date.tm_sec = 0; + subseconds = getbeuw(buf, 17)*1e-3; + /*@ -compdef -unrecog */ + session->gpsdata.fix.time = session->gpsdata.sentence_time = + (double)timegm(&unpacked_date)+subseconds; + /*@ +compdef +unrecog */ + gpsd_report(LOG_PROG, "MID 41 UTC: %lf\n", session->gpsdata.fix.time); +#ifdef NTPSHM_ENABLE + if (session->gpsdata.fix.mode > MODE_NO_FIX && unpacked_date.tm_year != 0) { + if ((session->driver.sirf.time_seen & TIME_SEEN_UTC_1) == 0) + gpsd_report(LOG_PROG, "valid time in message 0x29, seen=0x%02x\n", + session->driver.sirf.time_seen); + session->driver.sirf.time_seen |= TIME_SEEN_UTC_1; + if (session->context->enable_ntpshm && IS_HIGHEST_BIT(session->driver.sirf.time_seen,TIME_SEEN_UTC_1)) + (void)ntpshm_put(session, session->gpsdata.fix.time + 0.8); + } +#endif /* NTPSHM_ENABLE */ + /* skip 4 bytes of satellite map */ + session->gpsdata.fix.altitude = getbesl(buf, 35)*1e-2; + /* skip 1 byte of map datum */ + session->gpsdata.fix.speed = getbesw(buf, 40)*1e-2; + session->gpsdata.fix.track = getbesw(buf, 42)*1e-2; + /* skip 2 bytes of magnetic variation */ + session->gpsdata.fix.climb = getbesw(buf, 46)*1e-2; + mask |= TIME_SET | SPEED_SET | TRACK_SET; + if (session->gpsdata.fix.mode == MODE_3D) + mask |= ALTITUDE_SET | CLIMB_SET; + } + return mask; +} + +static gps_mask_t sirf_msg_sysparam(struct gps_device_t *session, unsigned char *buf, size_t len) +{ + + if (len != 65) + return 0; + +#ifdef ALLOW_RECONFIGURE + /* save these to restore them in the revert method */ + session->driver.sirf.nav_parameters_seen = true; + session->driver.sirf.altitude_hold_mode = getub(buf, 5); + session->driver.sirf.altitude_hold_source = getub(buf, 6); + session->driver.sirf.altitude_source_input = getbesw(buf, 7); + session->driver.sirf.degraded_mode = getub(buf, 9); + session->driver.sirf.degraded_timeout = getub(buf, 10); + session->driver.sirf.dr_timeout = getub(buf, 11); + session->driver.sirf.track_smooth_mode = getub(buf, 12); + gpsd_report(LOG_PROG, "Setting Navigation Parameters\n"); + (void)sirf_write(session->gpsdata.gps_fd, modecontrol); +#endif /* ALLOW_RECONFIGURE */ + return 0; +} + +static gps_mask_t sirf_msg_ublox(struct gps_device_t *session, unsigned char *buf, size_t len UNUSED) +{ + gps_mask_t mask; + unsigned short navtype; + + if (len != 39) + return 0; + + /* this packet is only sent by uBlox firmware from version 1.32 */ + mask = LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET | CLIMB_SET | + STATUS_SET | MODE_SET | HDOP_SET | VDOP_SET | PDOP_SET; + session->gpsdata.fix.latitude = getbesl(buf, 1) * RAD_2_DEG * 1e-8; + session->gpsdata.fix.longitude = getbesl(buf, 5) * RAD_2_DEG * 1e-8; + session->gpsdata.separation = wgs84_separation(session->gpsdata.fix.latitude, session->gpsdata.fix.longitude); + session->gpsdata.fix.altitude = getbesl(buf, 9) * 1e-3 - session->gpsdata.separation; + session->gpsdata.fix.speed = getbesl(buf, 13) * 1e-3; + session->gpsdata.fix.climb = getbesl(buf, 17) * 1e-3; + session->gpsdata.fix.track = getbesl(buf, 21) * RAD_2_DEG * 1e-8; + + navtype = (unsigned short)getub(buf, 25); + session->gpsdata.status = STATUS_NO_FIX; + session->gpsdata.fix.mode = MODE_NO_FIX; + if (navtype & 0x80) + session->gpsdata.status = STATUS_DGPS_FIX; + else if ((navtype & 0x07) > 0 && (navtype & 0x07) < 7) + session->gpsdata.status = STATUS_FIX; + if ((navtype & 0x07) == 4 || (navtype & 0x07) == 6) + session->gpsdata.fix.mode = MODE_3D; + else if (session->gpsdata.status) + session->gpsdata.fix.mode = MODE_2D; + gpsd_report(LOG_PROG, "EMND 0x62: Navtype = 0x%0x, Status = %d, mode = %d\n", + navtype, session->gpsdata.status, session->gpsdata.fix.mode); + + if (navtype & 0x40) { /* UTC corrected timestamp? */ + struct tm unpacked_date; + double subseconds; + mask |= TIME_SET; + unpacked_date.tm_year = (int)getbeuw(buf, 26) - 1900; + unpacked_date.tm_mon = (int)getub(buf, 28) - 1; + unpacked_date.tm_mday = (int)getub(buf, 29); + unpacked_date.tm_hour = (int)getub(buf, 30); + unpacked_date.tm_min = (int)getub(buf, 31); + unpacked_date.tm_sec = 0; + subseconds = ((unsigned short)getbeuw(buf, 32))*1e-3; + /*@ -compdef */ + session->gpsdata.fix.time = session->gpsdata.sentence_time = + (double)mkgmtime(&unpacked_date)+subseconds; + /*@ +compdef */ +#ifdef NTPSHM_ENABLE + if ((session->driver.sirf.time_seen & TIME_SEEN_UTC_2) == 0) + gpsd_report(LOG_PROG, "valid time in message 0x62, seen=0x%02x\n", + session->driver.sirf.time_seen); + session->driver.sirf.time_seen |= TIME_SEEN_UTC_2; + if (session->context->enable_ntpshm && IS_HIGHEST_BIT(session->driver.sirf.time_seen,TIME_SEEN_UTC_2)) + (void)ntpshm_put(session, session->gpsdata.fix.time + 0.8); +#endif /* NTPSHM_ENABLE */ + session->context->valid |= LEAP_SECOND_VALID; + } + + session->gpsdata.gdop = (int)getub(buf, 34) / 5.0; + session->gpsdata.pdop = (int)getub(buf, 35) / 5.0; + session->gpsdata.hdop = (int)getub(buf, 36) / 5.0; + session->gpsdata.vdop = (int)getub(buf, 37) / 5.0; + session->gpsdata.tdop = (int)getub(buf, 38) / 5.0; + session->driver.sirf.driverstate |= UBLOX; + return mask; +} + +static gps_mask_t sirf_msg_ppstime(struct gps_device_t *session, unsigned char *buf, size_t len) +{ + gps_mask_t mask = 0; + + if (len != 19) + return 0; + + gpsd_report(LOG_PROG, "PPS 0x34: Status = 0x%02x\n", getub(buf, 14)); + if (((int)getub(buf, 14) & 0x07) == 0x07) { /* valid UTC time? */ + struct tm unpacked_date; + unpacked_date.tm_hour = (int)getub(buf, 1); + unpacked_date.tm_min = (int)getub(buf, 2); + unpacked_date.tm_sec = (int)getub(buf, 3); + unpacked_date.tm_mday = (int)getub(buf, 4); + unpacked_date.tm_mon = (int)getub(buf, 5) - 1; + unpacked_date.tm_year = (int)getbeuw(buf, 6) - 1900; + /*@ -compdef */ + session->gpsdata.fix.time = session->gpsdata.sentence_time = + (double)mkgmtime(&unpacked_date); + /*@ +compdef */ + session->context->leap_seconds = (int)getbeuw(buf, 8); + session->context->valid |= LEAP_SECOND_VALID; +#ifdef NTPSHM_ENABLE + if ((session->driver.sirf.time_seen & TIME_SEEN_UTC_2) == 0) + gpsd_report(LOG_PROG, "valid time in message 0x34, seen=0x%02x\n", + session->driver.sirf.time_seen); + session->driver.sirf.time_seen |= TIME_SEEN_UTC_2; + if (session->context->enable_ntpshm && IS_HIGHEST_BIT(session->driver.sirf.time_seen,TIME_SEEN_UTC_2)) + (void)ntpshm_put(session, session->gpsdata.fix.time + 0.3); +#endif /* NTPSHM_ENABLE */ + mask |= TIME_SET; + } + return mask; +} + +gps_mask_t sirf_parse(struct gps_device_t *session, unsigned char *buf, size_t len) +{ + + if (len == 0) + return 0; + + buf += 4; + len -= 8; + gpsd_report(LOG_RAW, "Raw SiRF packet type 0x%02x length %zd: %s\n", + buf[0], len, gpsd_hexdump_wrapper(buf, len, LOG_RAW)); + (void)snprintf(session->gpsdata.tag, sizeof(session->gpsdata.tag), + "MID%d",(int)buf[0]); + + switch (buf[0]) + { + case 0x02: /* Measure Navigation Data Out */ + return sirf_msg_navsol(session, buf, len); + + case 0x04: /* Measured tracker data out */ + return sirf_msg_svinfo(session, buf, len); + + case 0x05: /* Raw Tracker Data Out */ + return 0; + + case 0x06: /* Software Version String */ + return sirf_msg_swversion(session, buf, len); + + case 0x07: /* Clock Status Data */ + gpsd_report(LOG_PROG, "CLK 0x07\n"); + return 0; + + case 0x08: /* subframe data -- extract leap-second from this */ + /* + * Chris Kuethe says: + * "Message 8 is generated as the data is received. It is not + * buffered on the chip. So when you enable message 8, you'll + * get one subframe every 6 seconds. Of the data received, the + * almanac and ephemeris are buffered and stored, so you can + * query them at will. Alas, the time parameters are not + * stored, which is really lame, as the UTC-GPS correction + * changes 1 second every few years. Maybe." + */ + return sirf_msg_navdata(session, buf, len); + + case 0x09: /* CPU Throughput */ + gpsd_report(LOG_PROG, + "THR 0x09: SegStatMax=%.3f, SegStatLat=%3.f, AveTrkTime=%.3f, Last MS=%u\n", + (float)getbeuw(buf, 1)/186, (float)getbeuw(buf, 3)/186, + (float)getbeuw(buf, 5)/186, getbeuw(buf, 7)); + return 0; + + case 0x0a: /* Error ID Data */ + return sirf_msg_errors(buf, len); + + case 0x0b: /* Command Acknowledgement */ + gpsd_report(LOG_PROG, "ACK 0x0b: %02x\n",getub(buf, 1)); + return 0; + + case 0x0c: /* Command NAcknowledgement */ + gpsd_report(LOG_PROG, "NAK 0x0c: %02x\n",getub(buf, 1)); + return 0; + + case 0x0d: /* Visible List */ + gpsd_report(LOG_PROG, "VIS 0x0d\n"); + return 0; + + case 0x0e: /* Almanac Data */ + gpsd_report(LOG_PROG, "ALM 0x0e: %s\n", + gpsd_hexdump_wrapper(buf, len, LOG_PROG)); + return 0; + + case 0x0f: /* Ephemeris Data */ + gpsd_report(LOG_PROG, "EPH 0x0f: %s\n", + gpsd_hexdump_wrapper(buf, len, LOG_PROG)); + return 0; + + case 0x11: /* Differential Corrections */ + gpsd_report(LOG_PROG, "DIFF 0x11: %s\n", + gpsd_hexdump_wrapper(buf, len, LOG_PROG)); + return 0; + + case 0x12: /* OK To Send */ + gpsd_report(LOG_PROG, "OTS 0x12: send indicator = %d\n",getub(buf, 1)); + return 0; + +#ifdef ALLOW_RECONFIGURE + case 0x13: /* Navigation Parameters */ + return sirf_msg_sysparam(session, buf, len); +#endif /* ALLOW_RECONFIGURE */ + + case 0x1b: /* DGPS status (undocumented) */ + /****************************************************************** + Not actually documented in any published materials. + Here is what Chris Kuethe got from the SiRF folks, + (plus some corrections from the GpsPaSsion forums): + + Start of message + ---------------- + Message ID 1 byte 27 + Correction Source 1 byte 0=None, 1=SBAS, 2=Serial, 3=Beacon, + 4=Software + + total: 2 bytes + + Middle part of message varies if using beacon or other: + ------------------------------------------------------- + If Beacon: + Receiver Freq Hz 4 bytes + Bit rate BPS 1 byte + Status bit map 1 byte 01=Signal Valid, + 02=Auto frequency detect + 04=Auto bit rate detect + Signal Magnitude 4 bytes Note: in internal units + Signal Strength dB 2 bytes derived from Signal Magnitude + SNR dB 2 bytes + + total: 14 bytes + + If Not Beacon: + Correction Age[12] 1 byte x 12 Age in seconds in same order as follows + Reserved 2 bytes + + total: 14 bytes + + End of Message + -------------- + Repeated 12 times (pad with 0 if less than 12 SV corrections): + SVID 1 byte + Correction (cm) 2 bytes (signed short) + + total 3 x 12 = 36 bytes + ******************************************************************/ + return 0; + + case 0x1c: /* Navigation Library Measurement Data */ + gpsd_report(LOG_PROG, "NLMD 0x1c: %s\n", + gpsd_hexdump_wrapper(buf, len, LOG_PROG)); + return 0; + + case 0x1d: /* Navigation Library DGPS Data */ + gpsd_report(LOG_PROG, "NLDG 0x1d: %s\n", + gpsd_hexdump_wrapper(buf, len, LOG_PROG)); + return 0; + + case 0x1e: /* Navigation Library SV State Data */ + gpsd_report(LOG_PROG, "NLSV 0x1e: %s\n", + gpsd_hexdump_wrapper(buf, len, LOG_PROG)); + return 0; + + case 0x1f: /* Navigation Library Initialization Data */ + gpsd_report(LOG_PROG, "NLID 0x1f: %s\n", + gpsd_hexdump_wrapper(buf, len, LOG_PROG)); + return 0; + + case 0x29: /* Geodetic Navigation Information */ + return sirf_msg_geodetic(session, buf, len); + + case 0x32: /* SBAS corrections */ + return 0; + + case 0x34: /* PPS Time */ + /* + * Carl Carter from SiRF writes: "We do not output on the + * second (unless you are using message ID 52). We make + * measurements in the receiver in time with an internal + * counter that is not slaved to GPS time, so the measurements + * are made at a time that wanders around the second. Then, + * after the measurements are made (all normalized to the same + * point in time) we dispatch the navigation software to make + * a solution, and that solution comes out some 200 to 300 ms + * after the measurement time. So you may get a message at + * 700 ms after the second that uses measurements time tagged + * 450 ms after the second. And if some other task jumps up + * and delays things, that message may not come out until 900 + * ms after the second. Things can get out of sync to the + * point that if you try to resolve the GPS time of our 1 PPS + * pulses using the navigation messages, you will find it + * impossible to be consistent. That is why I added message + * ID 52 to our system -- it is tied to the creation of the 1 + * PPS and always comes out right around the top of the + * second." + */ + return sirf_msg_ppstime(session, buf, len); + + case 0x62: /* uBlox Extended Measured Navigation Data */ + return sirf_msg_ublox(session, buf, len); + + case 0x80: /* Initialize Data Source */ + gpsd_report(LOG_PROG, "INIT 0x80: %s\n", + gpsd_hexdump_wrapper(buf, len, LOG_PROG)); + return 0; + + case 0xe1: /* Development statistics messages */ + /* FALLTHROUGH */ + case 0xff: /* Debug messages */ + (void)sirf_msg_debug(buf, len); + return 0; + + default: + gpsd_report(LOG_WARN, "Unknown SiRF packet id %d length %zd: %s\n", + buf[0], len, gpsd_hexdump_wrapper(buf, len, LOG_WARN)); + return 0; + } +} + +static gps_mask_t sirfbin_parse_input(struct gps_device_t *session) +{ + gps_mask_t st; + + if (session->packet.type == SIRF_PACKET){ + st = sirf_parse(session, session->packet.outbuffer, + session->packet.outbuflen); + session->gpsdata.driver_mode = MODE_BINARY; + return st; +#ifdef NMEA_ENABLE + } else if (session->packet.type == NMEA_PACKET) { + st = nmea_parse((char *)session->packet.outbuffer, session); + session->gpsdata.driver_mode = MODE_NMEA; + return st; +#endif /* NMEA_ENABLE */ + } else + return 0; +} + +#ifdef ALLOW_RECONFIGURE +static void sirfbin_configure(struct gps_device_t *session, unsigned int seq) +{ + if (seq != 0) + return; + if (session->packet.type == NMEA_PACKET) { + gpsd_report(LOG_PROG, "Switching chip mode to SiRF binary.\n"); + (void)nmea_send(session, + "$PSRF100,0,%d,8,1,0", session->gpsdata.baudrate); + } + /* do this every time*/ + { + /*@ +charint @*/ + static unsigned char navparams[] = {0xa0, 0xa2, 0x00, 0x02, + 0x98, 0x00, + 0x00, 0x00, 0xb0, 0xb3}; + static unsigned char dgpscontrol[] = {0xa0, 0xa2, 0x00, 0x07, + 0x85, 0x01, 0x00, 0x00, + 0x00, 0x00, 0x00, + 0x00, 0x00, 0xb0, 0xb3}; + static unsigned char sbasparams[] = {0xa0, 0xa2, 0x00, 0x06, + 0xaa, 0x00, 0x01, 0x00, + 0x00, 0x00, + 0x00, 0x00, 0xb0, 0xb3}; + static unsigned char versionprobe[] = {0xa0, 0xa2, 0x00, 0x02, + 0x84, 0x00, + 0x00, 0x00, 0xb0, 0xb3}; + static unsigned char requestecef[] = {0xa0, 0xa2, 0x00, 0x08, + 0xa6, 0x00, 0x02, 0x01, + 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0xb0, 0xb3}; + static unsigned char requesttracker[] = {0xa0, 0xa2, 0x00, 0x08, + 0xa6, 0x00, 0x04, 0x03, + 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0xb0, 0xb3}; + /*@ -charint @*/ + gpsd_report(LOG_PROG, "Requesting periodic ecef reports...\n"); + (void)sirf_write(session->gpsdata.gps_fd, requestecef); + gpsd_report(LOG_PROG, "Requesting periodic tracker reports...\n"); + (void)sirf_write(session->gpsdata.gps_fd, requesttracker); + gpsd_report(LOG_PROG, "Setting DGPS control to use SBAS...\n"); + (void)sirf_write(session->gpsdata.gps_fd, dgpscontrol); + gpsd_report(LOG_PROG, "Setting SBAS to auto/integrity mode...\n"); + (void)sirf_write(session->gpsdata.gps_fd, sbasparams); + gpsd_report(LOG_PROG, "Probing for firmware version...\n"); + (void)sirf_write(session->gpsdata.gps_fd, versionprobe); + gpsd_report(LOG_PROG, "Requesting navigation parameters...\n"); + (void)sirf_write(session->gpsdata.gps_fd, navparams); + } +} + +static void sirfbin_revert(struct gps_device_t *session) +{ + /*@ +charint @*/ + static unsigned char moderevert[] = {0xa0, 0xa2, 0x00, 0x0e, + 0x88, + 0x00, 0x00, /* pad bytes */ + 0x00, /* degraded mode */ + 0x00, 0x00, /* pad bytes */ + 0x00, 0x00, /* altitude source */ + 0x00, /* altitude hold mode */ + 0x00, /* use last computed alt */ + 0x00, /* reserved */ + 0x00, /* degraded mode timeout */ + 0x00, /* dead reckoning timeout */ + 0x00, /* track smoothing */ + 0x00, 0x00, 0xb0, 0xb3}; + /*@ -charint -shiftimplementation @*/ + putbyte(moderevert, 7, session->driver.sirf.degraded_mode); + putbeword(moderevert, 10, session->driver.sirf.altitude_source_input); + putbyte(moderevert, 12, session->driver.sirf.altitude_hold_mode); + putbyte(moderevert, 13, session->driver.sirf.altitude_hold_source); + putbyte(moderevert, 15, session->driver.sirf.degraded_timeout); + putbyte(moderevert, 16, session->driver.sirf.dr_timeout); + putbyte(moderevert, 17, session->driver.sirf.track_smooth_mode); + /*@ +shiftimplementation @*/ + gpsd_report(LOG_PROG, "Reverting navigation parameters...\n"); + (void)sirf_write(session->gpsdata.gps_fd, moderevert); +} +#endif /* ALLOW_RECONFIGURE */ + +static bool sirfbin_speed(struct gps_device_t *session, + speed_t speed, char parity, int stopbits) +{ + return sirf_speed(session->gpsdata.gps_fd, speed, parity, stopbits); +} + +/* this is everything we export */ +const struct gps_type_t sirf_binary = +{ + .type_name = "SiRF binary", /* full name of type */ + .packet_type = SIRF_PACKET, /* associated lexer packet type */ + .trigger = NULL, /* no trigger */ + .channels = SIRF_CHANNELS, /* consumer-grade GPS */ + .control_send = sirf_control_send,/* how to send a control string */ + .probe_wakeup = NULL, /* no wakeup to be done before hunt */ + .probe_detect = NULL, /* no probe */ + .probe_subtype = NULL, /* can't probe more in NMEA mode */ +#ifdef ALLOW_RECONFIGURE + .configurator = sirfbin_configure,/* initialize the device */ +#endif /* ALLOW_RECONFIGURE */ + .get_packet = sirf_get, /* be prepared for SiRF or NMEA */ + .parse_packet = sirfbin_parse_input,/* parse message packets */ + .rtcm_writer = pass_rtcm, /* send RTCM data straight */ + .speed_switcher = sirfbin_speed, /* we can change baud rate */ + .mode_switcher = sirfbin_mode, /* there's a mode switcher */ + .rate_switcher = NULL, /* no sample-rate switcher */ + .cycle_chars = -1, /* not relevant, no rate switch */ +#ifdef ALLOW_RECONFIGURE + .revert = sirfbin_revert, /* no reversion code */ +#endif /* ALLOW_RECONFIGURE */ + .wrapup = NULL, /* no close hook */ + .cycle = 1, /* updates every second */ +}; +#endif /* defined(SIRF_ENABLE) && defined(BINARY_ENABLE) */ diff --git a/driver_superstar2.c b/driver_superstar2.c new file mode 100644 index 00000000..34d86589 --- /dev/null +++ b/driver_superstar2.c @@ -0,0 +1,528 @@ +/* $Id$ */ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "gpsd_config.h" +#include "gpsd.h" + +#if defined(SUPERSTAR2_ENABLE) && defined(BINARY_ENABLE) +#include "bits.h" +#include "driver_superstar2.h" + +/* + * These routines are specific to this driver + */ + +static gps_mask_t superstar2_parse_input(struct gps_device_t *); +static gps_mask_t superstar2_dispatch(struct gps_device_t *, + unsigned char *, size_t ); +static gps_mask_t superstar2_msg_ack(struct gps_device_t *, + unsigned char *, size_t ); +static gps_mask_t superstar2_msg_navsol_lla(struct gps_device_t *, + unsigned char *, size_t ); +static gps_mask_t superstar2_msg_navsol_ecef(struct gps_device_t *, + unsigned char *, size_t ); +static gps_mask_t superstar2_msg_timing(struct gps_device_t *, + unsigned char *, size_t ); +static gps_mask_t superstar2_msg_svinfo(struct gps_device_t *, + unsigned char *, size_t ); + +/* + * These methods may be called elsewhere in gpsd + */ +static ssize_t superstar2_control_send(struct gps_device_t *, char *, size_t ); +static void superstar2_probe_wakeup(struct gps_device_t *); +static void superstar2_configurator(struct gps_device_t *, unsigned int ); +static bool superstar2_set_speed(struct gps_device_t *, speed_t, char, int); +static void superstar2_set_mode(struct gps_device_t *, int ); +static void superstar2_probe_wakeup(struct gps_device_t *); +static void superstar2_probe_subtype(struct gps_device_t *, unsigned int ); + +/* + * Decode the message ACK message + */ +static gps_mask_t +superstar2_msg_ack(struct gps_device_t *session UNUSED, + unsigned char *buf, size_t data_len) +{ + if (data_len == 11) + gpsd_report(LOG_PROG, + "superstar2 #126 - " + "ACK 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x\n", + buf[5], buf[6], buf[7], buf[8], buf[9]); + return ONLINE_SET; /* always returns ONLINE_SET, but avoid runt packets */ +} + +/* + * Decode the navigation solution message + */ +static gps_mask_t +superstar2_msg_navsol_lla(struct gps_device_t *session, + unsigned char *buf, size_t data_len) +{ + gps_mask_t mask; + unsigned char flags; + union int_float i_f; + union long_double l_d; + double d; + struct tm tm; + + if (data_len != 77) + return 0; + + gpsd_report(LOG_PROG, "superstar2 #20 - user navigation data\n"); + mask = ONLINE_SET; + + flags = getub(buf, 72); + if ((flags & 0x0f) != 3) /* mode 3 is navigation */ + return mask; + + /* extract time data */ + bzero(&tm, sizeof(tm)); + tm.tm_hour = getub(buf, 4) & 0x1f; + tm.tm_min = getub(buf, 5); + d = getled(buf, 6); + tm.tm_sec = (int)d; + tm.tm_mday = getub(buf, 14); + tm.tm_mon = getub(buf, 15) - 1; + tm.tm_year = getleuw(buf, 16) - 1900; + session->gpsdata.fix.time = session->gpsdata.sentence_time = + timegm(&tm) + (d - tm.tm_sec); + mask |= TIME_SET; + + /* extract the local tangential plane (ENU) solution */ + session->gpsdata.fix.latitude = getled(buf,18) * RAD_2_DEG; + session->gpsdata.fix.longitude = getled(buf,26) * RAD_2_DEG; + session->gpsdata.fix.altitude = getlef(buf,34); + session->gpsdata.fix.speed = getlef(buf,38); + session->gpsdata.fix.track = getlef(buf,42) * RAD_2_DEG; + session->gpsdata.fix.climb = getlef(buf,54); + mask |= LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET | CLIMB_SET ; + + session->gpsdata.satellites_used = getub(buf,71) & 0x0f; + session->gpsdata.hdop = getleuw(buf,66) * 0.1; + session->gpsdata.vdop = getleuw(buf,68) * 0.1; + /* other DOP if available */ + mask |= HDOP_SET | VDOP_SET | USED_SET; + + flags = getub(buf,70); + switch (flags & 0x1f) { + case 2: + session->gpsdata.fix.mode = MODE_3D; + session->gpsdata.status = STATUS_FIX; + break; + case 4: + session->gpsdata.fix.mode = MODE_3D; + session->gpsdata.status = STATUS_DGPS_FIX; + break; + case 5: + session->gpsdata.fix.mode = MODE_2D; + session->gpsdata.status = STATUS_DGPS_FIX; + break; + case 3: + case 6: + session->gpsdata.fix.mode = MODE_2D; + session->gpsdata.status = STATUS_FIX; + break; + default: + session->gpsdata.status = STATUS_NO_FIX; + session->gpsdata.fix.mode = MODE_NO_FIX; + } + + /* CYCLE_START_SET if this message starts a reporting period */ + mask |= MODE_SET | STATUS_SET | CYCLE_START_SET ; + + return mask; +} + +static gps_mask_t +superstar2_msg_navsol_ecef(struct gps_device_t *session, + unsigned char *buf, size_t data_len) +{ + gps_mask_t mask; + unsigned char flags; + union int_float i_f; + union long_double l_d; + double tm, tow; + + if (data_len != 85) + return 0; + + gpsd_report(LOG_PROG, "superstar2 #21 - ecef navigation data\n"); + mask = ONLINE_SET; + + flags = getub(buf, 79) & 0x1f; + if ((flags < 2) || (flags > 5)) + return mask; + + /* extract time data */ + tow = getled(buf, 4); + session->driver.superstar2.gps_week = getleuw(buf, 12); + tm = gpstime_to_unix((int)session->driver.superstar2.gps_week, tow) - + session->context->leap_seconds; + session->gpsdata.fix.time = session->gpsdata.sentence_time = tm; + mask |= TIME_SET; + + /* extract the earth-centered, earth-fixed (ECEF) solution */ + ecef_to_wgs84fix(&session->gpsdata, + getled(buf, 14), getled(buf, 22), getled(buf, 30), + getlef(buf, 38), getlef(buf, 42), getlef(buf, 46)); + mask |= LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET | CLIMB_SET ; + + session->gpsdata.satellites_used = getub(buf, 79) & 0x0f; + session->gpsdata.hdop = getleuw(buf, 74) * 0.1; + session->gpsdata.vdop = getleuw(buf, 76) * 0.1; + /* other DOP if available */ + mask |= HDOP_SET | VDOP_SET | USED_SET; + + flags = getub(buf,70); + switch (flags & 0x1f) { + case 2: + session->gpsdata.fix.mode = MODE_3D; + session->gpsdata.status = STATUS_FIX; + break; + case 4: + session->gpsdata.fix.mode = MODE_3D; + session->gpsdata.status = STATUS_DGPS_FIX; + break; + case 5: + session->gpsdata.fix.mode = MODE_2D; + session->gpsdata.status = STATUS_DGPS_FIX; + break; + case 3: + case 6: + session->gpsdata.fix.mode = MODE_2D; + session->gpsdata.status = STATUS_FIX; + break; + default: + session->gpsdata.status = STATUS_NO_FIX; + session->gpsdata.fix.mode = MODE_NO_FIX; + } + + /* CYCLE_START_SET if this message starts a reporting period */ + mask |= MODE_SET | STATUS_SET | CYCLE_START_SET; + + return mask; +} + +/** + * GPS Satellite Info + */ +static gps_mask_t +superstar2_msg_svinfo(struct gps_device_t *session, + unsigned char *buf, size_t data_len) +{ + unsigned char i, st, nchan, nsv; + + if (data_len != 67) + return 0; + + gpsd_report(LOG_PROG, "superstar2 #33 - satellite data"); + + nchan = 12; + gpsd_zero_satellites(&session->gpsdata); + nsv = 0; /* number of actually used satellites */ + for (i = st = 0; i < nchan; i++) { + /* get info for one channel/satellite */ + int off = i*5 + 5; + int porn; + if ((porn = getub(buf, off) & 0x1f) == 0) + porn = (getub(buf, off+3) >> 1) + 87; + + session->gpsdata.PRN[i] = porn; + session->gpsdata.ss[i] = getub(buf, off+4); + session->gpsdata.elevation[i] = getsb(buf, off+1); + session->gpsdata.azimuth[i] = (unsigned short)getub(buf, off+2) + ((unsigned short)(getub(buf, off+3) & 0x1) << 1); + + if ((getub(buf, off) & 0x60) == 0x60) + session->gpsdata.used[nsv++] = session->gpsdata.PRN[i]; + + if(session->gpsdata.PRN[i]) + st++; + } + session->gpsdata.satellites_used = nsv; + session->gpsdata.satellites = st; + return SATELLITE_SET | USED_SET | ONLINE_SET; +} + +static gps_mask_t +superstar2_msg_version(struct gps_device_t *session, + unsigned char *buf, size_t data_len) +{ +#define SZ 16 + char main_sw[SZ], hw_part[SZ], boot_sw[SZ], ser_num[SZ]; + + /* byte 98 is device type, value = 3 means superstar2 */ + if ((data_len != 101) || ((getub(buf,98) & 0x0f) != 3)) + return 0; + + snprintf(main_sw, 15, "%s", buf+4); + snprintf(hw_part, 15, "%s", buf+18); + snprintf(boot_sw, 15, "%s", buf+36); + snprintf(ser_num, 14, "%s", buf+73); + + gpsd_report(LOG_PROG, + "superstar2 #45 - " + "hw part %s boot sw %s main sw %s ser num %s\n", + hw_part, boot_sw, main_sw, ser_num); + strlcpy(session->subtype, main_sw, sizeof(session->subtype)); + return DEVICEID_SET | ONLINE_SET; +} + +/** + * GPS Leap Seconds + */ +static gps_mask_t +superstar2_msg_timing(struct gps_device_t *session, unsigned char *buf, size_t data_len) +{ + union long_double l_d; + double d; + struct tm tm; + + if (data_len != 65) + return 0; + + gpsd_report(LOG_PROG, "superstar2 #113 - timing status\n"); + if ((getub(buf, 55) & 0x30) != 0) + return ONLINE_SET; + + /* extract time data */ + bzero(&tm, sizeof(tm)); + tm.tm_mday = getsb(buf, 37); + tm.tm_mon = getsb(buf, 38) - 1; + tm.tm_year = getlesw(buf, 39) - 1900; + + tm.tm_hour = getsb(buf, 41); + tm.tm_min = getsb(buf, 42); + d = getled(buf, 43); + tm.tm_sec = (int)d; + session->gpsdata.sentence_time = session->gpsdata.fix.time = timegm(&tm); + session->context->leap_seconds = getsb(buf,20); + + return TIME_SET | ONLINE_SET; +} + + +/** + * Write data to the device, doing any required padding or checksumming + */ +static ssize_t +superstar2_control_send(struct gps_device_t *session, char *msg, size_t msglen) +{ + unsigned short c = 0; + size_t i; + + for (i = 0; i < msglen - 2; i++) + c += (unsigned char)msg[i]; +// c = htons(c); // XXX is this needed on big-endian machines? + memcpy(msg + msg[3] + 4, &c, 2); + gpsd_report(LOG_IO, "writing superstar2 control type %02x len %zu:%s\n", + (unsigned char)msg[1], msglen, + gpsd_hexdump_wrapper(msg, msglen, LOG_IO)); + return gpsd_write(session, msg, msglen); +} + +/** + * Parse the data from the device + */ +gps_mask_t +superstar2_dispatch(struct gps_device_t *session, unsigned char *buf, + size_t len) +{ + int type; + + if (len == 0) + return 0; + + type = buf[SUPERSTAR2_TYPE_OFFSET]; + (void)snprintf(session->gpsdata.tag, + sizeof(session->gpsdata.tag), "SS2-%u", (int)type); + + switch (type) + { + case SUPERSTAR2_ACK: /* Message Acknowledgement */ + return superstar2_msg_ack(session, buf, len); + case SUPERSTAR2_SVINFO: /* Satellite Visibility Data */ + return superstar2_msg_svinfo(session, buf, len); + case SUPERSTAR2_NAVSOL_LLA: /* Navigation Data */ + return superstar2_msg_navsol_lla(session, buf, len); + case SUPERSTAR2_NAVSOL_ECEF: /* Navigation Data */ + return superstar2_msg_navsol_ecef(session, buf, len); + case SUPERSTAR2_VERSION: /* Hardware/Software Version */ + return superstar2_msg_version(session, buf, len); + case SUPERSTAR2_TIMING: /* Timing Parameters */ + return superstar2_msg_timing(session, buf, len); + + default: + /* XXX This gets noisy in a hurry. */ + gpsd_report(LOG_WARN, + "unknown superstar2 packet id 0x%02x length %zd: %s\n", + type, len, gpsd_hexdump_wrapper(buf, len, LOG_WARN)); + return 0; + } +} + +/********************************************************** + * + * Externally called routines below here + * + **********************************************************/ +/* canned config messages */ +/* Initiate Link ID# 63 */ +static unsigned char link_msg[] = {0x01, 0x3f, 0xc0, 0x08, + 0x55, 0x47, 0x50, 0x53, 0x2d, 0x30, 0x30, 0x30, + 0x00, 0x00}; + +/* Request Hardware/Software Identification ID# 45 */ +static unsigned char version_msg[] = {0x01, 0x2d, 0xd2, 0x00, 0x00, 0x01}; + +static void +superstar2_probe_wakeup(struct gps_device_t *session) +{ + superstar2_control_send(session, link_msg, sizeof(link_msg)); + usleep(300000); + superstar2_control_send(session, version_msg, sizeof(version_msg)); + return; +} + +static void +superstar2_probe_subtype(struct gps_device_t *session, + unsigned int seq) +{ + if (seq == 0){ + superstar2_control_send(session, link_msg, sizeof(link_msg)); + usleep(300000); + superstar2_control_send(session, version_msg, sizeof(version_msg)); + } + return; +} + +static void superstar2_configurator(struct gps_device_t *session, + unsigned int seq UNUSED) +{ + unsigned char a; + unsigned char message_list[] = { + SUPERSTAR2_NAVSOL_LLA, + SUPERSTAR2_SVINFO, + SUPERSTAR2_TIMING, + SUPERSTAR2_NAVSOL_ECEF, + SUPERSTAR2_DUMMY}; + unsigned char message2_list[] = { + SUPERSTAR2_MEASUREMENT, + SUPERSTAR2_DUMMY}; + unsigned char tmpl_msg[] = { 0x01, 0x00, 0x00, 0x00, 0x00, 0x00}; + unsigned char tmpl2_msg[] = { 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00}; + + for(a = 0; message_list[a] != 0; a++){ + /* set high bit to enable continuous output */ + tmpl_msg[1] = (unsigned char)(message_list[a] | 0x80); + tmpl_msg[2] = (unsigned char)(tmpl_msg[1] ^ 0xff); + superstar2_control_send(session, tmpl_msg, sizeof(tmpl_msg)); + usleep(20000); + } + for(a = 0; message2_list[a] != 0; a++){ + /* set high bit to enable continuous output */ + tmpl2_msg[1] = (unsigned char)(message2_list[a] | 0x80); + tmpl2_msg[2] = (unsigned char)(tmpl2_msg[1] ^ 0xff); + superstar2_control_send(session, tmpl2_msg, sizeof(tmpl2_msg)); + usleep(20000); + } + superstar2_control_send(session, version_msg, sizeof(version_msg)); +} + +/* + * This is the entry point to the driver. When the packet sniffer recognizes + * a packet for this driver, it calls this method which passes the packet to + * the binary processor or the nmea processor, depending on the session type. + */ +static gps_mask_t superstar2_parse_input(struct gps_device_t *session) +{ + gps_mask_t st; + + if (session->packet.type == SUPERSTAR2_PACKET){ + st = superstar2_dispatch(session, session->packet.outbuffer, + session->packet.length); + session->gpsdata.driver_mode = MODE_BINARY; + return st; +#ifdef NMEA_ENABLE + } else if (session->packet.type == NMEA_PACKET) { + st = nmea_parse((char *)session->packet.outbuffer, session); + (void)gpsd_switch_driver(session, "Generic NMEA"); + session->gpsdata.driver_mode = MODE_NMEA; + return st; +#endif /* NMEA_ENABLE */ + } else + return 0; +} + +static bool superstar2_set_speed(struct gps_device_t *session, + speed_t speed, char parity, int stopbits) +{ + /* set port operating mode, speed, bits etc. here */ + return false; +} + +/* + * Switch between NMEA and binary mode, if supported + */ +static void superstar2_set_mode(struct gps_device_t *session, int mode) +{ + if (mode == MODE_NMEA) { + // superstar2_to_nmea(session->gpsdata.gps_fd,session->gpsdata.baudrate); /* send the mode switch control string */ + } else { + session->back_to_nmea = false; + } +} + +const struct gps_type_t superstar2_binary = { + /* Full name of type */ + .type_name = "SuperStarII binary", + /* associated lexer packet type */ + .packet_type = SUPERSTAR2_PACKET, + /* Response string that identifies device (not active) */ + .trigger = NULL, + /* Number of satellite channels supported by the device */ + .channels = 12, + /* Control string sender - should provide checksum and trailer */ + .control_send = superstar2_control_send, + /* Startup-time device detector */ + .probe_detect = NULL, + /* Wakeup to be done before each baud hunt */ + .probe_wakeup = superstar2_probe_wakeup, + /* Initialize the device and get subtype */ + .probe_subtype = superstar2_probe_subtype, +#ifdef ALLOW_RECONFIGURE + /* Enable what reports we need */ + .configurator = superstar2_configurator, +#endif /* ALLOW_RECONFIGURE */ + /* Packet getter (using default routine) */ + .get_packet = generic_get, + /* Parse message packets */ + .parse_packet = superstar2_parse_input, + /* RTCM handler (using default routine) */ + .rtcm_writer = pass_rtcm, + /* Speed (baudrate) switch */ + .speed_switcher = superstar2_set_speed, + /* Switch to NMEA mode */ + .mode_switcher = superstar2_set_mode, + /* Message delivery rate switcher (not active) */ + .rate_switcher = NULL, + /* Number of chars per report cycle (not active) */ + .cycle_chars = -1, +#ifdef ALLOW_RECONFIGURE + /* Undo the actions of .configurator */ + .revert = NULL, +#endif /* ALLOW_RECONFIGURE */ + /* Puts device back to original settings */ + .wrapup = NULL, + /* Number of updates per second */ + .cycle = 1 +}; +#endif /* defined(SUPERSTAR2_ENABLE) && defined(BINARY_ENABLE) */ diff --git a/driver_superstar2.h b/driver_superstar2.h new file mode 100644 index 00000000..8b1529ca --- /dev/null +++ b/driver_superstar2.h @@ -0,0 +1,59 @@ +/* $Id$ */ +#ifndef _GPSD_SUPERSTAR2_H_ +#define _GPSD_SUPERSTAR2_H_ + +#define SUPERSTAR2_BASE_SIZE 4 +#define SUPERSTAR2_TYPE_OFFSET 1 + +/* input-only */ +#define SUPERSTAR2_RESET 2 +#define SUPERSTAR2_LINKUP 63 +#define SUPERSTAR2_CHANNEL_INHIBIT 64 +#define SUPERSTAR2_TIME_PARAMS 69 +#define SUPERSTAR2_ALMANAC_INCREMENT 77 +#define SUPERSTAR2_ALMANAC_UPLOAD 79 +#define SUPERSTAR2_SET_OPMODE 80 +#define SUPERSTAR2_SET_MASK 81 +#define SUPERSTAR2_SET_DGPS 83 +#define SUPERSTAR2_SET_IONOMODEL 84 +#define SUPERSTAR2_SET_MSLMODEL 86 +#define SUPERSTAR2_SET_HEIGHT_MODE 87 +#define SUPERSTAR2_SET_DATUM 88 +#define SUPERSTAR2_SATELLITE_INHIBIT 90 +#define SUPERSTAR2_BASE_CONFIG 91 +#define SUPERSTAR2_SATELLITE_TRACK 95 +#define SUPERSTAR2_NVM_ERASE 99 +#define SUPERSTAR2_SET_TIME 103 +#define SUPERSTAR2_MESSAGE_CONFIG 105 +#define SUPERSTAR2_SERIAL_CONFIG 110 + +/* output-only */ +#define SUPERSTAR2_CHANINF2 7 +#define SUPERSTAR2_LINKERR 125 +#define SUPERSTAR2_ACK 126 + +/* bidirectional */ +#define SUPERSTAR2_DUMMY 0 +#define SUPERSTAR2_CHANINF 6 +#define SUPERSTAR2_NAVSOL_LLA 20 +#define SUPERSTAR2_NAVSOL_ECEF 21 +#define SUPERSTAR2_EPHEMERIS 22 +#define SUPERSTAR2_MEASUREMENT 23 +#define SUPERSTAR2_RECV_CONFIG 30 +#define SUPERSTAR2_SVINFO 33 +#define SUPERSTAR2_DGPSCONFIG 43 +#define SUPERSTAR2_VERSION 45 +#define SUPERSTAR2_BASE_STATUS 47 +#define SUPERSTAR2_DGPS_STATUS 48 +#define SUPERSTAR2_RECV_STATUS 49 +#define SUPERSTAR2_SAT_HEALTH 50 +#define SUPERSTAR2_SELFTEST 51 +#define SUPERSTAR2_RTCM_DATA 65 +#define SUPERSTAR2_SBAS_DATA 67 +#define SUPERSTAR2_SBAS_STATUS 68 +#define SUPERSTAR2_IONO_UTC 75 +#define SUPERSTAR2_ALMANAC_DATA 76 +#define SUPERSTAR2_ALMANAC_STATUS 78 +#define SUPERSTAR2_TIMING 113 + +#endif /* _GPSD_SUPERSTAR2_H_ */ diff --git a/driver_tsip.c b/driver_tsip.c new file mode 100644 index 00000000..7ee86523 --- /dev/null +++ b/driver_tsip.c @@ -0,0 +1,856 @@ +/* $Id$ */ +/* + * Handle the Trimble TSIP packet format + * by Rob Janssen, PE1CHL. + */ +#include +#include +#include +#include +#include +#include "gpsd_config.h" +#include "gpsd.h" +#include "bits.h" + +#define USE_SUPERPACKET 1 /* use Super Packet mode? */ + +#define SEMI_2_DEG (180.0 / 2147483647) /* 2^-31 semicircle to deg */ + +#ifdef TSIP_ENABLE +#define TSIP_CHANNELS 12 + +static int tsip_write(struct gps_device_t *session, + unsigned int id, /*@null@*/unsigned char *buf, size_t len) +{ +#ifdef ALLOW_RECONFIGURE + char *ep, *cp; + + gpsd_report(LOG_IO, "Sent TSIP packet id 0x%02x: %s\n", id, + gpsd_hexdump_wrapper(buf, len, LOG_IO)); + + /*@ +charint @*/ + session->msgbuf[0] = '\x10'; + session->msgbuf[1] = (char)id; + ep = session->msgbuf + 2; + /*@ -nullderef @*/ + for (cp = (char *)buf; len-- > 0; cp++) { + if (*cp == '\x10') + *ep++ = '\x10'; + *ep++ = *cp; + } + /*@ +nullderef @*/ + *ep++ = '\x10'; + *ep++ = '\x03'; + session->msgbuflen = (size_t)(ep - (char*)buf); + /*@ -charint @*/ + if (gpsd_write(session,session->msgbuf,session->msgbuflen) != (ssize_t)len) + return -1; + + return 0; +#else + gpsd_report(LOG_IO, "Not sending TSIP packet id 0x%02x: %s\n", id, + gpsd_hexdump_wrapper(buf,len,LOG_IO)); + return -1; +#endif /* ALLOW_RECONFIGURE */ +} + +static ssize_t tsip_control_send(struct gps_device_t *session, + char *buf, size_t buflen) +/* not used by the daemon, it's for gpsctl and friends */ +{ + return (ssize_t)tsip_write(session, + (unsigned int)buf[0], (unsigned char *)buf+1, buflen-1); +} + +static void tsip_probe_subtype(struct gps_device_t *session, unsigned int seq) +{ + unsigned char buf[100]; + + switch (seq) { + case 0: + /* TSIP is ODD parity 1 stopbit, save original values and change it */ + /* XXX Thunderbolts and Copernicus use 8N1... which isn't exactly a */ + /* XXX good idea due to the fragile wire format. We must divine a */ + /* XXX clever heuristic to decide if the parity change is required. */ + session->driver.tsip.parity = session->gpsdata.parity; + session->driver.tsip.stopbits = session->gpsdata.stopbits; + gpsd_set_speed(session, session->gpsdata.baudrate, 'O', 1); + break; + + case 1: + /* Request Software Versions */ + (void)tsip_write(session, 0x1f, NULL, 0); + /* Request Current Time */ + (void)tsip_write(session, 0x21, NULL, 0); + /* Request GPS Systems Message */ + (void)tsip_write(session, 0x28, NULL, 0); + /* Request Current Datum Values */ + putbyte(buf,0,0x15); + (void)tsip_write(session, 0x8e, buf, 1); + /* Request Navigation Configuration */ + putbyte(buf,0,0x03); + (void)tsip_write(session, 0xbb, buf, 1); + break; + } +} + +#ifdef ALLOW_RECONFIGURE +static void tsip_configurator(struct gps_device_t *session, unsigned int seq) +{ + if (seq == 0) { + unsigned char buf[100]; + + /* I/O Options */ + putbyte(buf,0,0x1e); /* Position: DP, MSL, LLA */ + putbyte(buf,1,0x02); /* Velocity: ENU */ + putbyte(buf,2,0x00); /* Time: GPS */ + putbyte(buf,3,0x08); /* Aux: dBHz */ + (void)tsip_write(session, 0x35, buf, 4); + } +} +#endif /* ALLOW_RECONFIGURE */ + +static void tsip_wrapup(struct gps_device_t *session) +{ + /* restore saved parity and stopbits when leaving TSIP mode */ + gpsd_set_speed(session, + session->gpsdata.baudrate, + (unsigned char)session->driver.tsip.parity, + session->driver.tsip.stopbits); +} + +static bool tsip_speed_switch(struct gps_device_t *session, + unsigned int speed, + char parity, int stopbits) +{ + /* parity and stopbit switching are not yet implemented */ + if (parity!=(char)session->gpsdata.parity || (unsigned int)stopbits!=session->gpsdata.parity) { + return false; + } else { + unsigned char buf[100]; + + putbyte(buf,0,0xff); /* current port */ + putbyte(buf,1,(round(log((double)speed/300)/M_LN2))+2); /* input baudrate */ + putbyte(buf,2,getub(buf,1)); /* output baudrate */ + putbyte(buf,3,8); /* character width (8 bits) */ + putbyte(buf,4,1); /* parity (odd) */ + putbyte(buf,5,0); /* stop bits (1 stopbit) */ + putbyte(buf,6,0); /* flow control (none) */ + putbyte(buf,7,0x02); /* input protocol (TSIP) */ + putbyte(buf,8,0x02); /* output protocol (TSIP) */ + putbyte(buf,9,0); /* reserved */ + (void)tsip_write(session, 0xbc, buf, 10); + + return true; /* it would be nice to error-check this */ + } +} + +static gps_mask_t tsip_analyze(struct gps_device_t *session) +{ + int i, j, len, count; + gps_mask_t mask = 0; + unsigned int id; + u_int8_t u1,u2,u3,u4,u5; + int16_t s1,s2,s3,s4; + int32_t sl1,sl2,sl3; + u_int32_t ul1,ul2; + float f1,f2,f3,f4,f5; + double d1,d2,d3,d4,d5; + union int_float i_f; + union long_double l_d; + time_t now; + unsigned char buf[BUFSIZ]; + char buf2[BUFSIZ]; + + if (session->packet.type != TSIP_PACKET) { + gpsd_report(LOG_INF, "tsip_analyze packet type %d\n",session->packet.type); + return 0; + } + + /*@ +charint @*/ + if (session->packet.outbuflen < 4 || session->packet.outbuffer[0] != 0x10) + return 0; + + /* remove DLE stuffing and put data part of message in buf */ + + memset(buf, 0, sizeof(buf)); + buf2[len = 0] = '\0'; + for (i = 2; i < (int)session->packet.outbuflen; i++) { + if (session->packet.outbuffer[i] == 0x10) + if (session->packet.outbuffer[++i] == 0x03) + break; + + (void)snprintf(buf2+strlen(buf2), + sizeof(buf2)-strlen(buf2), + "%02x", buf[len++] = session->packet.outbuffer[i]); + } + /*@ -charint @*/ + + (void)snprintf(session->gpsdata.tag, sizeof(session->gpsdata.tag), + "ID%02x", id = (unsigned)session->packet.outbuffer[1]); + + gpsd_report(LOG_IO, "TSIP packet id 0x%02x length %d: %s\n",id,len,buf2); + (void)time(&now); + + switch (id) { + case 0x13: /* Packet Received */ + u1 = getub(buf,0); + u2 = getub(buf,1); + gpsd_report(LOG_WARN, "Received packet of type %02x cannot be parsed\n",u1); +#if USE_SUPERPACKET + if ((int)u1 == 0x8e && (int)u2 == 0x23) { /* no Compact Super Packet */ + gpsd_report(LOG_WARN, "No Compact Super Packet, use LFwEI\n"); + + /* Request LFwEI Super Packet */ + putbyte(buf,0,0x20); + putbyte(buf,1,0x01); /* enabled */ + (void)tsip_write(session, 0x8e, buf, 2); + } +#endif /* USE_SUPERPACKET */ + break; + case 0x41: /* GPS Time */ + if (len != 10) + break; + session->driver.tsip.last_41 = now; /* keep timestamp for request */ + f1 = getbef(buf,0); /* gpstime */ + s1 = getbesw(buf,4); /* week */ + f2 = getbef(buf,6); /* leap seconds */ + if (f1 >= 0.0 && f2 > 10.0) { + session->driver.tsip.gps_week = s1; + session->context->leap_seconds = (int)round(f2); + session->context->valid |= LEAP_SECOND_VALID; + + session->gpsdata.sentence_time = gpstime_to_unix((int)s1, f1) - f2; + +#ifdef NTPSHM_ENABLE + if (session->context->enable_ntpshm) + (void)ntpshm_put(session,session->gpsdata.sentence_time+0.075); +#endif + mask |= TIME_SET; + } + gpsd_report(LOG_INF, "GPS Time %f %d %f\n",f1,s1,f2); + break; + case 0x42: /* Single-Precision Position Fix, XYZ ECEF */ + if (len != 16) + break; + f1 = getbef(buf,0); /* X */ + f2 = getbef(buf,4); /* Y */ + f3 = getbef(buf,8); /* Z */ + f4 = getbef(buf,12); /* time-of-fix */ + gpsd_report(LOG_INF, "GPS Position XYZ %f %f %f %f\n",f1,f2,f3,f4); + break; + case 0x43: /* Velocity Fix, XYZ ECEF */ + if (len != 20) + break; + f1 = getbef(buf,0); /* X velocity */ + f2 = getbef(buf,4); /* Y velocity */ + f3 = getbef(buf,8); /* Z velocity */ + f4 = getbef(buf,12); /* bias rate */ + f5 = getbef(buf,16); /* time-of-fix */ + gpsd_report(LOG_INF, "GPS Velocity XYZ %f %f %f %f %f\n",f1,f2,f3,f4,f5); + break; + case 0x45: /* Software Version Information */ + if (len != 10) + break; + /*@ -formattype @*/ + (void)snprintf(session->subtype, sizeof(session->subtype), + "%d.%d %02d%02d%02d %d.%d %02d%02d%02d", + getub(buf,0),getub(buf,1),getub(buf,4),getub(buf,2),getub(buf,3), + getub(buf,5),getub(buf,6),getub(buf,9),getub(buf,7),getub(buf,8)); + /*@ +formattype @*/ + gpsd_report(LOG_INF, "Software version: %s\n", session->subtype); + mask |= DEVICEID_SET; + break; + case 0x46: /* Health of Receiver */ + if (len != 2) + break; + session->driver.tsip.last_46 = now; + u1 = getub(buf,0); /* Status code */ + u2 = getub(buf,1); /* Antenna/Battery */ + if (u1 != (u_int8_t)0) { + session->gpsdata.status = STATUS_NO_FIX; + mask |= STATUS_SET; + } + else { + if (session->gpsdata.status < STATUS_FIX) { + session->gpsdata.status = STATUS_FIX; + mask |= STATUS_SET; + } + } + gpsd_report(LOG_PROG, "Receiver health %02x %02x\n",u1,u2); + break; + case 0x47: /* Signal Levels for all Satellites */ + count = (int)getub(buf,0); /* satellite count */ + if (len != (5*count + 1)) + break; + buf2[0] = '\0'; + for (i = 0; i < count; i++) { + u1 = getub(buf,5*i + 1); + if ((f1 = getbef(buf,5*i + 2)) < 0) + f1 = 0.0; + for (j = 0; j < TSIP_CHANNELS; j++) + if (session->gpsdata.PRN[j] == (int)u1) { + session->gpsdata.ss[j] = (int)round(f1); + break; + } + (void)snprintf(buf2+strlen(buf2), sizeof(buf2)-strlen(buf2), + " %d=%.1f",(int)u1,f1); + } + gpsd_report(LOG_PROG, "Signal Levels (%d):%s\n",count,buf2); + mask |= SATELLITE_SET; + break; + case 0x48: /* GPS System Message */ + buf[len] = '\0'; + gpsd_report(LOG_PROG, "GPS System Message: %s\n",buf); + break; + case 0x4a: /* Single-Precision Position LLA */ + if (len != 20) + break; + session->gpsdata.fix.latitude = getbef(buf,0) * RAD_2_DEG; + session->gpsdata.fix.longitude = getbef(buf,4) * RAD_2_DEG; + session->gpsdata.fix.altitude = getbef(buf,8); + f1 = getbef(buf,12); /* clock bias */ + f2 = getbef(buf,16); /* time-of-fix */ + if (session->driver.tsip.gps_week) { + session->gpsdata.fix.time = session->gpsdata.sentence_time = + gpstime_to_unix((int)session->driver.tsip.gps_week, f2) - session->context->leap_seconds; + mask |= TIME_SET; + } + gpsd_report(LOG_PROG, "GPS LLA %f %f %f %f\n", + session->gpsdata.fix.time, + session->gpsdata.fix.latitude, + session->gpsdata.fix.longitude, + session->gpsdata.fix.altitude); + mask |= LATLON_SET | ALTITUDE_SET | CYCLE_START_SET; + break; + case 0x4b: /* Machine/Code ID and Additional Status */ + if (len != 3) + break; + u1 = getub(buf,0); /* Machine ID */ + u2 = getub(buf,1); /* Status 1 */ + u3 = getub(buf,2); /* Status 2 */ + gpsd_report(LOG_INF, "Machine ID %02x %02x %02x\n",u1,u2,u3); +#if USE_SUPERPACKET + if ((u3 & 0x01) != (u_int8_t)0 && !session->driver.tsip.superpkt) { + gpsd_report(LOG_PROG, "Switching to Super Packet mode\n"); + + /* set new I/O Options for Super Packet output */ + putbyte(buf,0,0x2c); /* Position: SP, MSL */ + putbyte(buf,1,0x00); /* Velocity: none (via SP) */ + putbyte(buf,2,0x00); /* Time: GPS */ + putbyte(buf,3,0x08); /* Aux: dBHz */ + (void)tsip_write(session, 0x35, buf, 4); + session->driver.tsip.superpkt = true; + } +#endif /* USE_SUPERPACKET */ + break; + case 0x55: /* IO Options */ + if (len != 4) + break; + u1 = getub(buf,0); /* Position */ + u2 = getub(buf,1); /* Velocity */ + u3 = getub(buf,2); /* Timing */ + u4 = getub(buf,3); /* Aux */ + gpsd_report(LOG_INF, "IO Options %02x %02x %02x %02x\n",u1,u2,u3,u4); +#if USE_SUPERPACKET + if ((u1 & 0x20) != (u_int8_t)0) { /* Output Super Packets? */ + /* No LFwEI Super Packet */ + putbyte(buf,0,0x20); + putbyte(buf,1,0x00); /* disabled */ + (void)tsip_write(session, 0x8e, buf, 2); + + /* Request Compact Super Packet */ + putbyte(buf,0,0x23); + putbyte(buf,1,0x01); /* enabled */ + (void)tsip_write(session, 0x8e, buf, 2); + } +#endif /* USE_SUPERPACKET */ + break; + case 0x56: /* Velocity Fix, East-North-Up (ENU) */ + if (len != 20) + break; + f1 = getbef(buf,0); /* East velocity */ + f2 = getbef(buf,4); /* North velocity */ + f3 = getbef(buf,8); /* Up velocity */ + f4 = getbef(buf,12); /* clock bias rate */ + f5 = getbef(buf,16); /* time-of-fix */ + session->gpsdata.fix.climb = f3; + /*@ -evalorder @*/ + session->gpsdata.fix.speed = sqrt(pow(f2,2) + pow(f1,2)); + /*@ +evalorder @*/ + if ((session->gpsdata.fix.track = atan2(f1,f2) * RAD_2_DEG) < 0) + session->gpsdata.fix.track += 360.0; + gpsd_report(LOG_INF, "GPS Velocity ENU %f %f %f %f %f\n",f1,f2,f3,f4,f5); + mask |= SPEED_SET | TRACK_SET | CLIMB_SET; + break; + case 0x57: /* Information About Last Computed Fix */ + if (len != 8) + break; + u1 = getub(buf,0); /* Source of information */ + u2 = getub(buf,1); /* Mfg. diagnostic */ + f1 = getbef(buf,2); /* gps_time */ + s1 = getbesw(buf,6); /* tsip.gps_week */ + /*@ +charint @*/ + if (getub(buf,0) == 0x01) /* good current fix? */ + session->driver.tsip.gps_week = s1; + /*@ -charint @*/ + gpsd_report(LOG_INF, "Fix info %02x %02x %d %f\n",u1,u2,s1,f1); + break; + case 0x58: /* Satellite System Data/Acknowledge from Receiver */ + break; + case 0x59: /* Status of Satellite Disable or Ignore Health */ + break; + case 0x5a: /* Raw Measurement Data */ + if (len != 29) + break; + f1 = getbef(buf,5); /* Signal Level */ + f2 = getbef(buf,9); /* Code phase */ + f3 = getbef(buf,13); /* Doppler */ + d1 = getbed(buf,17); /* Time of Measurement */ + gpsd_report(LOG_PROG, "Raw Measurement Data %d %f %f %f %f\n",getub(buf,0),f1,f2,f3,d1); + break; + case 0x5c: /* Satellite Tracking Status */ + if (len != 24) + break; + session->driver.tsip.last_5c = now; /* keep timestamp for request */ + u1 = getub(buf,0); /* PRN */ + u2 = getub(buf,1); /* chan */ + u3 = getub(buf,2); /* Acquisition flag */ + u4 = getub(buf,3); /* Ephemeris flag */ + f1 = getbef(buf,4); /* Signal level */ + f2 = getbef(buf,8); /* time of Last measurement */ + d1 = getbef(buf,12) * RAD_2_DEG; /* Elevation */ + d2 = getbef(buf,16) * RAD_2_DEG; /* Azimuth */ + i = (int)(u2 >> 3); /* channel number */ + gpsd_report(LOG_INF, "Satellite Tracking Status: Ch %2d PRN %3d Res %d Acq %d Eph %2d SNR %4.1f LMT %.04f El %4.1f Az %5.1f\n",i,u1,u2&7,u3,u4,f1,f2,d1,d2); + if (i < TSIP_CHANNELS) { + if (d1 >= 0.0) { + session->gpsdata.PRN[i] = (int)u1; + session->gpsdata.ss[i] = (int)round(f1); + session->gpsdata.elevation[i] = (int)round(d1); + session->gpsdata.azimuth[i] = (int)round(d2); + } else { + session->gpsdata.PRN[i] = session->gpsdata.ss[i] = + session->gpsdata.elevation[i] = session->gpsdata.azimuth[i] = 0; + } + if (++i == session->gpsdata.satellites) + mask |= SATELLITE_SET; /* last of the series */ + if (i > session->gpsdata.satellites) + session->gpsdata.satellites = i; + } + break; + case 0x6d: /* All-In-View Satellite Selection */ + u1 = getub(buf,0); /* nsvs/dimension */ + count = (int)((u1 >> 4) & 0x0f); + if (len != (17 + count)) + break; + session->driver.tsip.last_6d = now; /* keep timestamp for request */ + switch (u1 & 7) /* dimension */ + { + case 3: + //session->gpsdata.status = STATUS_FIX; + session->gpsdata.fix.mode = MODE_2D; + break; + case 4: + //session->gpsdata.status = STATUS_FIX; + session->gpsdata.fix.mode = MODE_3D; + break; + default: + //session->gpsdata.status = STATUS_NO_FIX; + session->gpsdata.fix.mode = MODE_NO_FIX; + break; + } + session->gpsdata.satellites_used = count; + session->gpsdata.pdop = getbef(buf,1); + session->gpsdata.hdop = getbef(buf,5); + session->gpsdata.vdop = getbef(buf,9); + session->gpsdata.tdop = getbef(buf,13); + /*@ -evalorder @*/ + session->gpsdata.gdop = sqrt(pow(session->gpsdata.pdop,2)+pow(session->gpsdata.tdop,2)); + /*@ +evalorder @*/ + + memset(session->gpsdata.used,0,sizeof(session->gpsdata.used)); + buf2[0] = '\0'; + /*@ +charint @*/ + for (i = 0; i < count; i++) + (void)snprintf(buf2+strlen(buf2), sizeof(buf2)-strlen(buf2), + " %d",session->gpsdata.used[i] = getub(buf,17 + i)); + /*@ -charint @*/ + + gpsd_report(LOG_INF, "Sat info: mode %d, satellites used %d: %s\n", + session->gpsdata.fix.mode, session->gpsdata.satellites_used,buf2); + gpsd_report(LOG_INF, + "Sat info: DOP P=%.1f H=%.1f V=%.1f T=%.1f G=%.1f\n", + session->gpsdata.pdop, session->gpsdata.hdop, + session->gpsdata.vdop, session->gpsdata.tdop, + session->gpsdata.gdop); + mask |= HDOP_SET | VDOP_SET | PDOP_SET | TDOP_SET | GDOP_SET | STATUS_SET | MODE_SET | USED_SET; + break; + case 0x6e: /* Synchronized Measurements */ + break; + case 0x6f: /* Synchronized Measurements Report */ + /*@ +charint @*/ + if (len < 20 || getub(buf,0) != 1 || getub(buf,1) != 2) + break; + /*@ -charint @*/ + s1 = getbesw(buf,2); /* number of bytes */ + u1 = getub(buf,20); /* number of SVs */ + break; + case 0x70: /* Filter Report */ + break; + case 0x7a: /* NMEA settings */ + break; + case 0x82: /* Differential Position Fix Mode */ + if (len != 1) + break; + u1 = getub(buf,0); /* fix mode */ + /*@ +charint @*/ + if (session->gpsdata.status == STATUS_FIX && (u1 & 0x01)!=0) { + session->gpsdata.status = STATUS_DGPS_FIX; + mask |= STATUS_SET; + } + /*@ -charint @*/ + gpsd_report(LOG_INF, "DGPS mode %d\n",u1); + break; + case 0x83: /* Double-Precision XYZ Position Fix and Bias Information */ + if (len != 36) + break; + d1 = getbed(buf,0); /* X */ + d2 = getbed(buf,8); /* Y */ + d3 = getbed(buf,16); /* Z */ + d4 = getbed(buf,24); /* clock bias */ + f1 = getbef(buf,32); /* time-of-fix */ + gpsd_report(LOG_INF, "GPS Position XYZ %f %f %f %f %f\n",d1,d2,d3,d4,f1); + break; + case 0x84: /* Double-Precision LLA Position Fix and Bias Information */ + if (len != 36) + break; + session->gpsdata.fix.latitude = getbed(buf,0) * RAD_2_DEG; + session->gpsdata.fix.longitude = getbed(buf,8) * RAD_2_DEG; + session->gpsdata.fix.altitude = getbed(buf,16); + d1 = getbed(buf,24); /* clock bias */ + f1 = getbef(buf,32); /* time-of-fix */ + if (session->driver.tsip.gps_week) { + session->gpsdata.fix.time = session->gpsdata.sentence_time = + gpstime_to_unix((int)session->driver.tsip.gps_week, f1) - session->context->leap_seconds; + mask |= TIME_SET; + } + gpsd_report(LOG_INF, "GPS DP LLA %f %f %f %f\n", + session->gpsdata.fix.time, + session->gpsdata.fix.latitude, + session->gpsdata.fix.longitude, + session->gpsdata.fix.altitude); + mask |= LATLON_SET | ALTITUDE_SET | CYCLE_START_SET; + break; + case 0x8f: /* Super Packet. Well... */ + /*@ +charint @*/ + (void)snprintf(session->gpsdata.tag+strlen(session->gpsdata.tag), + sizeof(session->gpsdata.tag)-strlen(session->gpsdata.tag), + "%02x", u1 = getub(buf,0)); + /*@ -charint @*/ + switch (u1) /* sub-packet ID */ + { + case 0x15: /* Current Datum Values */ + if (len != 43) + break; + s1 = getbesw(buf,1); /* Datum Index */ + d1 = getbed(buf,3); /* DX */ + d2 = getbed(buf,11); /* DY */ + d3 = getbed(buf,19); /* DZ */ + d4 = getbed(buf,27); /* A-axis */ + d5 = getbed(buf,35); /* Eccentricity Squared */ + gpsd_report(LOG_INF, "Current Datum %d %f %f %f %f %f\n",s1,d1,d2,d3,d4,d5); + break; + + case 0x20: /* Last Fix with Extra Information (binary fixed point) */ + /* XXX CSK sez "why does my Lassen iQ output oversize packets?" */ + if ((len != 56) && (len != 64)) + break; + s1 = getbesw(buf,2); /* east velocity */ + s2 = getbesw(buf,4); /* north velocity */ + s3 = getbesw(buf,6); /* up velocity */ + ul1 = getbeul(buf,8); /* time */ + sl1 = getbesl(buf,12); /* latitude */ + ul2 = getbeul(buf,16); /* longitude */ + sl2 = getbesl(buf,20); /* altitude */ + u1 = getub(buf,24); /* velocity scaling */ + u2 = getub(buf,27); /* fix flags */ + u3 = getub(buf,28); /* num svs */ + u4 = getub(buf,29); /* utc offset */ + s4 = getbesw(buf,30); /* tsip.gps_week */ + /* PRN/IODE data follows */ + gpsd_report(LOG_RAW, "LFwEI %d %d %d %u %d %u %u %x %x %u %u %d\n",s1,s2,s3,ul1,sl1,ul2,sl2,u1,u2,u3,u4,s4); + + if ((u1 & 0x01) != (u_int8_t)0) /* check velocity scaling */ + d5 = 0.02; + else + d5 = 0.005; + d1 = s1 * d5; /* east velocity m/s */ + d2 = s2 * d5; /* north velocity m/s */ + session->gpsdata.fix.climb = s3 * d5; /* up velocity m/s */ + /*@ -evalorder @*/ + session->gpsdata.fix.speed = sqrt(pow(d2,2) + pow(d1,2)); + /*@ +evalorder @*/ + if ((session->gpsdata.fix.track = atan2(d1,d2) * RAD_2_DEG) < 0) + session->gpsdata.fix.track += 360.0; + session->gpsdata.fix.latitude = sl1 * SEMI_2_DEG; + if ((session->gpsdata.fix.longitude = ul2 * SEMI_2_DEG) > 180.0) + session->gpsdata.fix.longitude -= 360.0; + session->gpsdata.separation = wgs84_separation(session->gpsdata.fix.latitude, session->gpsdata.fix.longitude); + session->gpsdata.fix.altitude = sl2 * 1e-3 - session->gpsdata.separation;; + session->gpsdata.status = STATUS_NO_FIX; + session->gpsdata.fix.mode = MODE_NO_FIX; + if ((u2 & 0x01) == (u_int8_t)0) { /* Fix Available */ + session->gpsdata.status = STATUS_FIX; + if ((u2 & 0x02) != (u_int8_t)0) /* DGPS Corrected */ + session->gpsdata.status = STATUS_DGPS_FIX; + if ((u2 & 0x04) != (u_int8_t)0) /* Fix Dimension */ + session->gpsdata.fix.mode = MODE_2D; + else + session->gpsdata.fix.mode = MODE_3D; + } + session->gpsdata.satellites_used = (int)u3; + if ((int)u4 > 10) { + session->context->leap_seconds = (int)u4; + session->context->valid |= LEAP_SECOND_VALID; + } + session->driver.tsip.gps_week = s4; + session->gpsdata.fix.time = session->gpsdata.sentence_time = + gpstime_to_unix((int)s4, ul1 * 1e-3) - session->context->leap_seconds; + mask |= TIME_SET | LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET | CLIMB_SET | STATUS_SET | MODE_SET | CYCLE_START_SET; + break; + case 0x23: /* Compact Super Packet */ + /* XXX CSK sez "i don't trust this to not be oversized either." */ + if (len < 29) + break; + ul1 = getbeul(buf,1); /* time */ + s1 = getbesw(buf,5); /* tsip.gps_week */ + u1 = getub(buf,7); /* utc offset */ + u2 = getub(buf,8); /* fix flags */ + sl1 = getbesl(buf,9); /* latitude */ + ul2 = getbeul(buf,13); /* longitude */ + sl3 = getbesl(buf,17); /* altitude */ + s2 = getbesw(buf,21); /* east velocity */ + s3 = getbesw(buf,23); /* north velocity */ + s4 = getbesw(buf,25); /* up velocity */ + gpsd_report(LOG_INF, "CSP %u %d %u %u %d %u %d %d %d %d\n",ul1,s1,u1,u2,sl1,ul2,sl3,s2,s3,s4); + session->driver.tsip.gps_week = s1; + if ((int)u1 > 10) { + session->context->leap_seconds = (int)u1; + session->context->valid |= LEAP_SECOND_VALID; + } + session->gpsdata.fix.time = session->gpsdata.sentence_time = + gpstime_to_unix((int)s1, ul1 * 1e-3) - session->context->leap_seconds; + session->gpsdata.status = STATUS_NO_FIX; + session->gpsdata.fix.mode = MODE_NO_FIX; + if ((u2 & 0x01) == (u_int8_t)0) { /* Fix Available */ + session->gpsdata.status = STATUS_FIX; + if ((u2 & 0x02) != (u_int8_t)0) /* DGPS Corrected */ + session->gpsdata.status = STATUS_DGPS_FIX; + if ((u2 & 0x04) != (u_int8_t)0) /* Fix Dimension */ + session->gpsdata.fix.mode = MODE_2D; + else + session->gpsdata.fix.mode = MODE_3D; + } + session->gpsdata.fix.latitude = sl1 * SEMI_2_DEG; + if ((session->gpsdata.fix.longitude = ul2 * SEMI_2_DEG) > 180.0) + session->gpsdata.fix.longitude -= 360.0; + session->gpsdata.separation = wgs84_separation(session->gpsdata.fix.latitude, session->gpsdata.fix.longitude); + session->gpsdata.fix.altitude = sl3 * 1e-3 - session->gpsdata.separation;; + if ((u2 & 0x20) != (u_int8_t)0) /* check velocity scaling */ + d5 = 0.02; + else + d5 = 0.005; + d1 = s2 * d5; /* east velocity m/s */ + d2 = s3 * d5; /* north velocity m/s */ + session->gpsdata.fix.climb = s4 * d5; /* up velocity m/s */ + /*@ -evalorder @*/ + session->gpsdata.fix.speed = sqrt(pow(d2,2) + pow(d1,2)) * MPS_TO_KNOTS; + /*@ +evalorder @*/ + if ((session->gpsdata.fix.track = atan2(d1,d2) * RAD_2_DEG) < 0) + session->gpsdata.fix.track += 360.0; + mask |= TIME_SET | LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET | CLIMB_SET | STATUS_SET | MODE_SET | CYCLE_START_SET; + break; + + + case 0xab: /* Thunderbolt Timing Superpacket */ + if (len != 17) { + gpsd_report(4, "pkt 0xab len=%d\n", len); + break; + } + session->driver.tsip.last_41 = now; /* keep timestamp for request */ + ul1 = getbeul(buf,1); /* gpstime */ + s1 = (short)getbeuw(buf,5); /* week */ + s2 = getbesw(buf,7); /* leap seconds */ + + session->driver.tsip.gps_week = s1; + if ((int)u1 > 10) { + session->context->leap_seconds = (int)s2; + session->context->valid |= LEAP_SECOND_VALID; + + session->gpsdata.fix.time = session->gpsdata.sentence_time = + gpstime_to_unix((int)s1, (double)ul1) - (double)s2; +#ifdef NTPSHM_ENABLE + if (session->context->enable_ntpshm) + (void)ntpshm_put(session,session->gpsdata.sentence_time+0.075); +#endif + mask |= TIME_SET; + } + + gpsd_report(4, "GPS Time %u %d %d\n", ul1, s1, s2); + break; + + + case 0xac: /* Thunderbolt Position Superpacket */ + if (len != 68) { + gpsd_report(4, "pkt 0xac len=%d\n", len); + + break; + } + session->gpsdata.fix.latitude = getbed(buf,36) * RAD_2_DEG; + session->gpsdata.fix.longitude = getbed(buf,44) * RAD_2_DEG; + session->gpsdata.fix.altitude = getbed(buf,52); + f1 = getbef(buf,16); /* clock bias */ + + u1 = getub(buf, 12); /* GPS Decoding Status */ + u2 = getub(buf, 1); /* Reciever Mode */ + if (u1 != (u_int8_t)0) { + session->gpsdata.status = STATUS_NO_FIX; + mask |= STATUS_SET; + } + else { + if (session->gpsdata.status < STATUS_FIX) { + session->gpsdata.status = STATUS_FIX; + mask |= STATUS_SET; + } + } + + /* Decode Fix modes */ + switch (u2 & 7) { + case 6: /* Clock Hold 2D */ + case 3: /* 2D Position Fix */ + //session->gpsdata.status = STATUS_FIX; + session->gpsdata.fix.mode = MODE_2D; + break; + case 7: /* Thunderbolt overdetermined clock */ + case 4: /* 3D position Fix */ + //session->gpsdata.status = STATUS_FIX; + session->gpsdata.fix.mode = MODE_3D; + break; + default: + //session->gpsdata.status = STATUS_NO_FIX; + session->gpsdata.fix.mode = MODE_NO_FIX; + break; + } + + gpsd_report(4, "GPS status=%u mode=%u\n", u1, u2); + gpsd_report(4, "GPS DP LLA %f %f %f\n",session->gpsdata.fix.latitude, + session->gpsdata.fix.longitude, + session->gpsdata.fix.altitude); + + mask |= LATLON_SET | ALTITUDE_SET | CYCLE_START_SET | STATUS_SET | MODE_SET; + break; + + + default: + gpsd_report(LOG_WARN,"Unhandled TSIP superpacket type 0x%02x\n",u1); + } + break; + case 0xbb: /* Navigation Configuration */ + if (len != 40) + break; + u1 = getub(buf,0); /* Subcode */ + u2 = getub(buf,1); /* Operating Dimension */ + u3 = getub(buf,2); /* DGPS Mode */ + u4 = getub(buf,3); /* Dynamics Code */ + f1 = getbef(buf,5); /* Elevation Mask */ + f2 = getbef(buf,9); /* AMU Mask */ + f3 = getbef(buf,13); /* DOP Mask */ + f4 = getbef(buf,17); /* DOP Switch */ + u5 = getub(buf,21); /* DGPS Age Limit */ + gpsd_report(LOG_INF, "Navigation Configuration %u %u %u %u %f %f %f %f %u\n",u1,u2,u3,u4,f1,f2,f3,f4,u5); + break; + default: + gpsd_report(LOG_WARN,"Unhandled TSIP packet type 0x%02x\n",id); + break; + } + + /* see if it is time to send some request packets for reports that */ + /* the receiver won't send at fixed intervals */ + + if ((now - session->driver.tsip.last_41) > 5) { + /* Request Current Time */ + (void)tsip_write(session, 0x21, buf, 0); + session->driver.tsip.last_41 = now; + } + + if ((now - session->driver.tsip.last_6d) > 5) { + /* Request GPS Receiver Position Fix Mode */ + (void)tsip_write(session, 0x24, buf, 0); + session->driver.tsip.last_6d = now; + } + + if ((now - session->driver.tsip.last_5c) >= 5) { + /* Request Current Satellite Tracking Status */ + putbyte(buf,0,0x00); /* All satellites */ + (void)tsip_write(session, 0x3c, buf, 1); + session->driver.tsip.last_5c = now; + } + + if ((now - session->driver.tsip.last_46) > 5) { + /* Request Health of Receiver */ + (void)tsip_write(session, 0x26, buf, 0); + session->driver.tsip.last_46 = now; + } + + return mask; +} + +static gps_mask_t tsip_parse_input(struct gps_device_t *session) +{ + gps_mask_t st; + + if (session->packet.type == TSIP_PACKET){ + st = tsip_analyze(session); + session->gpsdata.driver_mode = MODE_BINARY; + return st; +#ifdef EVERMORE_ENABLE + } else if (session->packet.type == EVERMORE_PACKET) { + (void)gpsd_switch_driver(session, "EverMore binary"); + st = evermore_parse(session, session->packet.outbuffer, session->packet.outbuflen); + session->gpsdata.driver_mode = MODE_BINARY; + return st; +#endif /* EVERMORE_ENABLE */ + } else + return 0; +} + +/* this is everything we export */ +const struct gps_type_t tsip_binary = +{ + .type_name = "Trimble TSIP", /* full name of type */ + .packet_type = TSIP_PACKET, /* associated lexer packet type */ + .trigger = NULL, /* no trigger */ + .channels = TSIP_CHANNELS, /* consumer-grade GPS */ + .control_send = tsip_control_send,/* how to send commands */ + .probe_wakeup = NULL, /* no wakeup to be done before hunt */ + .probe_detect = NULL, /* no probe */ + .probe_subtype = tsip_probe_subtype, /* no more subtype discovery */ +#ifdef ALLOW_RECONFIGURE + .configurator = tsip_configurator,/* initial mode sets */ +#endif /* ALLOW_RECONFIGURE */ + .get_packet = generic_get, /* use the generic packet getter */ + .parse_packet = tsip_parse_input, /* parse message packets */ + .rtcm_writer = NULL, /* doesn't accept DGPS corrections */ + .speed_switcher = tsip_speed_switch,/* change baud rate */ + .mode_switcher = NULL, /* no mode switcher */ + .rate_switcher = NULL, /* no rate switcher */ + .cycle_chars = -1, /* not relevant, no rate switcher */ +#ifdef ALLOW_RECONFIGURE + .revert = NULL, /* FIXME: revert sentence mix */ +#endif /* ALLOW_RECONFIGURE */ + .wrapup = tsip_wrapup, /* restore comms parameters */ + .cycle = 1, /* updates every second */ +}; + +#endif /* TSIP_ENABLE */ diff --git a/driver_ubx.c b/driver_ubx.c new file mode 100644 index 00000000..f3757aba --- /dev/null +++ b/driver_ubx.c @@ -0,0 +1,688 @@ +/* $Id$ + * + * UBX driver + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "gpsd_config.h" +#include "gpsd.h" +#if defined(UBX_ENABLE) && defined(BINARY_ENABLE) +#include "driver_ubx.h" + +#include "bits.h" + +/* + * A ubx packet looks like this: + * leader: 0xb5 0x62 + * message class: 1 byte + * message type: 1 byte + * length of payload: 2 bytes + * payload: variable length + * checksum: 2 bytes + * + * see also the FV25 and UBX documents on reference.html + */ + +static bool have_port_configuration = false; +static unsigned char original_port_settings[20]; +static unsigned char sbas_in_use; + + bool ubx_write(struct gps_device_t *session, unsigned int msg_class, unsigned int msg_id, unsigned char *msg, unsigned short data_len); + gps_mask_t ubx_parse(struct gps_device_t *session, unsigned char *buf, size_t len); + void ubx_catch_model(struct gps_device_t *session, unsigned char *buf, size_t len); +static gps_mask_t ubx_msg_nav_sol(struct gps_device_t *session, unsigned char *buf, size_t data_len); +static gps_mask_t ubx_msg_nav_dop(struct gps_device_t *session, unsigned char *buf, size_t data_len); +static gps_mask_t ubx_msg_nav_timegps(struct gps_device_t *session, unsigned char *buf, size_t data_len); +static gps_mask_t ubx_msg_nav_svinfo(struct gps_device_t *session, unsigned char *buf, size_t data_len); +static void ubx_msg_sbas(unsigned char *buf); +static void ubx_msg_inf(unsigned char *buf, size_t data_len); + +/** + * Navigation solution message + */ +static gps_mask_t +ubx_msg_nav_sol(struct gps_device_t *session, unsigned char *buf, size_t data_len) +{ + unsigned short gw; + unsigned int tow, flags; + double epx, epy, epz, evx, evy, evz; + unsigned char navmode; + gps_mask_t mask; + double t; + + if (data_len != 52) + return 0; + + flags = (unsigned int)getub(buf, 11); + mask = ONLINE_SET; + if ((flags & (UBX_SOL_VALID_WEEK |UBX_SOL_VALID_TIME)) != 0){ + tow = getleul(buf, 0); + gw = (unsigned short)getlesw(buf, 8); + session->driver.ubx.gps_week = gw; + + t = gpstime_to_unix((int)session->driver.ubx.gps_week, tow/1000.0) - session->context->leap_seconds; + session->gpsdata.sentence_time = t; + session->gpsdata.fix.time = t; + mask |= TIME_SET; +#ifdef NTPSHM_ENABLE + /* TODO overhead */ + if (session->context->enable_ntpshm) + (void)ntpshm_put(session, session->gpsdata.sentence_time); +#endif + } + + epx = (double)(getlesl(buf, 12)/100.0); + epy = (double)(getlesl(buf, 16)/100.0); + epz = (double)(getlesl(buf, 20)/100.0); + evx = (double)(getlesl(buf, 28)/100.0); + evy = (double)(getlesl(buf, 32)/100.0); + evz = (double)(getlesl(buf, 36)/100.0); + ecef_to_wgs84fix(&session->gpsdata, epx, epy, epz, evx, evy, evz); + mask |= LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET | CLIMB_SET; + session->gpsdata.fix.eph = (double)(getlesl(buf, 24)/100.0); + session->gpsdata.fix.eps = (double)(getlesl(buf, 40)/100.0); + session->gpsdata.pdop = (double)(getleuw(buf, 44)/100.0); + session->gpsdata.satellites_used = (int)getub(buf, 47); + mask |= PDOP_SET ; + + navmode = getub(buf, 10); + switch (navmode){ + case UBX_MODE_TMONLY: + case UBX_MODE_3D: + session->gpsdata.fix.mode = MODE_3D; + break; + case UBX_MODE_2D: + case UBX_MODE_DR: /* consider this too as 2D */ + case UBX_MODE_GPSDR: /* XXX DR-aided GPS may be valid 3D */ + session->gpsdata.fix.mode = MODE_2D; + break; + default: + session->gpsdata.fix.mode = MODE_NO_FIX; + } + + if ((flags & UBX_SOL_FLAG_DGPS) != 0) + session->gpsdata.status = STATUS_DGPS_FIX; + else if (session->gpsdata.fix.mode != MODE_NO_FIX) + session->gpsdata.status = STATUS_FIX; + + mask |= MODE_SET | STATUS_SET | CYCLE_START_SET | USED_SET ; + + return mask; +} + +/** + * Dilution of precision message + */ +static gps_mask_t +ubx_msg_nav_dop(struct gps_device_t *session, unsigned char *buf, size_t data_len) +{ + if (data_len != 18) + return 0; + + session->gpsdata.gdop = (double)(getleuw(buf, 4)/100.0); + session->gpsdata.pdop = (double)(getleuw(buf, 6)/100.0); + session->gpsdata.tdop = (double)(getleuw(buf, 8)/100.0); + session->gpsdata.vdop = (double)(getleuw(buf, 10)/100.0); + session->gpsdata.hdop = (double)(getleuw(buf, 12)/100.0); + + return DOP_SET; +} + +/** + * GPS Leap Seconds + */ +static gps_mask_t +ubx_msg_nav_timegps(struct gps_device_t *session, unsigned char *buf, size_t data_len) +{ + unsigned int gw, tow, flags; + double t; + + if (data_len != 16) + return 0; + + tow = getleul(buf, 0); + gw = (uint)getlesw(buf, 8); + if (gw > session->driver.ubx.gps_week) + session->driver.ubx.gps_week = gw; + + flags = (unsigned int)getub(buf, 11); + if ((flags & 0x7) != 0) + session->context->leap_seconds = (int)getub(buf, 10); + + t = gpstime_to_unix((int)session->driver.ubx.gps_week, tow/1000.0) - session->context->leap_seconds; + session->gpsdata.sentence_time = session->gpsdata.fix.time = t; + + return TIME_SET | ONLINE_SET; +} + +/** + * GPS Satellite Info + */ +static gps_mask_t +ubx_msg_nav_svinfo(struct gps_device_t *session, unsigned char *buf, size_t data_len) +{ + unsigned int i, j, tow, nchan, nsv, st; + + if (data_len < 152 ) { + gpsd_report(LOG_PROG, "runt svinfo (datalen=%zd)\n", data_len); + return 0; + } + tow = getleul(buf, 0); +// session->gpsdata.sentence_time = gpstime_to_unix(gps_week, tow) +// - session->context->leap_seconds; + /*@ +charint @*/ + nchan = getub(buf, 4); + if (nchan > MAXCHANNELS){ + gpsd_report(LOG_WARN, "Invalid NAV SVINFO message, >%d reported",MAXCHANNELS); + return 0; + } + /*@ -charint @*/ + gpsd_zero_satellites(&session->gpsdata); + nsv = 0; + for (i = j = st = 0; i < nchan; i++) { + unsigned int off = 8 + 12 * i; + if((int)getub(buf, off+4) == 0) continue; /* LEA-5H seems to have a bug reporting sats it does not see or hear*/ + session->gpsdata.PRN[j] = (int)getub(buf, off+1); + session->gpsdata.ss[j] = (int)getub(buf, off+4); + session->gpsdata.elevation[j] = (int)getsb(buf, off+5); + session->gpsdata.azimuth[j] = (int)getlesw(buf, off+6); + if(session->gpsdata.PRN[j]) + st++; + /*@ -predboolothers */ + if (getub(buf, off+2) & 0x01) + session->gpsdata.used[nsv++] = session->gpsdata.PRN[j]; + if (session->gpsdata.PRN[j] == (int)sbas_in_use) + session->gpsdata.used[nsv++] = session->gpsdata.PRN[j]; + /*@ +predboolothers */ + j++; + } + session->gpsdata.satellites = (int)st; + session->gpsdata.satellites_used = (int)nsv; + return SATELLITE_SET | USED_SET; +} + +/* + * SBAS Info + */ +static void +ubx_msg_sbas(unsigned char *buf) +{ +#ifdef UBX_SBAS_DEBUG + unsigned int i, nsv; + + gpsd_report(LOG_WARN, "SBAS: %d %d %d %d %d\n", + (int)getub(buf, 4), (int)getub(buf, 5), (int)getub(buf, 6), (int)getub(buf, 7), (int)getub(buf, 8)); + + nsv = (int)getub(buf, 8); + for (i = 0; i < nsv; i++) { + int off = 12 + 12 * i; + gpsd_report(LOG_WARN, "SBAS info on SV: %d\n", (int)getub(buf, off)); + } +#endif +/* really 'in_use' depends on the sats info, EGNOS is still in test */ +/* In WAAS areas one might also check for the type of corrections indicated */ + sbas_in_use = getub(buf, 4); +} + +static void +ubx_msg_inf(unsigned char *buf, size_t data_len) +{ + unsigned short msgid; + static char txtbuf[MAX_PACKET_LENGTH]; + + msgid = (unsigned short)((buf[2] << 8) | buf[3]); + if (data_len > MAX_PACKET_LENGTH-1) + data_len = MAX_PACKET_LENGTH-1; + + (void)strlcpy(txtbuf, (char *)buf+6, MAX_PACKET_LENGTH); txtbuf[data_len] = '\0'; + switch (msgid) { + case UBX_INF_DEBUG: + gpsd_report(LOG_PROG, "UBX_INF_DEBUG: %s\n", txtbuf); + break; + case UBX_INF_TEST: + gpsd_report(LOG_PROG, "UBX_INF_TEST: %s\n", txtbuf); + break; + case UBX_INF_NOTICE: + gpsd_report(LOG_INF, "UBX_INF_NOTICE: %s\n", txtbuf); + break; + case UBX_INF_WARNING: + gpsd_report(LOG_WARN, "UBX_INF_WARNING: %s\n", txtbuf); + break; + case UBX_INF_ERROR: + gpsd_report(LOG_WARN, "UBX_INF_ERROR: %s\n", txtbuf); + break; + default: + break; + } + return ; +} + +/*@ +charint @*/ +gps_mask_t ubx_parse(struct gps_device_t *session, unsigned char *buf, size_t len) +{ + size_t data_len; + unsigned short msgid; + gps_mask_t mask = 0; + int i; + + if (len < 6) /* the packet at least contains a head of six bytes */ + return 0; + + /* extract message id and length */ + msgid = (buf[2] << 8) | buf[3]; + data_len = (size_t)getlesw(buf, 4); + switch (msgid) + { + case UBX_NAV_POSECEF: + gpsd_report(LOG_IO, "UBX_NAV_POSECEF\n"); + break; + case UBX_NAV_POSLLH: + gpsd_report(LOG_IO, "UBX_NAV_POSLLH\n"); + break; + case UBX_NAV_STATUS: + gpsd_report(LOG_IO, "UBX_NAV_STATUS\n"); + break; + case UBX_NAV_DOP: + gpsd_report(LOG_PROG, "UBX_NAV_DOP\n"); + mask = ubx_msg_nav_dop(session, &buf[6], data_len); + break; + case UBX_NAV_SOL: + gpsd_report(LOG_PROG, "UBX_NAV_SOL\n"); + mask = ubx_msg_nav_sol(session, &buf[6], data_len); + break; + case UBX_NAV_POSUTM: + gpsd_report(LOG_IO, "UBX_NAV_POSUTM\n"); + break; + case UBX_NAV_VELECEF: + gpsd_report(LOG_IO, "UBX_NAV_VELECEF\n"); + break; + case UBX_NAV_VELNED: + gpsd_report(LOG_IO, "UBX_NAV_VELNED\n"); + break; + case UBX_NAV_TIMEGPS: + gpsd_report(LOG_PROG, "UBX_NAV_TIMEGPS\n"); + mask = ubx_msg_nav_timegps(session, &buf[6], data_len); + break; + case UBX_NAV_TIMEUTC: + gpsd_report(LOG_IO, "UBX_NAV_TIMEUTC\n"); + break; + case UBX_NAV_CLOCK: + gpsd_report(LOG_IO, "UBX_NAV_CLOCK\n"); + break; + case UBX_NAV_SVINFO: + gpsd_report(LOG_PROG, "UBX_NAV_SVINFO\n"); + mask = ubx_msg_nav_svinfo(session, &buf[6], data_len); + break; + case UBX_NAV_DGPS: + gpsd_report(LOG_IO, "UBX_NAV_DGPS\n"); + break; + case UBX_NAV_SBAS: + gpsd_report(LOG_IO, "UBX_NAV_SBAS\n"); + ubx_msg_sbas(&buf[6]); + break; + case UBX_NAV_EKFSTATUS: + gpsd_report(LOG_IO, "UBX_NAV_EKFSTATUS\n"); + break; + + case UBX_RXM_RAW: + gpsd_report(LOG_IO, "UBX_RXM_RAW\n"); + break; + case UBX_RXM_SFRB: + gpsd_report(LOG_IO, "UBX_RXM_SFRB\n"); + break; + case UBX_RXM_SVSI: + gpsd_report(LOG_PROG, "UBX_RXM_SVSI\n"); + break; + case UBX_RXM_ALM: + gpsd_report(LOG_IO, "UBX_RXM_ALM\n"); + break; + case UBX_RXM_EPH: + gpsd_report(LOG_IO, "UBX_RXM_EPH\n"); + break; + case UBX_RXM_POSREQ: + gpsd_report(LOG_IO, "UBX_RXM_POSREQ\n"); + break; + + case UBX_MON_SCHED: + gpsd_report(LOG_IO, "UBX_MON_SCHED\n"); + break; + case UBX_MON_IO: + gpsd_report(LOG_IO, "UBX_MON_IO\n"); + break; + case UBX_MON_IPC: + gpsd_report(LOG_IO, "UBX_MON_IPC\n"); + break; + case UBX_MON_VER: + gpsd_report(LOG_IO, "UBX_MON_VER\n"); + break; + case UBX_MON_EXCEPT: + gpsd_report(LOG_IO, "UBX_MON_EXCEPT\n"); + break; + case UBX_MON_MSGPP: + gpsd_report(LOG_IO, "UBX_MON_MSGPP\n"); + break; + case UBX_MON_RXBUF: + gpsd_report(LOG_IO, "UBX_MON_RXBUF\n"); + break; + case UBX_MON_TXBUF: + gpsd_report(LOG_IO, "UBX_MON_TXBUF\n"); + break; + case UBX_MON_HW: + gpsd_report(LOG_IO, "UBX_MON_HW\n"); + break; + case UBX_MON_USB: + gpsd_report(LOG_IO, "UBX_MON_USB\n"); + break; + + case UBX_INF_DEBUG: + /* FALLTHROUGH */ + case UBX_INF_TEST: + /* FALLTHROUGH */ + case UBX_INF_NOTICE: + /* FALLTHROUGH */ + case UBX_INF_WARNING: + /* FALLTHROUGH */ + case UBX_INF_ERROR: + ubx_msg_inf(buf, data_len); + break; + + case UBX_TIM_TP: + gpsd_report(LOG_IO, "UBX_TIM_TP\n"); + break; + case UBX_TIM_TM: + gpsd_report(LOG_IO, "UBX_TIM_TM\n"); + break; + case UBX_TIM_TM2: + gpsd_report(LOG_IO, "UBX_TIM_TM2\n"); + break; + case UBX_TIM_SVIN: + gpsd_report(LOG_IO, "UBX_TIM_SVIN\n"); + break; + + case UBX_CFG_PRT: + gpsd_report(LOG_IO, "UBX_CFG_PRT\n"); + for(i=6;i<26;i++) + original_port_settings[i-6] = buf[i]; /* copy the original port settings */ + buf[14+6] &= ~0x02; /* turn off NMEA output on this port */ + (void)ubx_write(session, 0x06, 0x00, &buf[6], 20); /* send back with all other settings intact */ + have_port_configuration = true; + break; + + case UBX_ACK_NAK: + gpsd_report(LOG_IO, "UBX_ACK_NAK, class: %02x, id: %02x\n", buf[6], buf[7]); + break; + case UBX_ACK_ACK: + gpsd_report(LOG_IO, "UBX_ACK_ACK, class: %02x, id: %02x\n", buf[6], buf[7]); + break; + + default: + gpsd_report(LOG_WARN, + "UBX: unknown packet id 0x%04hx (length %zd) %s\n", + msgid, len, gpsd_hexdump_wrapper(buf, len, LOG_WARN)); + } + + if (mask) + (void)snprintf(session->gpsdata.tag, sizeof(session->gpsdata.tag), + "0x%04hx", msgid); + + return mask | ONLINE_SET; +} +/*@ -charint @*/ + +static gps_mask_t parse_input(struct gps_device_t *session) +{ + gps_mask_t st; + + if (session->packet.type == UBX_PACKET){ + st = ubx_parse(session, session->packet.outbuffer, session->packet.outbuflen); + session->gpsdata.driver_mode = MODE_BINARY; + return st; +#ifdef NMEA_ENABLE + } else if (session->packet.type == NMEA_PACKET) { + st = nmea_parse((char *)session->packet.outbuffer, session); + session->gpsdata.driver_mode = MODE_NMEA; + return st; +#endif /* NMEA_ENABLE */ + } else + return 0; +} + +void ubx_catch_model(struct gps_device_t *session, unsigned char *buf, size_t len) +{ + /*@ +charint */ + unsigned char *ip = &buf[19]; + unsigned char *op = (unsigned char *)session->subtype; + size_t end = ((len - 19) < 63)?(len - 19):63; + size_t i; + + for(i=0;imsgbuf[0] = 0xb5; + session->msgbuf[1] = 0x62; + + CK_A = CK_B = 0; + session->msgbuf[2] = msg_class; + session->msgbuf[3] = msg_id; + session->msgbuf[4] = data_len & 0xff; + session->msgbuf[5] = (data_len >> 8) & 0xff; + + assert(msg != NULL || data_len == 0); + if (msg != NULL) + (void)memcpy(&session->msgbuf[6], msg, data_len); + + /* calculate CRC */ + for (i = 2; i < 6; i++) { + CK_A += session->msgbuf[i]; + CK_B += CK_A; + } + /*@ -nullderef @*/ + for (i = 0; i < data_len; i++) { + CK_A += msg[i]; + CK_B += CK_A; + } + + session->msgbuf[6 + data_len] = CK_A; + session->msgbuf[7 + data_len] = CK_B; + session->msgbuflen = data_len + 7; + /*@ +type @*/ + + gpsd_report(LOG_IO, + "=> GPS: UBX class: %02x, id: %02x, len: %d, data:%s, crc: %02x%02x\n", + msg_class, msg_id, data_len, + gpsd_hexdump_wrapper(msg, (size_t)data_len, LOG_IO), + CK_A, CK_B); + + count = write(session->gpsdata.gps_fd, + session->msgbuf, session->msgbuflen); + (void)tcdrain(session->gpsdata.gps_fd); + ok = (count == (ssize_t)session->msgbuflen); + /*@ +nullderef @*/ + return(ok); +} + +static ssize_t ubx_control_send(struct gps_device_t *session, char *msg, size_t data_len) +/* not used by gpsd, it's for gpsctl and friends */ +{ + return ubx_write(session, (unsigned int)msg[0], (unsigned int)msg[1], + (unsigned char *)msg+2, (unsigned short)(data_len-2)) ? ((ssize_t)(data_len+7)) : -1; +} + +#ifdef ALLOW_RECONFIGURE +static void ubx_configure(struct gps_device_t *session, unsigned int seq) +{ + unsigned char msg[32]; + + gpsd_report(LOG_IO, "UBX configure: %d\n",seq); + + (void)ubx_write(session, 0x06u, 0x00, NULL, 0); /* get this port's settings */ + + /*@ -type @*/ + msg[0] = 0x03; /* SBAS mode enabled, accept testbed mode */ + msg[1] = 0x07; /* SBAS usage: range, differential corrections and integrity */ + msg[2] = 0x03; /* use the maximun search range: 3 channels */ + msg[3] = 0x00; /* PRN numbers to search for all set to 0 => auto scan */ + msg[4] = 0x00; + msg[5] = 0x00; + msg[6] = 0x00; + msg[7] = 0x00; + (void)ubx_write(session, 0x06u, 0x16, msg, 8); + + msg[0] = 0x01; /* class */ + msg[1] = 0x04; /* msg id = UBX_NAV_DOP */ + msg[2] = 0x01; /* rate */ + (void)ubx_write(session, 0x06u, 0x01, msg, 3); + msg[0] = 0x01; /* class */ + msg[1] = 0x06; /* msg id = NAV-SOL */ + msg[2] = 0x01; /* rate */ + (void)ubx_write(session, 0x06u, 0x01, msg, 3); + msg[0] = 0x01; /* class */ + msg[1] = 0x20; /* msg id = UBX_NAV_TIMEGPS */ + msg[2] = 0x01; /* rate */ + (void)ubx_write(session, 0x06u, 0x01, msg, 3); + msg[0] = 0x01; /* class */ + msg[1] = 0x30; /* msg id = NAV-SVINFO */ + msg[2] = 0x0a; /* rate */ + (void)ubx_write(session, 0x06u, 0x01, msg, 3); + msg[0] = 0x01; /* class */ + msg[1] = 0x32; /* msg id = NAV-SBAS */ + msg[2] = 0x0a; /* rate */ + (void)ubx_write(session, 0x06u, 0x01, msg, 3); + /*@ +type @*/ + +} + +static void ubx_revert(struct gps_device_t *session) +{ + /*@ -type @*/ + unsigned char msg[4] = { + 0x00, 0x00, /* hotstart */ + 0x01, /* controlled software reset */ + 0x00}; /* reserved */ + /*@ +type @*/ + + gpsd_report(LOG_IO, "UBX revert\n"); + + /* Reverting all in one fast and reliable reset */ + (void)ubx_write(session, 0x06, 0x04, msg, 4); /* CFG-RST */ +} +#endif /* ALLOW_RECONFIGURE */ + +static void ubx_nmea_mode(struct gps_device_t *session, int mode) +{ + int i; + unsigned char buf[20]; + + if(!have_port_configuration) + return; + + /*@ +charint -usedef @*/ + for(i=0;i<22;i++) + buf[i] = original_port_settings[i]; /* copy the original port settings */ + if(buf[0] == 0x01) /* set baudrate on serial port only */ + putlelong(buf, 8, session->gpsdata.baudrate); + + if (mode == 0) { + buf[14] &= ~0x01; /* turn off UBX output on this port */ + buf[14] |= 0x02; /* turn on NMEA output on this port */ + } else { + buf[14] &= ~0x02; /* turn off NMEA output on this port */ + buf[14] |= 0x01; /* turn on UBX output on this port */ + } + /*@ -charint +usedef @*/ + (void)ubx_write(session, 0x06u, 0x00, &buf[6], 20); /* send back with all other settings intact */ +} + +static bool ubx_speed(struct gps_device_t *session, + speed_t speed, char parity, int stopbits) +{ + int i; + unsigned char buf[20]; + unsigned long usart_mode; + + /*@ +charint -usedef -compdef */ + if((!have_port_configuration) || (buf[0] != 0x01)) /* set baudrate on serial port only */ + return false; + + for(i=0;i<22;i++) + buf[i] = original_port_settings[i]; /* copy the original port settings */ + usart_mode = getleul(buf, 4); + usart_mode &=~ 0xE00; /* zero bits 11:9 */ + switch (parity) { + case (int)'E': + case 2: + usart_mode |= 0x00; + break; + case (int)'O': + case 1: + usart_mode |= 0x01; + break; + case (int)'N': + case 0: + default: + usart_mode |= 0x4; /* 0x5 would work too */ + break; + } + usart_mode &=~ 0x03000; /* zero bits 13:12 */ + if (stopbits == 2) + usart_mode |= 0x2000; /* zero value means 1 stop bit */ + putlelong(buf, 4, usart_mode); + putlelong(buf, 8, speed); + (void)ubx_write(session, 0x06, 0x00, &buf[6], 20); /* send back with all other settings intact */ + /*@ -charint +usedef +compdef */ + return true; +} + +/* This is everything we export */ +const struct gps_type_t ubx_binary = { + .type_name = "uBlox UBX binary", /* Full name of type */ + .packet_type = UBX_PACKET, /* associated lexer packet type */ + .trigger = NULL, /* Response string that identifies device (not active) */ + .channels = 50, /* Number of satellite channels supported by the device */ + .control_send = ubx_control_send, /* no control sender yet */ + .probe_detect = NULL, /* Startup-time device detector */ + .probe_wakeup = NULL, /* Wakeup to be done before each baud hunt */ + .probe_subtype = NULL, /* Initialize the device and get subtype */ +#ifdef ALLOW_RECONFIGURE + .configurator = ubx_configure, /* Enable what reports we need */ +#endif /* ALLOW_RECONFIGURE */ + .get_packet = generic_get, /* Packet getter (using default routine) */ + .parse_packet = parse_input, /* Parse message packets */ + .rtcm_writer = NULL, /* RTCM handler (using default routine) */ + .speed_switcher = ubx_speed, /* Speed (baudrate) switch */ + .mode_switcher = ubx_nmea_mode, /* Switch to NMEA mode */ + .rate_switcher = NULL, /* Message delivery rate switcher */ + .cycle_chars = -1, /* Number of chars per report cycle */ +#ifdef ALLOW_RECONFIGURE + .revert = ubx_revert, /* Undo the actions of .configurator */ +#endif /* ALLOW_RECONFIGURE */ + .wrapup = NULL, /* Puts device back to original settings */ + .cycle = 1 /* Number of updates per second */ +}; +#endif /* defined(UBX_ENABLE) && defined(BINARY_ENABLE) */ diff --git a/driver_ubx.h b/driver_ubx.h new file mode 100644 index 00000000..e5102967 --- /dev/null +++ b/driver_ubx.h @@ -0,0 +1,101 @@ +/* $Id$ */ +#ifndef _GPSD_UBX_H_ +#define _GPSD_UBX_H_ + +#define UBX_MESSAGE_BASE_SIZE 6 +#define UBX_MESSAGE_DATA_OFFSET UBX_MESSAGE_BASE_SIZE + +typedef enum { + UBX_CLASS_NAV = 0x01, /**< Navigation */ + UBX_CLASS_RXM = 0x02, /**< Receiver Manager */ + UBX_CLASS_INF = 0x04, /**< Informative text messages */ + UBX_CLASS_ACK = 0x05, /**< (Not) Acknowledges for cfg messages */ + UBX_CLASS_CFG = 0x06, /**< Configuration requests */ + UBX_CLASS_UPD = 0x09, /**< Firmware updates */ + UBX_CLASS_MON = 0x0a, /**< System monitoring */ + UBX_CLASS_AID = 0x0b, /**< AGPS */ + UBX_CLASS_TIM = 0x0d, /**< Time */ +} ubx_classes_t; + +#define UBX_MSGID(cls_, id_) (((cls_)<<8)|(id_)) + +typedef enum { + UBX_NAV_POSECEF = UBX_MSGID(UBX_CLASS_NAV, 0x01), + UBX_NAV_POSLLH = UBX_MSGID(UBX_CLASS_NAV, 0x02), + UBX_NAV_STATUS = UBX_MSGID(UBX_CLASS_NAV, 0x03), + UBX_NAV_DOP = UBX_MSGID(UBX_CLASS_NAV, 0x04), + UBX_NAV_SOL = UBX_MSGID(UBX_CLASS_NAV, 0x06), + UBX_NAV_POSUTM = UBX_MSGID(UBX_CLASS_NAV, 0x08), + UBX_NAV_VELECEF = UBX_MSGID(UBX_CLASS_NAV, 0x11), + UBX_NAV_VELNED = UBX_MSGID(UBX_CLASS_NAV, 0x12), + UBX_NAV_TIMEGPS = UBX_MSGID(UBX_CLASS_NAV, 0x20), + UBX_NAV_TIMEUTC = UBX_MSGID(UBX_CLASS_NAV, 0x21), + UBX_NAV_CLOCK = UBX_MSGID(UBX_CLASS_NAV, 0x22), + UBX_NAV_SVINFO = UBX_MSGID(UBX_CLASS_NAV, 0x30), + UBX_NAV_DGPS = UBX_MSGID(UBX_CLASS_NAV, 0x31), + UBX_NAV_SBAS = UBX_MSGID(UBX_CLASS_NAV, 0x32), + UBX_NAV_EKFSTATUS = UBX_MSGID(UBX_CLASS_NAV, 0x40), + + UBX_RXM_RAW = UBX_MSGID(UBX_CLASS_RXM, 0x10), + UBX_RXM_SFRB = UBX_MSGID(UBX_CLASS_RXM, 0x11), + UBX_RXM_SVSI = UBX_MSGID(UBX_CLASS_RXM, 0x20), + UBX_RXM_ALM = UBX_MSGID(UBX_CLASS_RXM, 0x30), + UBX_RXM_EPH = UBX_MSGID(UBX_CLASS_RXM, 0x31), + UBX_RXM_POSREQ = UBX_MSGID(UBX_CLASS_RXM, 0x40), + + UBX_INF_ERROR = UBX_MSGID(UBX_CLASS_INF, 0X00), + UBX_INF_WARNING = UBX_MSGID(UBX_CLASS_INF, 0X01), + UBX_INF_NOTICE = UBX_MSGID(UBX_CLASS_INF, 0x02), + UBX_INF_TEST = UBX_MSGID(UBX_CLASS_INF, 0x03), + UBX_INF_DEBUG = UBX_MSGID(UBX_CLASS_INF, 0x04), + UBX_INF_USER = UBX_MSGID(UBX_CLASS_INF, 0x07), + + UBX_ACK_NAK = UBX_MSGID(UBX_CLASS_ACK, 0x00), + UBX_ACK_ACK = UBX_MSGID(UBX_CLASS_ACK, 0x01), + + UBX_CFG_PRT = UBX_MSGID(UBX_CLASS_CFG, 0x00), + + UBX_UPD_DOWNL = UBX_MSGID(UBX_CLASS_UPD, 0x01), + UBX_UPD_UPLOAD = UBX_MSGID(UBX_CLASS_UPD, 0x02), + UBX_UPD_EXEC = UBX_MSGID(UBX_CLASS_UPD, 0x03), + UBX_UPD_MEMCPY = UBX_MSGID(UBX_CLASS_UPD, 0x04), + + UBX_MON_SCHED = UBX_MSGID(UBX_CLASS_MON, 0x01), + UBX_MON_IO = UBX_MSGID(UBX_CLASS_MON, 0x02), + UBX_MON_IPC = UBX_MSGID(UBX_CLASS_MON, 0x03), + UBX_MON_VER = UBX_MSGID(UBX_CLASS_MON, 0x04), + UBX_MON_EXCEPT = UBX_MSGID(UBX_CLASS_MON, 0x05), + UBX_MON_MSGPP = UBX_MSGID(UBX_CLASS_MON, 0x06), + UBX_MON_RXBUF = UBX_MSGID(UBX_CLASS_MON, 0x07), + UBX_MON_TXBUF = UBX_MSGID(UBX_CLASS_MON, 0x08), + UBX_MON_HW = UBX_MSGID(UBX_CLASS_MON, 0x09), + UBX_MON_USB = UBX_MSGID(UBX_CLASS_MON, 0x0a), + + UBX_AID_REQ = UBX_MSGID(UBX_CLASS_AID, 0x00), + UBX_AID_INI = UBX_MSGID(UBX_CLASS_AID, 0x01), + UBX_AID_HUI = UBX_MSGID(UBX_CLASS_AID, 0x02), + UBX_AID_DATA = UBX_MSGID(UBX_CLASS_AID, 0x10), + UBX_AID_ALM = UBX_MSGID(UBX_CLASS_AID, 0x30), + UBX_AID_EPH = UBX_MSGID(UBX_CLASS_AID, 0x31), + + UBX_TIM_TP = UBX_MSGID(UBX_CLASS_TIM, 0x01), + UBX_TIM_TM = UBX_MSGID(UBX_CLASS_TIM, 0x02), + UBX_TIM_TM2 = UBX_MSGID(UBX_CLASS_TIM, 0x03), + UBX_TIM_SVIN = UBX_MSGID(UBX_CLASS_TIM, 0x04), +} ubx_message_t; + +typedef enum { + UBX_MODE_NOFIX = 0x00, /* no fix available */ + UBX_MODE_DR = 0x01, /* Dead reckoning */ + UBX_MODE_2D = 0x02, /* 2D fix */ + UBX_MODE_3D = 0x03, /* 3D fix */ + UBX_MODE_GPSDR = 0x04, /* GPS + dead reckoning */ + UBX_MODE_TMONLY = 0x05, /* Time-only fix */ +} ubx_mode_t; + +#define UBX_SOL_FLAG_GPS_FIX_OK 0x01 +#define UBX_SOL_FLAG_DGPS 0x02 +#define UBX_SOL_VALID_WEEK 0x04 +#define UBX_SOL_VALID_TIME 0x08 + +#endif /* _GPSD_UBX_H_ */ diff --git a/driver_zodiac.c b/driver_zodiac.c new file mode 100644 index 00000000..4afcc211 --- /dev/null +++ b/driver_zodiac.c @@ -0,0 +1,496 @@ +/* $Id$ */ +/* + * Handle the Rockwell binary packet format supported by the old Zodiac chipset + */ +#include +#include +#include +#include +#include +#include +#include "gpsd_config.h" +#include "gpsd.h" + +#include "bits.h" + +#ifdef ZODIAC_ENABLE +struct header { + unsigned short sync; + unsigned short id; + unsigned short ndata; + unsigned short flags; + unsigned short csum; +}; + +static unsigned short zodiac_checksum(unsigned short *w, int n) +{ + unsigned short csum = 0; + + while (n-- > 0) + csum += *(w++); + return -csum; +} + +/* zodiac_spew - Takes a message type, an array of data words, and a length + for the array, and prepends a 5 word header (including checksum). + The data words are expected to be checksummed */ +#if defined (WORDS_BIGENDIAN) +/* data is assumed to contain len/2 unsigned short words + * we change the endianness to little, when needed. + */ +static int end_write(int fd, void *d, int len) +{ + char buf[BUFSIZ]; + char *p = buf; + char *data = (char *)d; + size_t n = (size_t)len; + + while (n>0) { + *p++ = *(data+1); *p++ = *data; + data += 2; n -= 2; + } + return write(fd, buf, len); +} +#else +#define end_write write +#endif + +static ssize_t zodiac_spew(struct gps_device_t *session, unsigned short type, unsigned short *dat, int dlen) +{ + struct header h; + int i; + char buf[BUFSIZ]; + + h.sync = 0x81ff; + h.id = (unsigned short)type; + h.ndata = (unsigned short)(dlen - 1); + h.flags = 0; + h.csum = zodiac_checksum((unsigned short *) &h, 4); + +#ifdef ALLOW_RECONFIGURE + if (session->gpsdata.gps_fd != -1) { + size_t hlen, datlen; + hlen = sizeof(h); + datlen = sizeof(unsigned short) * dlen; + if (end_write(session->gpsdata.gps_fd, &h, hlen) != (ssize_t)hlen || + end_write(session->gpsdata.gps_fd, dat, datlen) != (ssize_t)datlen) { + gpsd_report(LOG_RAW, "Reconfigure write failed\n"); + return -1; + } + } +#endif /* ALLOW_RECONFIGURE */ + + (void)snprintf(buf, sizeof(buf), + "%04x %04x %04x %04x %04x", + h.sync,h.id,h.ndata,h.flags,h.csum); + for (i = 0; i < dlen; i++) + (void)snprintf(buf+strlen(buf), sizeof(buf)-strlen(buf), + " %04x", dat[i]); + + gpsd_report(LOG_RAW, "Sent Zodiac packet: %s\n",buf); + + return 0; +} + +static bool zodiac_speed_switch(struct gps_device_t *session, + speed_t speed, char parity, int stopbits) +{ + unsigned short data[15]; + + if (session->driver.zodiac.sn++ > 32767) + session->driver.zodiac.sn = 0; + + switch (parity) { + case 'E': + case 2: + parity = (char)2; + break; + case 'O': + case 1: + parity = (char)1; + break; + case 'N': + case 0: + default: + parity = (char)0; + break; + } + + memset(data, 0, sizeof(data)); + /* data is the part of the message starting at word 6 */ + data[0] = session->driver.zodiac.sn; /* sequence number */ + data[1] = 1; /* port 1 data valid */ + data[2] = (unsigned short)parity; /* port 1 character width (8 bits) */ + data[3] = (unsigned short)(stopbits-1); /* port 1 stop bits (1 stopbit) */ + data[4] = 0; /* port 1 parity (none) */ + data[5] = (unsigned short)(round(log((double)speed/300)/M_LN2)+1); /* port 1 speed */ + data[14] = zodiac_checksum(data, 14); + + (void)zodiac_spew(session, 1330, data, 15); +#ifdef ALLOW_RECONFIGURE + return true; /* it would be nice to error-check this */ +#else + return false; +#endif /* ALLOW_RECONFIGURE */ +} + +static ssize_t zodiac_control_send(struct gps_device_t *session, + char *msg, size_t len) +{ + unsigned short *shortwords = (unsigned short *)msg; + + /* and if len isn't even, it's your own fault */ + return zodiac_spew(session, shortwords[0], shortwords+1, (int)(len/2-1)); +} + +static void send_rtcm(struct gps_device_t *session, + char *rtcmbuf, size_t rtcmbytes) +{ + unsigned short data[34]; + int n = 1 + (int)(rtcmbytes/2 + rtcmbytes%2); + + if (session->driver.zodiac.sn++ > 32767) + session->driver.zodiac.sn = 0; + + memset(data, 0, sizeof(data)); + data[0] = session->driver.zodiac.sn; /* sequence number */ + memcpy(&data[1], rtcmbuf, rtcmbytes); + data[n] = zodiac_checksum(data, n); + + (void)zodiac_spew(session, 1351, data, n+1); +} + +static ssize_t zodiac_send_rtcm(struct gps_device_t *session, + char *rtcmbuf, size_t rtcmbytes) +{ + size_t len; + + while (rtcmbytes > 0) { + len = (size_t)(rtcmbytes>64?64:rtcmbytes); + send_rtcm(session, rtcmbuf, len); + rtcmbytes -= len; + rtcmbuf += len; + } + return 1; +} + +#define getzword(n) getwordz(session->packet.outbuffer, n) +#define getzlong(n) getlongz(session->packet.outbuffer, n) + +static gps_mask_t handle1000(struct gps_device_t *session) +{ + double subseconds; + struct tm unpacked_date; + /* ticks = getzlong(6); */ + /* sequence = getzword(8); */ + /* measurement_sequence = getzword(9); */ + /*@ -boolops -predboolothers @*/ + session->gpsdata.status = (getzword(10) & 0x1c) ? 0 : 1; + if (session->gpsdata.status != 0) + session->gpsdata.fix.mode = (getzword(10) & 1) ? MODE_2D : MODE_3D; + else + session->gpsdata.fix.mode = MODE_NO_FIX; + /*@ +boolops -predboolothers @*/ + + /* solution_type = getzword(11); */ + session->gpsdata.satellites_used = (int)getzword(12); + /* polar_navigation = getzword(13); */ + /* gps_week = getzword(14); */ + /* gps_seconds = getzlong(15); */ + /* gps_nanoseconds = getzlong(17); */ + unpacked_date.tm_mday = (int)getzword(19); + unpacked_date.tm_mon = (int)getzword(20) - 1; + unpacked_date.tm_year = (int)getzword(21) - 1900; + unpacked_date.tm_hour = (int)getzword(22); + unpacked_date.tm_min = (int)getzword(23); + unpacked_date.tm_sec = (int)getzword(24); + subseconds = (int)getzlong(25) / 1e9; + /*@ -compdef */ + session->gpsdata.fix.time = session->gpsdata.sentence_time = + (double)mkgmtime(&unpacked_date) + subseconds; + /*@ +compdef */ +#ifdef NTPSHM_ENABLE + /* Removing/changing the magic number below is likely to disturb + * the handling of the 1pps signal from the gps device. The regression + * tests and simple gps applications do not detect this. A live test + * with the 1pps signal active is required. */ + if (session->context->enable_ntpshm && session->gpsdata.fix.mode > MODE_NO_FIX) + (void)ntpshm_put(session, session->gpsdata.fix.time + 1.1); +#endif + /*@ -type @*/ + session->gpsdata.fix.latitude = ((long)getzlong(27)) * RAD_2_DEG * 1e-8; + session->gpsdata.fix.longitude = ((long)getzlong(29)) * RAD_2_DEG * 1e-8; + /* + * The Rockwell Jupiter TU30-D140 reports altitude as uncorrected height + * above WGS84 geoid. The Zodiac binary protocol manual does not + * specify whether word 31 is geodetic or WGS 84. + */ + session->gpsdata.fix.altitude = ((long)getzlong(31)) * 1e-2; + /*@ +type @*/ + session->gpsdata.separation = ((short)getzword(33)) * 1e-2; + session->gpsdata.fix.altitude -= session->gpsdata.separation; + session->gpsdata.fix.speed = (int)getzlong(34) * 1e-2; + session->gpsdata.fix.track = (int)getzword(36) * RAD_2_DEG * 1e-3; + session->mag_var = ((short)getzword(37)) * RAD_2_DEG * 1e-4; + session->gpsdata.fix.climb = ((short)getzword(38)) * 1e-2; + /* map_datum = getzword(39); */ + /* manual says these are 1-sigma */ + session->gpsdata.fix.eph = (int)getzlong(40) * 1e-2 * GPSD_CONFIDENCE; + session->gpsdata.fix.epv = (int)getzlong(42) * 1e-2 * GPSD_CONFIDENCE; + session->gpsdata.fix.ept = (int)getzlong(44) * 1e-2 * GPSD_CONFIDENCE; + session->gpsdata.fix.eps = (int)getzword(46) * 1e-2 * GPSD_CONFIDENCE; + /* clock_bias = (int)getzlong(47) * 1e-2; */ + /* clock_bias_sd = (int)getzlong(49) * 1e-2; */ + /* clock_drift = (int)getzlong(51) * 1e-2; */ + /* clock_drift_sd = (int)getzlong(53) * 1e-2; */ + +#if 0 + gpsd_report(LOG_INF, "date: %lf\n", session->gpsdata.fix.time); + gpsd_report(LOG_INF, " solution invalid:\n"); + gpsd_report(LOG_INF, " altitude: %d\n", (getzword(10) & 1) ? 1 : 0); + gpsd_report(LOG_INF, " no diff gps: %d\n", (getzword(10) & 2) ? 1 : 0); + gpsd_report(LOG_INF, " not enough satellites: %d\n", (getzword(10) & 4) ? 1 : 0); + gpsd_report(LOG_INF, " exceed max EHPE: %d\n", (getzword(10) & 8) ? 1 : 0); + gpsd_report(LOG_INF, " exceed max EVPE: %d\n", (getzword(10) & 16) ? 1 : 0); + gpsd_report(LOG_INF, " solution type:\n"); + gpsd_report(LOG_INF, " propagated: %d\n", (getzword(11) & 1) ? 1 : 0); + gpsd_report(LOG_INF, " altitude: %d\n", (getzword(11) & 2) ? 1 : 0); + gpsd_report(LOG_INF, " differential: %d\n", (getzword(11) & 4) ? 1 : 0); + gpsd_report(LOG_INF, "Number of measurements in solution: %d\n", getzword(12)); + gpsd_report(LOG_INF, "Lat: %f\n", getzlong(27) * RAD_2_DEG * 1e-8); + gpsd_report(LOG_INF, "Lon: %f\n", getzlong(29) * RAD_2_DEG * 1e-8); + gpsd_report(LOG_INF, "Alt: %f\n", (double) getzlong(31) * 1e-2); + gpsd_report(LOG_INF, "Speed: %f\n", (double) getzlong(34) * 1e-2 * MPS_TO_KNOTS); + gpsd_report(LOG_INF, "Map datum: %d\n", getzword(39)); + gpsd_report(LOG_INF, "Magnetic variation: %f\n", getzword(37) * RAD_2_DEG * 1e-4); + gpsd_report(LOG_INF, "Course: %f\n", getzword(36) * RAD_2_DEG * 1e-4); + gpsd_report(LOG_INF, "Separation: %f\n", getzword(33) * 1e-2); +#endif + + session->gpsdata.sentence_length = 55; + return TIME_SET|LATLON_SET|ALTITUDE_SET|CLIMB_SET|SPEED_SET|TRACK_SET|STATUS_SET|MODE_SET|CYCLE_START_SET; /* |HERR_SET|VERR_SET|SPEEDERR_SET */ +} + +static gps_mask_t handle1002(struct gps_device_t *session) +{ + int i, j, status, prn; + + session->gpsdata.satellites_used = 0; + memset(session->gpsdata.used,0,sizeof(session->gpsdata.used)); + /* ticks = getzlong(6); */ + /* sequence = getzword(8); */ + /* measurement_sequence = getzword(9); */ + /* gps_week = getzword(10); */ + /* gps_seconds = getzlong(11); */ + /* gps_nanoseconds = getzlong(13); */ + for (i = 0; i < ZODIAC_CHANNELS; i++) { + /*@ -type @*/ + session->driver.zodiac.Zv[i] = status = (int)getzword(15 + (3 * i)); + session->driver.zodiac.Zs[i] = prn = (int)getzword(16 + (3 * i)); + /*@ +type @*/ +#if 0 + gpsd_report(LOG_INF, "Sat%02d:\n", i); + gpsd_report(LOG_INF, " used:%d\n", (status & 1) ? 1 : 0); + gpsd_report(LOG_INF, " eph:%d\n", (status & 2) ? 1 : 0); + gpsd_report(LOG_INF, " val:%d\n", (status & 4) ? 1 : 0); + gpsd_report(LOG_INF, " dgps:%d\n", (status & 8) ? 1 : 0); + gpsd_report(LOG_INF, " PRN:%d\n", prn); + gpsd_report(LOG_INF, " C/No:%d\n", getzword(17 + (3 * i))); +#endif + if (status & 1) + session->gpsdata.used[session->gpsdata.satellites_used++] = prn; + for (j = 0; j < ZODIAC_CHANNELS; j++) { + if (session->gpsdata.PRN[j] != prn) + continue; + session->gpsdata.ss[j] = (int)getzword(17 + (3 * i)); + break; + } + } + return SATELLITE_SET | USED_SET; +} + +static gps_mask_t handle1003(struct gps_device_t *session) +{ + int i; + + /* ticks = getzlong(6); */ + /* sequence = getzword(8); */ + session->gpsdata.gdop = (unsigned int)getzword(9) * 1e-2; + session->gpsdata.pdop = (unsigned int)getzword(10) * 1e-2; + session->gpsdata.hdop = (unsigned int)getzword(11) * 1e-2; + session->gpsdata.vdop = (unsigned int)getzword(12) * 1e-2; + session->gpsdata.tdop = (unsigned int)getzword(13) * 1e-2; + session->gpsdata.satellites = (int)getzword(14); + + for (i = 0; i < ZODIAC_CHANNELS; i++) { + if (i < session->gpsdata.satellites) { + session->gpsdata.PRN[i] = (int)getzword(15 + (3 * i)); + session->gpsdata.azimuth[i] = (int)(((short)getzword(16 + (3 * i))) * RAD_2_DEG * 1e-4); + if (session->gpsdata.azimuth[i] < 0) + session->gpsdata.azimuth[i] += 360; + session->gpsdata.elevation[i] = (int)(((short)getzword(17 + (3 * i))) * RAD_2_DEG * 1e-4); +#if 0 + gpsd_report(LOG_INF, "Sat%02d: PRN:%d az:%d el:%d\n", + i, getzword(15+(3 * i)),getzword(16+(3 * i)),getzword(17+(3 * i))); +#endif + } else { + session->gpsdata.PRN[i] = 0; + session->gpsdata.azimuth[i] = 0; + session->gpsdata.elevation[i] = 0; + } + } + return SATELLITE_SET | HDOP_SET | VDOP_SET | PDOP_SET; +} + +static void handle1005(struct gps_device_t *session UNUSED) +{ + /* ticks = getzlong(6); */ + /* sequence = getzword(8); */ + int numcorrections = (int)getzword(12); +#if 0 + int i; + + gpsd_report(LOG_INF, "Packet: %d\n", session->driver.zodiac.sn); + gpsd_report(LOG_INF, "Station bad: %d\n", (getzword(9) & 1) ? 1 : 0); + gpsd_report(LOG_INF, "User disabled: %d\n", (getzword(9) & 2) ? 1 : 0); + gpsd_report(LOG_INF, "Station ID: %d\n", getzword(10)); + gpsd_report(LOG_INF, "Age of last correction in seconds: %d\n", getzword(11)); + gpsd_report(LOG_INF, "Number of corrections: %d\n", getzword(12)); + for (i = 0; i < numcorrections; i++) { + gpsd_report(LOG_INF, "Sat%02d:\n", getzword(13+i) & 0x3f); + gpsd_report(LOG_INF, "ephemeris:%d\n", (getzword(13+i) & 64) ? 1 : 0); + gpsd_report(LOG_INF, "rtcm corrections:%d\n", (getzword(13+i) & 128) ? 1 : 0); + gpsd_report(LOG_INF, "rtcm udre:%d\n", (getzword(13+i) & 256) ? 1 : 0); + gpsd_report(LOG_INF, "sat health:%d\n", (getzword(13+i) & 512) ? 1 : 0); + gpsd_report(LOG_INF, "rtcm sat health:%d\n", (getzword(13+i) & 1024) ? 1 : 0); + gpsd_report(LOG_INF, "corrections state:%d\n", (getzword(13+i) & 2048) ? 1 : 0); + gpsd_report(LOG_INF, "iode mismatch:%d\n", (getzword(13+i) & 4096) ? 1 : 0); + } +#endif + if (session->gpsdata.fix.mode == MODE_NO_FIX) + session->gpsdata.status = STATUS_NO_FIX; + else if (numcorrections == 0) + session->gpsdata.status = STATUS_FIX; + else + session->gpsdata.status = STATUS_DGPS_FIX; +} + +static gps_mask_t handle1011(struct gps_device_t *session) +{ + /* + * This is UNTESTED -- but harmless if buggy. Added to support + * client querying of the ID with firmware version in 2006. + * The Zodiac is supposed to send one of these messages on startup. + */ + getstringz(session->subtype, + session->packet.outbuffer, + 19, 28); /* software version field */ + gpsd_report(LOG_INF, "Software version: %s\n", session->subtype); + return DEVICEID_SET; +} + + +static void handle1108(struct gps_device_t *session) +{ + /* ticks = getzlong(6); */ + /* sequence = getzword(8); */ + /* utc_week_seconds = getzlong(14); */ + /* leap_nanoseconds = getzlong(17); */ + if ((int)(getzword(19) & 3) == 3) + session->context->leap_seconds = (int)getzword(16); +#if 0 + gpsd_report(LOG_INF, "Leap seconds: %d.%09d\n", getzword(16), getzlong(17)); + gpsd_report(LOG_INF, "UTC validity: %d\n", getzword(19) & 3); +#endif +} + +static gps_mask_t zodiac_analyze(struct gps_device_t *session) +{ + char buf[BUFSIZ]; + int i; + unsigned int id = (unsigned int)((session->packet.outbuffer[3]<<8) | session->packet.outbuffer[2]); + + if (session->packet.type != ZODIAC_PACKET) { + const struct gps_type_t **dp; + gpsd_report(LOG_PROG, "zodiac_analyze packet type %d\n",session->packet.type); + // Wrong packet type ? + // Maybe find a trigger just in case it's an Earthmate + gpsd_report(LOG_RAW+4, "Is this a trigger: %s ?\n", (char*)session->packet.outbuffer); + + for (dp = gpsd_drivers; *dp; dp++) { + char *trigger = (*dp)->trigger; + + if (trigger!=NULL && strncmp((char *)session->packet.outbuffer, trigger, strlen(trigger))==0 && isatty(session->gpsdata.gps_fd)!=0) { + gpsd_report(LOG_PROG, "found %s.\n", trigger); + + (void)gpsd_switch_driver(session, (*dp)->type_name); + return 0; + } + } + return 0; + } + + buf[0] = '\0'; + for (i = 0; i < (int)session->packet.outbuflen; i++) + (void)snprintf(buf+strlen(buf), sizeof(buf)-strlen(buf), + "%02x", (unsigned int)session->packet.outbuffer[i]); + (void)strlcat(buf, "\n", BUFSIZ); + gpsd_report(LOG_RAW, "Raw Zodiac packet type %d length %zd: %s\n", + id, session->packet.outbuflen, buf); + + if (session->packet.outbuflen < 10) + return 0; + + (void)snprintf(session->gpsdata.tag,sizeof(session->gpsdata.tag),"%u",id); + + switch (id) { + case 1000: + return handle1000(session); + case 1002: + return handle1002(session); + case 1003: + return handle1003(session); + case 1005: + handle1005(session); + return 0; + case 1011: + return handle1011(session); + case 1108: + handle1108(session); + return 0; + default: + return 0; + } +} + +/* caller needs to specify a wrapup function */ + +/* this is everything we export */ +const struct gps_type_t zodiac_binary = +{ + .type_name = "Zodiac binary", /* full name of type */ + .packet_type = ZODIAC_PACKET, /* associated lexer packet type */ + .trigger = NULL, /* no trigger */ + .channels = 12, /* consumer-grade GPS */ + .control_send = zodiac_control_send, /* for gpsctl and friends */ + .probe_wakeup = NULL, /* no probe on baud rate change */ + .probe_detect = NULL, /* no probe */ + .probe_subtype = NULL, /* no initialization */ +#ifdef ALLOW_RECONFIGURE + .configurator = NULL, /* no configuration */ +#endif /* ALLOW_RECONFIGURE */ + .get_packet = generic_get, /* use the generic packet getter */ + .parse_packet = zodiac_analyze, /* parse message packets */ + .rtcm_writer = zodiac_send_rtcm, /* send DGPS correction */ + .speed_switcher = zodiac_speed_switch,/* we can change baud rate */ + .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 reversion hook */ +#endif /* ALLOW_RECONFIGURE */ + .wrapup = NULL, /* caller might supply a close hook */ + .cycle = 1, /* updates every second */ +}; + +#endif /* ZODIAC_ENABLE */ diff --git a/evermore.c b/evermore.c deleted file mode 100644 index 41031121..00000000 --- a/evermore.c +++ /dev/null @@ -1,522 +0,0 @@ -/* $Id$ */ -/* - * This is the gpsd driver for EverMore GPSes operating in binary mode. - * About the only thing this gives us that NMEA won't is TDOP. - * But we'll get atomic position reports from it, which is good. - * - * The vendor site is . - * - * This driver was written by Petr Slansky based on a framework by Eric S. - * Raymond. The following remarks are by Petr Slansky. - * - * Snooping on the serial the communication between a Windows program and - * an Evermore chipset reveals some messages not described in the vendor - * documentation (Issue C of Aug 2002): - * - * 10 02 06 84 00 00 00 84 10 03 switch to binary mode (84 00 00 00) - * 10 02 06 84 01 00 00 85 10 03 switch to NMEA mode (84 01 00 00) - * - * 10 02 06 89 01 00 00 8a 10 03 set baud rate 4800 - * 10 02 06 89 01 01 00 8b 10 03 set baud rate 9600 - * 10 02 06 89 01 02 00 8c 10 03 set baud rate 19200 - * 10 02 06 89 01 03 00 8d 10 03 set baud rate 38400 - * - * 10 02 06 8D 00 01 00 8E 10 03 switch to datum ID 001 (WGS-84) - * 10 02 06 8D 00 D8 00 65 10 03 switch to datum ID 217 (WGS-72) - * - * These don't entail a reset of GPS as the 0x80 message does. - * - * 10 02 04 38 85 bd 10 03 answer from GPS to 0x85 message; ACK message - * 10 02 04 38 8d c5 10 03 answer from GPS to 0x8d message; ACK message - * 10 02 04 38 8e c6 10 03 answer from GPS to 0x8e message; ACK message - * 10 02 04 38 8f c7 10 03 answer from GPS to 0x8f message; ACK message - * - * The chip sometimes sends vendor extension messages with the prefix - * $PEMT,100. After restart, it sends a $PEMT,100 message describing the - * chip's configuration. Here is a sample: - * - * $PEMT,100,05.42g,100303,180,05,1,20,15,08,0,0,2,1*5A - * 100 - message type - * 05.42g - firmware version - * 100303 - date of firmware release DDMMYY - * 180 - datum ID; 001 is WGS-84 - * 05 - default elevation mask; see message 0x86 - * 1 - default DOP select (1 is auto DOP mask); see message 0x87 - * 20 - default GDOP; see message 0x87 - * 15 - default PDOP - * 08 - default HDOP - * 0 - Normal mode, without 1PPS - * 0 - default position pinning control (0 disable, 1 enable) - * 2 - altitude hold mode (0 disable, 1 always, 2 auto) - * 1 - 2/1 satellite nav mode (0,1,2,3,4) - * 0 disable 2/1 sat nav mode - * 1 hold direction (2 sat) - * 2 clock hold only (2 sat) - * 3 direction hold then clock hold (1 sat) - * 4 clock hold then direction hold (1 sat) - * - * Message $PEMT,100 could be forced with message 0x85 (restart): - * 10 02 12 85 00 00 00 00 00 01 01 00 00 00 00 00 00 00 00 87 10 03 - * 0x85 ID, Restart - * 0x00 restart mode (0 default, 1 hot, 2 warm, 3 cold, 4 test) - * 0x00 test start search PRN (1-32) - * 0x00 UTC second (0-59) - * 0x00 UTC Minute (0-59) - * 0x00 UTC Hour (0-23) - * 0x01 UTC Day (1-31) - * 0x01 UTC Month (1-12) - * 0x0000 UTC year (1980+x, uint16) - * 0x0000 Latitude WGS-84 (+/-900, 1/10 degree, + for N, int16) - * 0x0000 Longtitude WGS-84 (+/-1800, 1/10 degree, + for E, int16) - * 0x0000 Altitude WGS-84 (-1000..+18000, meters, int16) - * 0x87 CRC - * - * With message 0x8e it is possible to define how often each NMEA - * message is sent (0-255 seconds). It is possible with message 0x8e - * to activate PEMT,101 messages that have information about time, - * position, velocity and HDOP. - * - * $PEMT,101,1,02,00.0,300906190446,5002.5062,N,01427.6166,E,00259,000,0000*27 - * $PEMT,101,2,06,02.1,300906185730,5002.7546,N,01426.9524,E,00323,020,0011*26 - * 101 - message type, Compact Navigation Solution - * 2 - position status (1,2,3,4,5,6) - * (1 invalid, 2 2D fix, 3 3D fix, 4 2D with DIFF, 5 3D with DIFF, - * 6 2/1 sat degrade mode) - * 06 - number of used satelites - * 02.1 - DOP (00.0 no fix, HDOP 2D fix, PDOP 3D fix) - * 300906185730 - date and time, UTC ddmmyyHHMMSS (30/09/2006 18:57:30) - * 5002.7546,N - Latitude (degree) - * 01426.9524,E - Longitude (degree) - * 00323 - Altitude (323 metres) - * 020 - heading (20 degrees from true north) - * 0011 - speed over ground (11 metres per second); documentation says km per h - * - * This is an exampe of an 0x8e message that activates all NMEA sentences - * with 1s period: - * 10 02 12 8E 7F 01 01 01 01 01 01 01 01 00 00 00 00 00 00 15 10 03 - * - * There is a way to probe for this chipset. When binary message 0x81 is sent: - * 10 02 04 81 13 94 10 03 - * - * EverMore will reply with message like this: - * *10 *02 *0D *20 E1 00 00 *00 0A 00 1E 00 32 00 5B *10 *03 - * bytes marked with * are fixed - * Message in reply is information about logging configuration of GPS - * - * Another way to detect the EverMore chipset is to send one of the messages - * 0x85, 0x8d, 0x8e or 0x8f and check for a reply. - * The reply message from an EverMore GPS will look like this: - * *10 *02 *04 *38 8d c5 *10 *03 - * 8d indicates that message 0x8d was sent; - * c5 is EverMore checksum, other bytes are fixed - * - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "gpsd_config.h" -#include "gpsd.h" -#if defined(EVERMORE_ENABLE) && defined(BINARY_ENABLE) - -#define GET_ORIGIN 1 -#define PUT_ORIGIN 0 -#include "bits.h" - -#define EVERMORE_CHANNELS 12 - -/*@ +charint -usedef -compdef @*/ -static ssize_t evermore_control_send(struct gps_device_t *session, char *buf, size_t len) -{ - unsigned int crc; - size_t i; - char *cp; - - /*@ +charint +ignoresigns @*/ - /* prepare a DLE-stuffed copy of the message */ - cp = session->msgbuf; - *cp++ = 0x10; /* message starts with DLE STX */ - *cp++ = 0x02; - - session->msgbuflen = (size_t)(len + 2); /* len < 254 !! */ - *cp++ = (char)session->msgbuflen; /* message length */ - if (session->msgbuflen == 0x10) *cp++ = 0x10; - - /* payload */ - crc = 0; - for (i = 0; i < len; i++) { - *cp++ = buf[i]; - if (buf[i] == 0x10) - *cp++ = 0x10; - crc += buf[i]; - } - - crc &= 0xff; - - /* enter CRC after payload */ - *cp++ = crc; - if (crc == 0x10) - *cp++ = 0x10; - - *cp++ = 0x10; /* message ends with DLE ETX */ - *cp++ = 0x03; - - session->msgbuflen = (size_t)(cp - session->msgbuf); - /*@ -charint -ignoresigns @*/ - -#ifdef ALLOW_RECONFIGURE - return gpsd_write(session, session->msgbuf, session->msgbuflen); -#else - return -1; -#endif /* ALLOW_RECONFIGURE */ -} -/*@ -charint +usedef +compdef @*/ - -/*@ +charint @*/ -gps_mask_t evermore_parse(struct gps_device_t *session, unsigned char *buf, size_t len) -{ - unsigned char buf2[MAX_PACKET_LENGTH], *cp, *tp; - size_t i, datalen; - unsigned int used, visible, satcnt; - double version; - gps_mask_t mask = 0; - - if (len == 0) - return 0; - - /* time to unstuff it and discard the header and footer */ - cp = buf + 2; - tp = buf2; - if (*cp == 0x10) cp++; - datalen = (size_t)*cp++; - - gpsd_report(LOG_RAW, "raw EverMore packet type 0x%02x, length %zd: %s\n", - *cp, len, gpsd_hexdump_wrapper(buf, len, LOG_RAW)); - - datalen -= 2; - - for (i = 0; i < (size_t)datalen; i++) { - *tp = *cp++; - if (*tp == 0x10) cp++; - tp++; - } - - /*@ -usedef -compdef @*/ - gpsd_report(LOG_RAW, "EverMore packet type 0x%02x, length %zd: %s\n", - buf2[0], datalen, gpsd_hexdump_wrapper(buf2, datalen, LOG_RAW)); - /*@ +usedef +compdef @*/ - - (void)snprintf(session->gpsdata.tag, sizeof(session->gpsdata.tag), - "EID%d",(int)buf2[0]); - - switch (getub(buf2, 1)) - { - case 0x02: /* Navigation Data Output */ - session->gpsdata.fix.time = session->gpsdata.sentence_time - = gpstime_to_unix((int)getleuw(buf2, 2), getleul(buf2, 4)*0.01) - session->context->leap_seconds; - ecef_to_wgs84fix(&session->gpsdata, - getlesl(buf2, 8)*1.0, getlesl(buf2, 12)*1.0, getlesl(buf2, 16)*1.0, - getlesw(buf2, 20)/10.0, getlesw(buf2, 22)/10.0, getlesw(buf2, 24)/10.0); - used = getub(buf2, 26) & 0x0f; - visible = (getub(buf2, 26) & 0xf0) >> 4; - version = getleuw(buf2, 27)/100.0; - /* that's all the information in this packet */ - if (used < 3) - session->gpsdata.fix.mode = MODE_NO_FIX; - else if (used == 3) - session->gpsdata.fix.mode = MODE_2D; - else { - session->gpsdata.fix.mode = MODE_3D; - mask |= ALTITUDE_SET | CLIMB_SET; - } - gpsd_report(LOG_PROG, "NDO 0x02: version %3.2f, mode=%d, status=%d, visible=%d, used=%d\n", - version, - session->gpsdata.fix.mode, - session->gpsdata.status, - visible, - used); - mask |= TIME_SET | LATLON_SET | TRACK_SET | SPEED_SET | MODE_SET | CYCLE_START_SET; - if (session->subtype[0] == '\0') { - (void)snprintf(session->subtype, sizeof(session->subtype), - "%3.2f", version); - mask |= DEVICEID_SET; - } - return mask; - - case 0x04: /* DOP Data Output */ - session->gpsdata.fix.time = session->gpsdata.sentence_time - = gpstime_to_unix((int)getleuw(buf2, 2), getleul(buf2, 4)*0.01) - session->context->leap_seconds; - session->gpsdata.gdop = (double)getub(buf2, 8)*0.1; - session->gpsdata.pdop = (double)getub(buf2, 9)*0.1; - session->gpsdata.hdop = (double)getub(buf2, 10)*0.1; - session->gpsdata.vdop = (double)getub(buf2, 11)*0.1; - session->gpsdata.tdop = (double)getub(buf2, 12)*0.1; - switch (getub(buf2, 13)) { - case 0: /* no position fix */ - case 1: /* manual calls this "1D navigation" */ - session->gpsdata.status = STATUS_NO_FIX; - session->gpsdata.fix.mode = MODE_NO_FIX; - break; - case 2: /* 2D navigation */ - session->gpsdata.status = STATUS_FIX; - session->gpsdata.fix.mode = MODE_2D; - break; - case 3: /* 3D navigation */ - session->gpsdata.status = STATUS_FIX; - session->gpsdata.fix.mode = MODE_3D; - break; - case 4: /* 3D navigation with DGPS */ - session->gpsdata.status = STATUS_DGPS_FIX; - session->gpsdata.fix.mode = MODE_3D; - break; - } - /* that's all the information in this packet */ - gpsd_report(LOG_PROG, "DDO 0x04: mode=%d, status=%d\n", - session->gpsdata.fix.mode, - session->gpsdata.status); - return TIME_SET | DOP_SET | MODE_SET | STATUS_SET; - - case 0x06: /* Channel Status Output */ - session->gpsdata.fix.time = session->gpsdata.sentence_time - = gpstime_to_unix((int)getleuw(buf2, 2), getleul(buf2, 4)*0.01) - session->context->leap_seconds; - session->gpsdata.satellites = (int)getub(buf2, 8); - session->gpsdata.satellites_used = 0; - memset(session->gpsdata.used, 0, sizeof(session->gpsdata.used)); - if (session->gpsdata.satellites > 12) { - gpsd_report(LOG_WARN, "Warning: EverMore packet has information about %d satellites!\n", - session->gpsdata.satellites); - } - if (session->gpsdata.satellites > EVERMORE_CHANNELS) session->gpsdata.satellites = EVERMORE_CHANNELS; - satcnt = 0; - for (i = 0; i < (size_t)session->gpsdata.satellites; i++) { - int prn; - // channel = getub(buf2, 7*i+7+2) - prn = (int)getub(buf2, 7*i+7+3); - if (prn == 0) continue; /* satellite record is not valid */ - session->gpsdata.PRN[satcnt] = prn; - session->gpsdata.azimuth[satcnt] = (int)getleuw(buf2, 7*i+7+4); - session->gpsdata.elevation[satcnt] = (int)getub(buf2, 7*i+7+6); - session->gpsdata.ss[satcnt] = (int)getub(buf2, 7*i+7+7); - /* - * Status bits at offset 8: - * bit0 = 1 satellite acquired - * bit1 = 1 code-tracking loop locked - * bit2 = 1 carrier-tracking loop locked - * bit3 = 1 data-bit synchronization done - * bit4 = 1 frame synchronization done - * bit5 = 1 ephemeris data collected - * bit6 = 1 used for position fix - */ - if (getub(buf2, 7*i+7+8) & 0x40) { - session->gpsdata.used[session->gpsdata.satellites_used++]=prn; - } - - satcnt++; - - } - session->gpsdata.satellites = (int)satcnt; - /* that's all the information in this packet */ - gpsd_report(LOG_PROG, "CSO 0x04: %d satellites used\n", - session->gpsdata.satellites_used); - return TIME_SET | SATELLITE_SET | USED_SET; - - case 0x08: /* Measurement Data Output */ - /* clock offset is a manufacturer diagnostic */ - /* (int)getleuw(buf2, 8); clock offset, 29000..29850 ?? */ - session->gpsdata.fix.time = session->gpsdata.sentence_time - = gpstime_to_unix((int)getleuw(buf2, 2), getleul(buf2, 4)*0.01) - session->context->leap_seconds; - visible = getub(buf2, 10); - /* FIXME: read full statellite status for each channel */ - /* we can get pseudo range (m), delta-range (m/s), doppler (Hz) and status for each channel */ - /* gpsd_report(LOG_PROG, "MDO 0x04: visible=%d\n", visible); */ - gpsd_report(LOG_PROG, "MDO 0x04:\n"); - return TIME_SET; - - case 0x20: /* LogConfig Info, could be used as a probe for EverMore GPS */ - gpsd_report(LOG_IO, "LogConfig EverMore packet, length %zd: %s\n", - datalen, gpsd_hexdump_wrapper(buf2, datalen, LOG_IO)); - return ONLINE_SET; - - case 0x22: /* LogData */ - gpsd_report(LOG_IO, "LogData EverMore packet, length %zd: %s\n", - datalen, gpsd_hexdump_wrapper(buf2, datalen, LOG_IO)); - return ONLINE_SET; - - case 0x38: /* ACK */ - gpsd_report(LOG_PROG, "EverMore command %02X ACK\n", getub(buf2, 2)); - return ONLINE_SET; - - default: - gpsd_report(LOG_WARN, - "unknown EverMore packet id 0x%02x, length %zd: %s\n", buf2[0], - datalen, gpsd_hexdump_wrapper(buf2, datalen, LOG_IO)); - return 0; - } -} -/*@ -charint @*/ - -static gps_mask_t evermore_parse_input(struct gps_device_t *session) -{ - gps_mask_t st; - - if (session->packet.type == EVERMORE_PACKET){ - st = evermore_parse(session, session->packet.outbuffer, session->packet.outbuflen); - return st; -#ifdef NMEA_ENABLE - } else if (session->packet.type == NMEA_PACKET) { - st = nmea_parse((char *)session->packet.outbuffer, session); - return st; -#endif /* NMEA_ENABLE */ - } else - return 0; -} - -static bool evermore_speed(struct gps_device_t *session, - speed_t speed, char parity, int stopbits) -{ - /*@ -type @*/ - gpsd_report(LOG_PROG, "evermore_speed(%d%c%d)\n", speed, parity, stopbits); - /* parity and stopbit switching aren't available on this chip */ - if (parity!=session->gpsdata.parity || stopbits!=session->gpsdata.parity) { - return false; - } else { - unsigned char tmp8; - char msg[] = { - 0x89, /* 0: msg ID, Serial Port Configuration */ - 0x01, /* 1: bit 0 cfg for main serial, bit 1 cfg for DGPS port */ - 0x00, /* 2: baud rate for main serial; 4800(0), 9600(1), 19200(2), 38400(3) */ - 0x00, /* 3: baud rate for DGPS serial port; 4800(0), 9600(1), etc */ - }; - switch (speed) { - case 4800: tmp8 = 0; break; - case 9600: tmp8 = 1; break; - case 19200: tmp8 = 2; break; - case 38400: tmp8 = 3; break; - default: return false; - } - msg[2] = tmp8; - return (evermore_control_send(session, msg, sizeof(msg)) != -1); - } - /*@ +type @*/ -} - -static bool evermore_protocol(struct gps_device_t *session, int protocol) -{ - /*@ +charint */ - char tmp8; - char evrm_protocol_config[] = { - 0x84, /* 0: msg ID, Protocol Configuration */ - 0x00, /* 1: mode; EverMore binary(0), NMEA(1) */ - 0x00, /* 2: reserved */ - 0x00, /* 3: reserved */ - }; - /*@ -charint */ - gpsd_report(LOG_PROG, "evermore_protocol(%d)\n", protocol); - /*@i1@*/tmp8 = (protocol != 0) ? 1 : 0; /* NMEA : binary */ - evrm_protocol_config[1] = tmp8; - return (evermore_control_send(session, evrm_protocol_config, sizeof(evrm_protocol_config)) != -1); -} - -static bool evermore_nmea_config(struct gps_device_t *session, int mode) -/* mode = 0 : EverMore default */ -/* mode = 1 : gpsd best */ -/* mode = 2 : EverMore search, activate PEMT101 message */ -{ - char tmp8; - /*@ +charint */ - char evrm_nmeaout_config[] = { - 0x8e, /* 0: msg ID, NMEA Message Control */ - 0xff, /* 1: NMEA sentence bitmask, GGA(0), GLL(1), GSA(2), GSV(3), ... */ - 0x01, /* 2: nmea checksum no(0), yes(1) */ - 1, /* 3: GPGGA, interval 0-255s */ - 0, /* 4: GPGLL, interval 0-255s */ - 1, /* 5: GPGSA, interval 0-255s */ - 1, /* 6: GPGSV, interval 0-255s */ - 1, /* 7: GPRMC, interval 0-255s */ - 0, /* 8: GPVTG, interval 0-255s */ - 0, /* 9: PEMT,101, interval 0-255s */ - 0, 0, 0, 0, 0, 0, /* 10-15: reserved */ - }; - /*@ -charint */ - gpsd_report(LOG_PROG, "evermore_nmea_config(%d)\n", mode); - /*@i1@*/tmp8 = (mode == 1) ? 5 : 1; /* NMEA GPGSV, gpsd */ - evrm_nmeaout_config[6] = tmp8; /* GPGSV, 1s or 5s */ - /*@i1@*/tmp8 = (mode == 2) ? 1 : 0; /* NMEA PEMT101 */ - evrm_nmeaout_config[9] = tmp8; /* PEMT101, 1s or 0s */ - return (evermore_control_send(session, evrm_nmeaout_config, sizeof(evrm_nmeaout_config)) != -1); -} - -static void evermore_mode(struct gps_device_t *session, int mode) -{ - gpsd_report(LOG_PROG, "evermore_mode(%d), %d\n", mode, session->back_to_nmea ? 1 : 0); - if (mode == MODE_NMEA) { - /* NMEA */ - (void) evermore_protocol(session, 1); - session->gpsdata.driver_mode = MODE_NMEA; - (void) evermore_nmea_config(session, 1); /* configure NMEA messages for gpsd */ - } else { - /* binary */ - (void) evermore_protocol(session, 0); - session->back_to_nmea = false; - session->gpsdata.driver_mode = MODE_BINARY; - } -} - -#ifdef ALLOW_RECONFIGURE -static void evermore_configurator(struct gps_device_t *session, unsigned int seq) -{ - gpsd_report(LOG_PROG, "evermore_configurator(%d)\n", seq); - (void) evermore_nmea_config(session, 1); /* configure NMEA messages for gpsd (GPGSV every 5s) */ - if (seq == 0) { - if (session->packet.type == NMEA_PACKET) { - gpsd_report(LOG_WARN, "NMEA_PACKET packet\n"); - } - (void) evermore_mode(session, 1); /* switch GPS to binary mode */ - session->back_to_nmea = true; - } -} -#endif /* ALLOW_RECONFIGURE */ - - -static void evermore_wrap(struct gps_device_t *session) -{ - gpsd_report(LOG_PROG, "evermore_wrap\n"); - (void) evermore_nmea_config(session, 0); /* configure NMEA messages to default */ -} - - -/* this is everything we export */ -const struct gps_type_t evermore_binary = -{ - .type_name = "EverMore binary", /* full name of type */ - .packet_type = NMEA_PACKET, /* lexer packet type */ - .trigger = "$PEMT,", /* recognize the type */ - .channels = EVERMORE_CHANNELS, /* consumer-grade GPS */ - .control_send = evermore_control_send, /* how to send a control string */ - .probe_wakeup = NULL, /* no wakeup to be done before hunt */ - .probe_detect = NULL, /* no probe */ - .probe_subtype = NULL, /* no subtype probing */ -#ifdef ALLOW_RECONFIGURE - .configurator = evermore_configurator, /* switch to binary */ -#endif /* ALLOW_RECONFIGURE */ - .get_packet = generic_get, /* use generic one */ - .parse_packet = evermore_parse_input, /* parse message packets */ - .rtcm_writer = pass_rtcm, /* send RTCM data straight */ - .speed_switcher = evermore_speed, /* we can change baud rates */ - .mode_switcher = evermore_mode, /* there is a mode switcher */ - .rate_switcher = NULL, /* no sample-rate switcher */ - .cycle_chars = -1, /* ignore, no rate switch */ -#ifdef ALLOW_RECONFIGURE - .revert = NULL, /* reversion code */ -#endif /* ALLOW_RECONFIGURE */ - .wrapup = evermore_wrap, /* wrapup method */ - .cycle = 1, /* updates every second */ -}; -#endif /* defined(EVERMORE_ENABLE) && defined(BINARY_ENABLE) */ diff --git a/garmin.c b/garmin.c deleted file mode 100644 index 7d763dce..00000000 --- a/garmin.c +++ /dev/null @@ -1,1247 +0,0 @@ -/* $Id$ */ -/* - * Handle the Garmin binary packet format supported by the USB Garmins - * tested with the Garmin 18 and other models. This driver is NOT for - * serial port connected Garmins, they provide adequate NMEA support. - * - * This code is partly from the Garmin IOSDK and partly from the - * sample code in the Linux garmin_gps driver. - * - * This code supports both Garmin on a serial port and USB Garmins. - * - * USB Garmins need the Linux garmin_gps driver and will not function - * without it. This code has been tested and at least at one time is - * known to work on big- and little-endian CPUs and 32 and 64 bit cpu - * modes. - * - * Protocol info from: - * 425_TechnicalSpecification.pdf - * ( formerly GPS18_TechnicalSpecification.pdf ) - * iop_spec.pdf - * http://www.garmin.com/support/commProtocol.html - * - * bad code by: Gary E. Miller - * 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: - * - * do not have from garmin: - * pdop - * hdop - * vdop - * magnetic variation - * - * known bugs: - * hangs in the fread loop instead of keeping state and returning. - * may or may not work on a little-endian machine - */ - -#define __USE_POSIX199309 1 -#include -#include // for nanosleep() - -#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" - -#ifdef GARMIN_ENABLE - -#define USE_RMD 0 - -#define ETX 0x03 -#define ACK 0x06 -#define DLE 0x10 -#define NAK 0x15 - -#define GARMIN_LAYERID_TRANSPORT (uint8_t) 0 -#define GARMIN_LAYERID_APPL (uint32_t) 20 -// Linux Garmin USB driver layer-id to use for some control mechanisms -#define GARMIN_LAYERID_PRIVATE 0x01106E4B - -// packet ids used in private layer -#define PRIV_PKTID_SET_DEBUG 1 -#define PRIV_PKTID_SET_MODE 2 -#define PRIV_PKTID_INFO_REQ 3 -#define PRIV_PKTID_INFO_RESP 4 -#define PRIV_PKTID_RESET_REQ 5 -#define PRIV_PKTID_SET_DEF_MODE 6 - -#define MODE_NATIVE 0 -#define MODE_GARMIN_SERIAL 1 - -#define GARMIN_PKTID_TRANSPORT_START_SESSION_REQ 5 -#define GARMIN_PKTID_TRANSPORT_START_SESSION_RESP 6 - -#define GARMIN_PKTID_PROTOCOL_ARRAY 253 -#define GARMIN_PKTID_PRODUCT_RQST 254 -#define GARMIN_PKTID_PRODUCT_DATA 255 -/* 0x29 ')' */ -#define GARMIN_PKTID_RMD41_DATA 41 -/* 0x33 '3' */ -#define GARMIN_PKTID_PVT_DATA 51 -/* 0x33 '4' */ -#define GARMIN_PKTID_RMD_DATA 52 -/* 0x72 'r' */ -#define GARMIN_PKTID_SAT_DATA 114 - -#define GARMIN_PKTID_L001_XFER_CMPLT 12 -#define GARMIN_PKTID_L001_COMMAND_DATA 10 -#define GARMIN_PKTID_L001_DATE_TIME_DATA 14 -#define GARMIN_PKTID_L001_RECORDS 27 -#define GARMIN_PKTID_L001_WPT_DATA 35 - -#define CMND_ABORT 0 -#define CMND_START_PVT_DATA 49 -#define CMND_STOP_PVT_DATA 50 -#define CMND_START_RM_DATA 110 - -#define MAX_BUFFER_SIZE 4096 - -#define GARMIN_CHANNELS 12 - -// something magic about 64, garmin driver will not return more than -// 64 at a time. If you read less than 64 bytes the next read will -// just get the last of the 64 byte buffer. -#define ASYNC_DATA_SIZE 64 - - -#pragma pack(1) -// This is the data format of the satellite data from the garmin USB -typedef struct { - uint8_t svid; - int16_t snr; // 0 - 0xffff - uint8_t elev; - uint16_t azmth; - uint8_t status; // bit 0, has ephemeris, 1, has diff correction - // bit 2 used in solution - // bit 3?? -} cpo_sat_data; - -/* Garmin D800_Pvt_Datetype_Type */ -/* packet type: GARMIN_PKTID_PVT_DATA 52 */ -/* This is the data format of the position data from the garmin USB */ -typedef struct { - float alt; /* altitude above WGS 84 (meters) */ - float epe; /* estimated position error, 2 sigma (meters) */ - float eph; /* epe, but horizontal only (meters) */ - float epv; /* epe but vertical only (meters ) */ - int16_t fix; /* 0 - failed integrity check - * 1 - invalid or unavailable fix - * 2 - 2D - * 3 - 3D - * 4 - 2D Diff - * 5 - 3D Diff - */ - double gps_tow; /* gps time os week (seconds) */ - double lat; /* ->latitude (radians) */ - double lon; /* ->longitude (radians) */ - float lon_vel; /* velocity east (meters/second) */ - float lat_vel; /* velocity north (meters/second) */ - float alt_vel; /* velocity up (meters/sec) */ - // Garmin GPS25 uses pkt_id 0x28 and does not output the - // next 3 items - float msl_hght; /* height of WGS 84 above MSL (meters) */ - int16_t leap_sec; /* diff between GPS and UTC (seconds) */ - int32_t grmn_days; -} cpo_pvt_data; - -typedef struct { - uint32_t cycles; - double pr; - uint16_t phase; - int8_t slp_dtct; - uint8_t snr_dbhz; - uint8_t svid; - int8_t valid; -} cpo_rcv_sv_data; - -/* packet type: GARMIN_PKTID_RMD_DATA 53 */ -/* seems identical to the packet id 0x29 from the Garmin GPS 25 */ -typedef struct { - double rcvr_tow; - int16_t rcvr_wn; - cpo_rcv_sv_data sv[GARMIN_CHANNELS]; -} cpo_rcv_data; - -// This is the packet format to/from the Garmin USB -typedef struct { - uint8_t mPacketType; - uint8_t mReserved1; - uint16_t mReserved2; - uint16_t mPacketId; - uint16_t mReserved3; - uint32_t mDataSize; - union { - int8_t chars[MAX_BUFFER_SIZE]; - uint8_t uchars[MAX_BUFFER_SIZE]; - cpo_pvt_data pvt; - cpo_sat_data sats; - } mData; -} Packet_t; - -// useful funcs to read/write ints -// floats and doubles are Intel order only... -static inline void set_int16(uint8_t *buf, uint32_t value) -{ - buf[0] = (uint8_t)(0x0FF & value); - buf[1] = (uint8_t)(0x0FF & (value >> 8)); -} - -static inline void set_int32(uint8_t *buf, uint32_t value) -{ - buf[0] = (uint8_t)(0x0FF & value); - buf[1] = (uint8_t)(0x0FF & (value >> 8)); - buf[2] = (uint8_t)(0x0FF & (value >> 16)); - buf[3] = (uint8_t)(0x0FF & (value >> 24)); -} - -static inline uint16_t get_uint16(const uint8_t *buf) -{ - return (uint16_t)(0xFF & buf[0]) - | ((uint16_t)(0xFF & buf[1]) << 8); -} - -static inline uint32_t get_int32(const uint8_t *buf) -{ - return (uint32_t)(0xFF & buf[0]) - | ((uint32_t)(0xFF & buf[1]) << 8) - | ((uint32_t)(0xFF & buf[2]) << 16) - | ((uint32_t)(0xFF & buf[3]) << 24); -} - -// convert radians to degrees -static inline double radtodeg( double rad) { - return (double)(rad * RAD_2_DEG ); -} - -static gps_mask_t PrintSERPacket(struct gps_device_t *session, unsigned char pkt_id, int pkt_len, unsigned char *buf ); -static gps_mask_t PrintUSBPacket(struct gps_device_t *session, Packet_t *pkt ); - -gps_mask_t PrintSERPacket(struct gps_device_t *session, unsigned char pkt_id - , int pkt_len, unsigned char *buf ) -{ - - gps_mask_t mask = 0; - int i = 0, j = 0; - uint16_t prod_id = 0; - uint16_t ver = 0; - int maj_ver; - int min_ver; - time_t time_l = 0; - double track; - char msg_buf[512] = ""; - char *msg = NULL; - cpo_sat_data *sats = NULL; - cpo_pvt_data *pvt = NULL; - cpo_rcv_data *rmd = NULL; - - gpsd_report(LOG_IO, "PrintSERPacket(, %#02x, %#02x, )\n", pkt_id, pkt_len); - - switch( pkt_id ) { - case ACK: - gpsd_report(LOG_PROG, "ACK\n"); - break; - case NAK: - gpsd_report(LOG_PROG, "NAK\n"); - break; - case GARMIN_PKTID_L001_COMMAND_DATA: - prod_id = get_uint16((uint8_t *)buf); - /*@ -branchstate @*/ - switch ( prod_id ) { - case CMND_ABORT: - msg = "Abort current xfer"; - break; - case CMND_START_PVT_DATA: - msg = "Start Xmit PVT data"; - break; - case CMND_STOP_PVT_DATA: - msg = "Stop Xmit PVT data"; - break; - case CMND_START_RM_DATA: - msg = "Start RMD data"; - break; - default: - (void)snprintf(msg_buf, sizeof(msg_buf), "Unknown: %u", - (unsigned int)prod_id); - msg = msg_buf; - break; - } - /*@ +branchstate @*/ - gpsd_report(LOG_PROG, "Appl, Command Data: %s\n", msg); - break; - case GARMIN_PKTID_PRODUCT_RQST: - gpsd_report(LOG_PROG, "Appl, Product Data req\n"); - break; - case GARMIN_PKTID_PRODUCT_DATA: - prod_id = get_uint16((uint8_t *)buf); - ver = get_uint16((uint8_t *)&buf[2]); - maj_ver = (int)(ver / 100); - min_ver = (int)(ver - (maj_ver * 100)); - gpsd_report(LOG_PROG, "Appl, Product Data, sz: %d\n", pkt_len); - (void)snprintf(session->subtype, sizeof(session->subtype), - "%d: %d.%02d" - , (int)prod_id, maj_ver, min_ver); - gpsd_report(LOG_INF, "Garmin Product ID: %d, SoftVer: %d.%02d\n" - , prod_id, maj_ver, min_ver); - gpsd_report(LOG_INF, "Garmin Product Desc: %s\n" - , &buf[4]); - mask |= DEVICEID_SET; - break; - case GARMIN_PKTID_PVT_DATA: - gpsd_report(LOG_PROG, "Appl, PVT Data Sz: %d\n", pkt_len); - - pvt = (cpo_pvt_data*) buf; - - // 631065600, unix seconds for 31 Dec 1989 Zulu - time_l = (time_t)(631065600 + (pvt->grmn_days * 86400)); - time_l -= pvt->leap_sec; - session->context->leap_seconds = pvt->leap_sec; - session->context->valid = LEAP_SECOND_VALID; - // gps_tow is always like x.999 or x.998 so just round it - time_l += (time_t) round(pvt->gps_tow); - session->gpsdata.fix.time - = session->gpsdata.sentence_time - = (double)time_l; - gpsd_report(LOG_PROG, "time_l: %ld\n", (long int)time_l); - - session->gpsdata.fix.latitude = radtodeg(pvt->lat); - /* sanity check the lat */ - if ( 90.0 < session->gpsdata.fix.latitude ) { - session->gpsdata.fix.latitude = 90.0; - gpsd_report(LOG_INF, "ERROR: Latitude overrange\n"); - } else if ( -90.0 > session->gpsdata.fix.latitude ) { - session->gpsdata.fix.latitude = -90.0; - gpsd_report(LOG_INF, "ERROR: Latitude negative overrange\n"); - } - session->gpsdata.fix.longitude = radtodeg(pvt->lon); - /* sanity check the lon */ - if ( 180.0 < session->gpsdata.fix.longitude ) { - session->gpsdata.fix.longitude = 180.0; - gpsd_report(LOG_INF, "ERROR: Longitude overrange\n"); - } else if ( -180.0 > session->gpsdata.fix.longitude ) { - session->gpsdata.fix.longitude = -180.0; - gpsd_report(LOG_INF, "ERROR: Longitude negative overrange\n"); - } - - // altitude over WGS84 converted to MSL - session->gpsdata.fix.altitude = pvt->alt + pvt->msl_hght; - - // geoid separation from WGS 84 - // gpsd sign is opposite of garmin sign - session->gpsdata.separation = -pvt->msl_hght; - - // Estimated position error in meters. - // We follow the advice at . - // If this assumption changes here, it should also change in - // nmea_parse.c where we analyze PGRME. - session->gpsdata.epe = pvt->epe * (GPSD_CONFIDENCE/CEP50_SIGMA); - session->gpsdata.fix.eph = pvt->eph * (GPSD_CONFIDENCE/CEP50_SIGMA); - session->gpsdata.fix.epv = pvt->epv * (GPSD_CONFIDENCE/CEP50_SIGMA); - - // convert lat/lon to directionless speed - session->gpsdata.fix.speed = hypot(pvt->lon_vel, pvt->lat_vel); - - // keep climb in meters/sec - session->gpsdata.fix.climb = pvt->alt_vel; - - track = atan2(pvt->lon_vel, pvt->lat_vel); - if (track < 0) { - track += 2 * GPS_PI; - } - session->gpsdata.fix.track = radtodeg(track); - - switch ( pvt->fix) { - case 0: - case 1: - default: - // no fix - session->gpsdata.status = STATUS_NO_FIX; - session->gpsdata.fix.mode = MODE_NO_FIX; - break; - case 2: - // 2D fix - session->gpsdata.status = STATUS_FIX; - session->gpsdata.fix.mode = MODE_2D; - break; - case 3: - // 3D fix - session->gpsdata.status = STATUS_FIX; - session->gpsdata.fix.mode = MODE_3D; - break; - case 4: - // 2D Differential fix - session->gpsdata.status = STATUS_DGPS_FIX; - session->gpsdata.fix.mode = MODE_2D; - break; - case 5: - // 3D differential fix - session->gpsdata.status = STATUS_DGPS_FIX; - session->gpsdata.fix.mode = MODE_3D; - break; - } -#ifdef NTPSHM_ENABLE - if (session->context->enable_ntpshm && session->gpsdata.fix.mode > MODE_NO_FIX) - (void) ntpshm_put(session, session->gpsdata.fix.time); -#endif /* NTPSHM_ENABLE */ - - gpsd_report(LOG_PROG, "Appl, mode %d, status %d\n" - , session->gpsdata.fix.mode - , session->gpsdata.status); - - gpsd_report(LOG_INF, "UTC Time: %lf\n", session->gpsdata.fix.time); - gpsd_report(LOG_INF - , "Geoid Separation (MSL-WGS84): from garmin %lf, calculated %lf\n" - , -pvt->msl_hght - , wgs84_separation(session->gpsdata.fix.latitude - , session->gpsdata.fix.longitude)); - - gpsd_report(LOG_INF, "Alt: %.3f, Epe: %.3f, Eph: %.3f, Epv: %.3f, Fix: %d, Gps_tow: %f, Lat: %.3f, Lon: %.3f, LonVel: %.3f, LatVel: %.3f, AltVel: %.3f, MslHgt: %.3f, Leap: %d, GarminDays: %d\n" - , pvt->alt - , pvt->epe - , pvt->eph - , pvt->epv - , pvt->fix - , pvt->gps_tow - , session->gpsdata.fix.latitude - , session->gpsdata.fix.longitude - , pvt->lon_vel - , pvt->lat_vel - , pvt->alt_vel - , pvt->msl_hght - , pvt->leap_sec - , pvt->grmn_days); - - mask |= TIME_SET | LATLON_SET | ALTITUDE_SET | STATUS_SET | MODE_SET | SPEED_SET | TRACK_SET | CLIMB_SET | HERR_SET | VERR_SET | PERR_SET | CYCLE_START_SET; - break; - case GARMIN_PKTID_RMD_DATA: - case GARMIN_PKTID_RMD41_DATA: - rmd = (cpo_rcv_data *) buf; - gpsd_report(LOG_IO, "PVT RMD Data Sz: %d\n", pkt_len); - gpsd_report(LOG_PROG, "PVT RMD rcvr_tow: %f, rcvr_wn: %d\n", - rmd->rcvr_tow, rmd->rcvr_wn); - for ( i = 0 ; i < GARMIN_CHANNELS ; i++ ) { - gpsd_report(LOG_INF, - "PVT RMD Sat: %3u, cycles: %9u, pr: %16.6f, " - "phase: %7.3f, slp_dtct: %3s, snr: %3u, Valid: %3s\n", - rmd->sv[i].svid + 1, rmd->sv[i].cycles, rmd->sv[i].pr, - (rmd->sv[i].phase * 360.0)/2048.0, - rmd->sv[i].slp_dtct!='\0' ? "Yes" : "No", - rmd->sv[i].snr_dbhz, - rmd->sv[i].valid!='\0' ? "Yes" : "No"); - } - break; - - case GARMIN_PKTID_SAT_DATA: - gpsd_report(LOG_PROG, "SAT Data Sz: %d\n", pkt_len); - sats = (cpo_sat_data *)buf; - - session->gpsdata.satellites_used = 0; - memset(session->gpsdata.used,0,sizeof(session->gpsdata.used)); - gpsd_zero_satellites(&session->gpsdata); - for ( i = 0, j = 0 ; i < GARMIN_CHANNELS ; i++, sats++ ) { - gpsd_report(LOG_INF," Sat %3d, snr: %5d, elev: %2d, Azmth: %3d, Stat: %x\n" - , sats->svid - , sats->snr - , sats->elev - , sats->azmth - , sats->status); - - if ( 255 == (int)sats->svid ) { - // Garmin uses 255 for empty - // gpsd uses 0 for empty - continue; - } - - session->gpsdata.PRN[j] = (int)sats->svid; - session->gpsdata.azimuth[j] = (int)sats->azmth; - session->gpsdata.elevation[j] = (int)sats->elev; - // Garmin does not document this. snr is in dB*100 - // Known, but not seen satellites have a dB value of -1*100 - session->gpsdata.ss[j] = (int)round((float)sats->snr / 100); - if (session->gpsdata.ss[j] < 0) { - session->gpsdata.ss[j] = 0; - } - // FIXME: Garmin documents this, but Daniel Dorau - // says the behavior on his GPSMap60CSX - // doesn't match it. - if ( (uint8_t)0 != (sats->status & 4 ) ) { - // used in solution? - session->gpsdata.used[session->gpsdata.satellites_used++] - = (int)sats->svid; - } - session->gpsdata.satellites++; - j++; - - } - mask |= SATELLITE_SET | USED_SET; - break; - case GARMIN_PKTID_PROTOCOL_ARRAY: - // this packet is never requested, it just comes, in some case - // after a GARMIN_PKTID_PRODUCT_RQST - gpsd_report(LOG_INF, "Appl, Product Capability, sz: %d\n", pkt_len); - for ( i = 0; i < pkt_len ; i += 3 ) { - gpsd_report(LOG_INF, " %c%03d\n", buf[i], get_uint16((uint8_t *)&buf[i+1] ) ); - } - break; - default: - gpsd_report(LOG_WARN, "Unknown packet id: %#02x, Sz: %#02x, pkt:%s\n", - pkt_id, pkt_len, - gpsd_hexdump_wrapper(buf, (size_t)pkt_len, LOG_WARN)); - break; - } - gpsd_report(LOG_IO, "PrintSERPacket(, %#02x, %#02x, ) = %#02x\n", - pkt_id, pkt_len, mask); - return mask; -} - - -/*@ -branchstate @*/ -// For debugging, decodes and prints some known packets. -static gps_mask_t PrintUSBPacket(struct gps_device_t *session, Packet_t *pkt) -{ - gps_mask_t mask = 0; - int maj_ver; - int min_ver; - uint32_t mode = 0; - uint16_t prod_id = 0; - uint32_t veri = 0; - uint32_t serial; - uint32_t mDataSize = get_int32( (uint8_t*)&pkt->mDataSize); - -// - uint8_t *buffer = (uint8_t*)pkt; - - gpsd_report(LOG_PROG, "PrintUSBPacket()\n"); -// gem - if ( DLE == pkt->mPacketType) { - gpsd_report(LOG_PROG, "really a SER packet!\n"); - return PrintSERPacket ( session, - (unsigned char)buffer[1], - (int)buffer[2], - (unsigned char*)(buffer + 3)); - } - -// gem - if ( 4096 < mDataSize) { - gpsd_report(LOG_WARN, "bogus packet, size too large=%d\n", mDataSize); - return 0; - } - - (void)snprintf(session->gpsdata.tag, sizeof(session->gpsdata.tag), "%u" - , (unsigned int)pkt->mPacketType); - switch ( pkt->mPacketType ) { - case GARMIN_LAYERID_TRANSPORT: - /* Garmin USB layer specific */ - switch( pkt->mPacketId ) { - case GARMIN_PKTID_TRANSPORT_START_SESSION_REQ: - gpsd_report(LOG_PROG, "Transport, Start Session req\n"); - break; - case GARMIN_PKTID_TRANSPORT_START_SESSION_RESP: - mode = get_int32(&pkt->mData.uchars[0]); - gpsd_report(LOG_PROG, "Transport, Start Session resp, unit: 0x%x\n" - , mode); - break; - default: - gpsd_report(LOG_PROG, "Transport, Packet: Type %d %d %d, ID: %d, Sz: %d\n" - , pkt->mPacketType - , pkt->mReserved1 - , pkt->mReserved2 - , pkt->mPacketId - , mDataSize); - break; - } - break; - case GARMIN_LAYERID_APPL: - /* raw data transport, shared with Garmin Serial Driver */ - - mask = PrintSERPacket(session, - (unsigned char)pkt->mPacketId, - (int)mDataSize, - (unsigned char *)pkt->mData.uchars); - break; - case 75: - // private, garmin USB kernel driver specific - switch( pkt->mPacketId ) { - case PRIV_PKTID_SET_MODE: - prod_id = get_uint16(&pkt->mData.uchars[0]); - gpsd_report(LOG_PROG, "Private, Set Mode: %d\n", prod_id); - break; - case PRIV_PKTID_INFO_REQ: - gpsd_report(LOG_PROG, "Private, ID: Info Req\n"); - break; - case PRIV_PKTID_INFO_RESP: - veri = get_int32(pkt->mData.uchars); - maj_ver = (int)(veri >> 16); - min_ver = (int)(veri & 0xffff); - mode = get_int32(&pkt->mData.uchars[4]); - serial = get_int32(&pkt->mData.uchars[8]); - gpsd_report(LOG_PROG, "Private, ID: Info Resp\n"); - gpsd_report(LOG_INF, "Garmin USB Driver found, Version %d.%d, Mode: %d, GPS Serial# %u\n" - , maj_ver, min_ver, mode, serial); - break; - default: - gpsd_report(LOG_PROG, "Private, Packet: ID: %d, Sz: %d\n" - , pkt->mPacketId - , mDataSize); - break; - } - break; - default: - gpsd_report(LOG_PROG, "Packet: Type %d %d %d, ID: %d, Sz: %d\n" - , pkt->mPacketType - , pkt->mReserved1 - , pkt->mReserved2 - , pkt->mPacketId - , mDataSize); - break; - } - - return mask; -} -/*@ +branchstate @*/ - - -/* build and send a packet w/ USB protocol */ -static void Build_Send_USB_Packet( struct gps_device_t *session, - uint32_t layer_id, uint32_t pkt_id, uint32_t length, uint32_t data ) -{ - uint8_t *buffer = (uint8_t *)session->driver.garmin.Buffer; - Packet_t *thePacket = (Packet_t*)buffer; - ssize_t theBytesReturned = 0; - ssize_t theBytesToWrite = 12 + (ssize_t)length; - - set_int32(buffer, layer_id); - set_int32(buffer+4, pkt_id); - set_int32(buffer+8, length); - if ( 2 == length ) { - set_int16(buffer+12, data); - } else if ( 4 == length ) { - set_int32(buffer+12, data); - } - -#if 0 - gpsd_report(LOG_IO, "SendPacket(), writing %d bytes: %s\n", - theBytesToWrite, - gpsd_hexdump_wrapper(thePacket, theBytesToWrite, LOG_IO)); -#endif - (void)PrintUSBPacket ( session, thePacket); - - theBytesReturned = gpsd_write( session , thePacket, - (size_t)theBytesToWrite); - gpsd_report(LOG_IO, "SendPacket(), wrote %zd bytes\n", - theBytesReturned); - - // Garmin says: - // If the packet size was an exact multiple of the USB packet - // size, we must make a final write call with no data - - // as a practical matter no known packets are 64 bytes long so - // this is untested - - // So here goes just in case - if( 0 == (theBytesToWrite % ASYNC_DATA_SIZE) ) { - char *n = ""; - theBytesReturned = gpsd_write( session , &n, 0); - } -} -/* build and send a packet in serial protocol */ -/* layer_id unused */ -// FIXME: This should go through the common message buffer someday -static void Build_Send_SER_Packet( struct gps_device_t *session, - uint32_t layer_id UNUSED, uint32_t pkt_id, uint32_t length, - uint32_t data ) -{ - uint8_t *buffer = (uint8_t *)session->driver.garmin.Buffer; - uint8_t *buffer0 = buffer; - Packet_t *thePacket = (Packet_t*)buffer; - ssize_t theBytesReturned = 0; - ssize_t theBytesToWrite = 6 + (ssize_t)length; - uint8_t chksum = 0; - - *buffer++ = (uint8_t)DLE; - *buffer++ = (uint8_t)pkt_id; - chksum = pkt_id; - *buffer++ = (uint8_t)length; - chksum += length; - if ( 2 == length ) { - /* carefull! no DLE stuffing here! */ - set_int16(buffer, data); - chksum += buffer[0]; - chksum += buffer[1]; - } else if ( 4 == length ) { - /* carefull! no DLE stuffing here! */ - set_int32(buffer, data); - chksum += buffer[0]; - chksum += buffer[1]; - chksum += buffer[2]; - chksum += buffer[3]; - } - buffer += length; - - // Add checksum - *buffer++ = -chksum; - if ( DLE == -chksum ) { - /* stuff another DLE */ - *buffer++ = (uint8_t)DLE; - theBytesToWrite++; - } - - // Add DLE, ETX - *buffer++ = (uint8_t)DLE; - *buffer++ = (uint8_t)ETX; - -#if 1 - gpsd_report(LOG_IO, "SendPacket(), writing %zd bytes: %s\n", - theBytesToWrite, - gpsd_hexdump_wrapper(thePacket, (size_t)theBytesToWrite, LOG_IO)); -#endif - (void)PrintSERPacket ( session, - (unsigned char)buffer0[1], - (int)buffer0[2], - (unsigned char *)(buffer0 + 3)); - - theBytesReturned = gpsd_write(session, thePacket, - (size_t)theBytesToWrite); - gpsd_report(LOG_IO, "SendPacket(), wrote %zd bytes\n", - theBytesReturned); - -} - - -/* - * garmin_detect() - * - * check that the garmin_gps driver is installed in the kernel - * and that an active USB device is using it. - * - * It does not yet check that the currect device is the one - * attached to the garmin. So if you have a garmin and another - * gps this could be a problem. - * - * this is very linux specific. - * - * return 1 if garmin_gps device found - * return 0 if not - * - */ -static bool garmin_detect(struct gps_device_t *session) -{ - - FILE *fp = NULL; - char buf[256]; - bool ok = false; - - /* check for garmin USB serial driver -- very Linux-specific */ - if (access("/sys/module/garmin_gps", R_OK) != 0) { - gpsd_report(LOG_WARN, "garmin_gps not active.\n"); - return false; - } - // check for a garmin_gps device in /proc - if ( !(fp = fopen( "/proc/bus/usb/devices", "r") ) ) { - gpsd_report(LOG_ERROR, "Can't open /proc/bus/usb/devices\n"); - return false; - } - - ok = false; - while ( 0 != fgets( buf, (int)sizeof(buf), fp ) ) { - if ( strstr( buf, "garmin_gps") ) { - ok = true; - break; - } - } - (void)fclose(fp); - if ( !ok ) { - // no device using garmin now - gpsd_report(LOG_WARN, "garmin_gps not in /proc/bus/usb/devices.\n"); - return false; - } - - if (!gpsd_set_raw(session)) { - gpsd_report(LOG_ERROR, "garmin_detect: error changing port attributes: %s\n", - strerror(errno)); - return false; - } - -#ifdef __UNUSED - Packet_t *thePacket = NULL; - uint8_t *buffer = NULL; - /* reset the buffer and buffer length */ - memset( session->driver.garmin.Buffer, 0, sizeof(session->driver.garmin.Buffer) ); - session->driver.garmin.BufferLen = 0; - - if (sizeof(session->driver.garmin.Buffer) < sizeof(Packet_t)) { - gpsd_report(LOG_ERROR, "garmin_detect: Compile error, garmin.Buffer too small.\n", - strerror(errno)); - return false; - } - - buffer = (uint8_t *)session->driver.garmin.Buffer; - thePacket = (Packet_t*)buffer; -#endif /* __UNUSED__ */ - - // set Mode 1, mode 0 is broken somewhere past 2.6.14 - // but how? - gpsd_report(LOG_PROG, "Set garmin_gps driver mode = 0\n"); - Build_Send_USB_Packet( session, GARMIN_LAYERID_PRIVATE - , PRIV_PKTID_SET_MODE, 4, MODE_GARMIN_SERIAL); - // expect no return packet !? - - return true; -} - -static void garmin_probe_subtype(struct gps_device_t *session, unsigned int seq) -{ - if (seq == 0) { - // Tell the device to send product data - gpsd_report(LOG_PROG, "Get Garmin Product Data\n"); - Build_Send_SER_Packet(session, GARMIN_LAYERID_APPL - , GARMIN_PKTID_PRODUCT_RQST, 0, 0); - - // turn on PVT data 49 - gpsd_report(LOG_PROG, "Set Garmin to send reports every 1 second\n"); - - Build_Send_SER_Packet(session, GARMIN_LAYERID_APPL - , GARMIN_PKTID_L001_COMMAND_DATA, 2, CMND_START_PVT_DATA); - -#if USE_RMD - // turn on RMD data 110 - gpsd_report(LOG_PROG, "Set Garmin to send Raw sat data\n"); - Build_Send_SER_Packet(session, GARMIN_LAYERID_APPL - , GARMIN_PKTID_L001_COMMAND_DATA, 2, CMND_START_RM_DATA); -#endif - } -} - -static void garmin_close(struct gps_device_t *session UNUSED) -{ - /* FIXME -- do we need to put the garmin to sleep? or is closing the port - sufficient? */ - gpsd_report(LOG_PROG, "garmin_close()\n"); - return; -} - -#define Send_ACK() Build_Send_SER_Packet(session, 0, ACK, 0, 0) -#define Send_NAK() Build_Send_SER_Packet(session, 0, NAK, 0, 0) - -/*@ +charint @*/ -gps_mask_t garmin_ser_parse(struct gps_device_t *session) -{ - unsigned char *buf = session->packet.outbuffer; - size_t len = session->packet.outbuflen; - unsigned char data_buf[MAX_BUFFER_SIZE]; - unsigned char c; - int i = 0; - size_t n = 0; - int data_index = 0; - int got_dle = 0; - unsigned char pkt_id = 0; - unsigned char pkt_len = 0; - unsigned char chksum = 0; - gps_mask_t mask = 0; - - gpsd_report(LOG_RAW, "garmin_ser_parse()\n"); - if ( 6 > len ) { - /* WTF? */ - /* minimum packet; [pkt id] [length=0] [chksum] */ - Send_NAK(); - gpsd_report(LOG_RAW+1, "Garmin serial too short: %zd\n", len); - return 0; - } - /* debug */ - for ( i = 0 ; i < (int)len ; i++ ) { - gpsd_report(LOG_RAW+1, "Char: %#02x\n", buf[i]); - } - - if ( '\x10' != buf[0] ) { - Send_NAK(); - gpsd_report(LOG_RAW+1, "buf[0] not DLE\n"); - return 0; - } - n = 1; - pkt_id = buf[n++]; - chksum = pkt_id; - if ( '\x10' == pkt_id ) { - if ( '\x10' != buf[n++] ) { - Send_NAK(); - gpsd_report(LOG_RAW+1, "Bad pkt_id %#02x\n", pkt_id); - return 0; - } - } - - pkt_len = buf[n++]; - chksum += pkt_len; - if ( '\x10' == pkt_len ) { - if ( '\x10' != buf[n++] ) { - gpsd_report(LOG_RAW+1, "Bad pkt_len %#02x\n", pkt_len); - Send_NAK(); - return 0; - } - } - data_index = 0; - for ( i = 0; i < 256 ; i++ ) { - - if ( (int)pkt_len == data_index ) { - // got it all - break; - } - if ( len < n + i ) { - gpsd_report(LOG_RAW+1, "Packet too short %zd < %zd\n", len, n + i); - Send_NAK(); - return 0; - } - c = buf[n + i]; - if ( got_dle ) { - got_dle = 0; - if ( '\x10' != c ) { - Send_NAK(); - gpsd_report(LOG_RAW+1, "Bad DLE %#02x\n", c); - return 0; - } - } else { - chksum += c; - data_buf[ data_index++ ] = c; - if ( '\x10' == c ) { - got_dle = 1; - } - } - } - /* get checksum */ - if ( len < n + i ) { - Send_NAK(); - gpsd_report(LOG_RAW+1, "No checksum, Packet too short %zd < %zd\n", - len, n + i); - return 0; - } - c = buf[n + i++]; - chksum += c; - /* get final DLE */ - if ( len < n + i ) { - Send_NAK(); - gpsd_report(LOG_RAW+1, "No final DLE, Packet too short %zd < %zd\n", - len, n + i); - return 0; - } - c = buf[n + i++]; - if ( '\x10' != c ) { - Send_NAK(); - gpsd_report(LOG_RAW+1, "Final DLE not DLE\n"); - return 0; - } - /* get final ETX */ - if ( len < n + i ) { - Send_NAK(); - gpsd_report(LOG_RAW+1, "No final ETX, Packet too short %zd < %zd\n", - len, n + i); - return 0; - } - c = buf[n + i++]; - if ( '\x03' != c ) { - Send_NAK(); - gpsd_report(LOG_RAW+1, "Final ETX not ETX\n"); - return 0; - } - - /* debug */ - /*@ -usedef -compdef @*/ - for ( i = 0 ; i < data_index ; i++ ) { - gpsd_report(LOG_RAW+1, "Char: %#02x\n", data_buf[i]); - } - - - gpsd_report(LOG_IO, - "garmin_ser_parse() Type: %#02x, Len: %#02x, chksum: %#02x\n", - pkt_id, pkt_len, chksum); - mask = PrintSERPacket(session, pkt_id, pkt_len, data_buf); - - // sending ACK too soon might hang the session - // so send ACK last, after a pause - (void)usleep(300); - Send_ACK(); - /*@ +usedef +compdef @*/ - return mask; -} -/*@ -charint @*/ - -#ifdef ALLOW_RECONFIGURE -static void settle(void) -{ - struct timespec delay, rem; - /*@ -type -unrecog @*/ - memset( &delay, 0, sizeof(delay)); - delay.tv_sec = 0; - delay.tv_nsec = 333000000L; - nanosleep(&delay, &rem); - /*@ +type +unrecog @*/ -} -#endif /* ALLOW_RECONFIGURE */ - -static void garmin_switcher(struct gps_device_t *session, int mode) -{ -#ifdef ALLOW_RECONFIGURE - if (mode == MODE_NMEA) { - /*@ +charint @*/ - const char switcher[] = {0x10,0x0A,0x02,0x26,0x00,0xCE,0x10,0x03}; - // Note hard-coded string length in the next line... - int status = (int)gpsd_write(session, switcher, sizeof(switcher)); - /*@ -charint @*/ - if (status == (int)sizeof(switcher)) { - gpsd_report(LOG_IO, "=> GPS: turn off binary %02x %02x %02x... \n" - , switcher[0], switcher[1], switcher[2]); - } else { - gpsd_report(LOG_ERROR, "=> GPS: FAILED\n"); - } - settle(); // wait 333mS, essential! - - /* once a sec, no binary, no averaging, NMEA 2.3, WAAS */ - (void)nmea_send(session, "$PGRMC1,1,1"); - //(void)nmea_send(fd, "$PGRMC1,1,1,1,,,,2,W,N"); - (void)nmea_send(session, "$PGRMI,,,,,,,R"); - settle(); // wait 333mS, essential! - } else { - (void)nmea_send(session, "$PGRMC1,1,2,1,,,,2,W,N"); - (void)nmea_send(session, "$PGRMI,,,,,,,R"); - // garmin serial binary is 9600 only! - gpsd_report(LOG_ERROR, "NOTE: Garmin binary is 9600 baud only!\n"); - settle(); // wait 333mS, essential! - } -#endif /* ALLOW_RECONFIGURE */ -} - -static ssize_t garmin_control_send(struct gps_device_t *session, - char *buf, size_t buflen) -/* not used by the daemon, it's for gpsctl and friends */ -{ - /*@ -mayaliasunique **/ - session->msgbuflen = buflen; - (void)memcpy(session->msgbuf, buf, buflen); - return gpsd_write(session, session->msgbuf, session->msgbuflen); - /*@ +mayaliasunique **/ -} - -/* this is everything we export */ -#ifdef __UNUSED__ -static int GetPacket (struct gps_device_t *session ); -//----------------------------------------------------------------------------- -// Gets a single packet. -// this is odd, the garmin usb driver will only return 64 bytes, or less -// at a time, no matter what you ask for. -// -// is you ask for less than 64 bytes then the next packet will include -// just the remaining bytes of the last 64 byte packet. -// -// Reading a packet of length Zero, or less than 64, signals the end of -// the entire packet. -// -// The Garmin sample WinXX code also assumes the same behavior, so -// maybe it is something in the USB protocol. -// -// Return: 0 = got a good packet -// -1 = error -// 1 = got partial packet -static int GetPacket (struct gps_device_t *session ) -{ - struct timespec delay, rem; - int cnt = 0; - // int x = 0; // for debug dump - - memset( session->driver.garmin.Buffer, 0, sizeof(Packet_t)); - memset( &delay, 0, sizeof(delay)); - session->driver.garmin.BufferLen = 0; - session->packet.outbuflen = 0; - - gpsd_report(LOG_IO, "GetPacket()\n"); - - for( cnt = 0 ; cnt < 10 ; cnt++ ) { - size_t pkt_size; - // Read async data until the driver returns less than the - // max async data size, which signifies the end of a packet - - // not optimal, but given the speed and packet nature of - // the USB not too bad for a start - ssize_t theBytesReturned = 0; - uint8_t *buf = (uint8_t *)session->driver.garmin.Buffer; - Packet_t *thePacket = (Packet_t*)buf; - - theBytesReturned = read(session->gpsdata.gps_fd - , buf + session->driver.garmin.BufferLen - , ASYNC_DATA_SIZE); - // zero byte returned is a legal value and denotes the end of a - // binary packet. - if ( 0 > theBytesReturned ) { - // read error... - // or EAGAIN, but O_NONBLOCK is never set - gpsd_report(LOG_ERROR, "GetPacket() read error=%d, errno=%d\n" - , theBytesReturned, errno); - continue; - } - gpsd_report(LOG_RAW, "got %d bytes\n", theBytesReturned); -#if 1 - gpsd_report(LOG_IO, "getPacket(), got %d bytes: %s\n", - theBytesReturned, - gpsd_hexdump_wrapper(thePacket, theBytesReturned, LOG_IO)); -#endif - - session->driver.garmin.BufferLen += theBytesReturned; - if ( 256 <= session->driver.garmin.BufferLen ) { - // really bad read error... - gpsd_report(LOG_ERROR, "GetPacket() packet too long, %ld > 255 !\n" - , session->driver.garmin.BufferLen); - session->driver.garmin.BufferLen = 0; - break; - } - pkt_size = 12 + get_int32((uint8_t*)&thePacket->mDataSize); - if ( 12 <= session->driver.garmin.BufferLen) { - // have enough data to check packet size - if ( session->driver.garmin.BufferLen > pkt_size) { - // wrong amount of data in buffer - gpsd_report(LOG_ERROR - , "GetPacket() packet size wrong! Packet: %ld, s/b %ld\n" - , session->driver.garmin.BufferLen - , pkt_size); - session->driver.garmin.BufferLen = 0; - break; - } - } - if ( 64 > theBytesReturned ) { - // zero length, or short, read is a flag for got the whole packet - break; - } - - - /*@ ignore @*/ - delay.tv_sec = 0; - delay.tv_nsec = 3330000L; - while (nanosleep(&delay, &rem) < 0) - continue; - /*@ end @*/ - } - // dump the individual bytes, debug only - // for ( x = 0; x < session->driver.garmin.BufferLen; x++ ) { - // gpsd_report(LOG_RAW+1, "p[%d] = %x\n", x, session->driver.garmin.Buffer[x]); - // } - if ( 10 <= cnt ) { - gpsd_report(LOG_ERROR, "GetPacket() packet too long or too slow!\n"); - return -1; - } - - gpsd_report(LOG_RAW, "GotPacket() sz=%d \n", session->driver.garmin.BufferLen); - session->packet.outbuflen = session->driver.garmin.BufferLen; - return 0; -} -static gps_mask_t garmin_usb_parse(struct gps_device_t *session) -{ - gpsd_report(LOG_PROG, "garmin_usb_parse()\n"); - return PrintUSBPacket(session, (Packet_t*)session->driver.garmin.Buffer); -} - -static ssize_t garmin_get_packet(struct gps_device_t *session) -{ - return (ssize_t)( 0 == GetPacket( session ) ? 1 : 0); -} - -const struct gps_type_t garmin_usb_binary_old = -{ - .type_name = "Garmin USB binary", /* full name of type */ - .packet_type = GARMIN_PACKET; /* associated lexer packet type */ - .trigger = NULL, /* no trigger, it has a probe */ - .channels = GARMIN_CHANNELS, /* consumer-grade GPS */ - .control_send = garmin_control_send, /* send raw bytes */ - .probe_wakeup = NULL, /* no wakeup to be done before hunt */ - .probe_detect = garmin_detect, /* how to detect at startup time */ - .probe_subtype = garmin_probe_subtype, /* get subtype info */ -#ifdef ALLOW_RECONFIGURE - .configurator = NULL, /* does not allow reconfiguration */ -#endif /* ALLOW_RECONFIGURE */ - .get_packet = garmin_get_packet,/* how to grab a packet */ - .parse_packet = garmin_usb_parse, /* parse message packets */ - .rtcm_writer = NULL, /* don't send DGPS corrections */ - .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 = garmin_close, /* close hook */ - .cycle = 1, /* updates every second */ -}; -#endif /* __UNUSED__ */ - -const struct gps_type_t garmin_usb_binary = -{ - .type_name = "Garmin USB binary", /* full name of type */ - .packet_type = GARMIN_PACKET, /* associated lexer packet type */ - .trigger = NULL, /* no trigger, it has a probe */ - .channels = GARMIN_CHANNELS, /* consumer-grade GPS */ - .control_send = garmin_control_send, /* send raw bytes */ - .probe_wakeup = NULL, /* no wakeup to be done before hunt */ - .probe_detect = garmin_detect, /* how to detect at startup time */ - .probe_subtype = garmin_probe_subtype, /* get subtype info */ -#ifdef ALLOW_RECONFIGURE - .configurator = NULL, /* enable what we need */ -#endif /* ALLOW_RECONFIGURE */ - .get_packet = generic_get, /* how to grab a packet */ - .parse_packet = garmin_ser_parse, /* parse message packets */ - .rtcm_writer = NULL, /* don't send DGPS corrections */ - .speed_switcher = NULL, /* no speed switcher */ - .mode_switcher = garmin_switcher, /* how to change modes */ - .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 = garmin_close, /* close hook */ - .cycle = 1, /* updates every second */ -}; - -const struct gps_type_t garmin_ser_binary = -{ - .type_name = "Garmin Serial binary", /* full name of type */ - .packet_type = GARMIN_PACKET, /* associated lexer packet type */ - .trigger = NULL, /* no trigger, it has a probe */ - .channels = GARMIN_CHANNELS, /* consumer-grade GPS */ - .control_send = garmin_control_send, /* send raw bytes */ - .probe_wakeup = NULL, /* no wakeup to be done before hunt */ - .probe_detect = NULL, /* how to detect at startup time */ - .probe_subtype = NULL, /* initialize the device */ -#ifdef ALLOW_RECONFIGURE - .configurator = NULL, /* enable what we need */ -#endif /* ALLOW_RECONFIGURE */ - .get_packet = generic_get, /* how to grab a packet */ - .parse_packet = garmin_ser_parse, /* parse message packets */ - .rtcm_writer = NULL, /* don't send DGPS corrections */ - .speed_switcher = NULL, /* no speed switcher */ - .mode_switcher = garmin_switcher, /* how to change modes */ - .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, /* close hook */ - .cycle = 1, /* updates every second */ -}; - -#endif /* GARMIN_ENABLE */ - diff --git a/garmin_txt.c b/garmin_txt.c deleted file mode 100644 index ecd13aac..00000000 --- a/garmin_txt.c +++ /dev/null @@ -1,405 +0,0 @@ -/* - * 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 size_t length, const char *prefix, const double dividor, /*@out@*/double *result) -{ - char buf[10]; - float sign = 1.0; - int preflen = (int)strlen(prefix); - int offset = 1; /* assume one character prefix (E,W,S,N,U, D, etc) */ - int intresult; - - /* splint is buggy here, thinks buf can be a null pointer */ - /*@ -mustdefine -nullderef -nullpass @*/ - if (length >= sizeof(buf)) { - gpsd_report(LOG_ERROR, "internal buffer too small\n"); - return -1; - } - - bzero(buf, (int)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; - } - /*@ +mustdefine +nullderef +nullpass @*/ - - 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 size_t length, const int min, const int max, /*@out@*/int *result) -{ - char buf[3]; - int res; - - /*@ -mustdefine @*/ - if (length >= sizeof(buf)) { - gpsd_report(LOG_ERROR, "internal buffer too small\n"); - return -1; - } - - bzero(buf, (int)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; - } - - /*@ -nullpass @*/ /* splint bug */ - 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; - } - /*@ +mustdefine +nullpass @*/ -} - - -/************************************************************************** - * - * 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_wrapper(session->packet.outbuffer, - session->packet.outbuflen, LOG_RAW)); - - 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 = (char)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/italk.c b/italk.c deleted file mode 100644 index 68d526ac..00000000 --- a/italk.c +++ /dev/null @@ -1,527 +0,0 @@ -/* $Id$ */ -/* - * Driver for the iTalk binary protocol used by FasTrax - */ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "gpsd_config.h" -#include "gpsd.h" -#if defined(ITRAX_ENABLE) && defined(BINARY_ENABLE) - -#include "bits.h" -#include "italk.h" - -static gps_mask_t italk_parse(struct gps_device_t *, unsigned char *, size_t); -static gps_mask_t decode_itk_navfix(struct gps_device_t *, unsigned char *, size_t); -static gps_mask_t decode_itk_prnstatus(struct gps_device_t *, unsigned char *, size_t); -static gps_mask_t decode_itk_utcionomodel(struct gps_device_t *, unsigned char *, size_t); - -static gps_mask_t decode_itk_navfix(struct gps_device_t *session, unsigned char *buf, size_t len) -{ - unsigned int tow; - unsigned short gps_week, flags, cflags, pflags; - gps_mask_t mask = 0; - double epx, epy, epz, evx, evy, evz; - double t; - - if (len != 296){ - gpsd_report(LOG_PROG, "ITALK: bad NAV_FIX (len %zu, should be 296)\n", - len); - return -1; - } - - flags = getleuw(buf, 7 + 4); - cflags = getleuw(buf, 7 + 6); - pflags = getleuw(buf, 7 + 8); - - session->gpsdata.status = STATUS_NO_FIX; - session->gpsdata.fix.mode = MODE_NO_FIX; - mask = ONLINE_SET | MODE_SET | STATUS_SET | CYCLE_START_SET; - - /* just bail out if this fix is not marked valid */ - if (0 != (pflags & FIX_FLAG_MASK_INVALID) || 0 == (flags & FIXINFO_FLAG_VALID)) - return mask; - - gps_week = (ushort)getlesw(buf, 7 + 82); - tow = getleul(buf, 7 + 84); - t = gpstime_to_unix((int)gps_week, tow/1000.0) - session->context->leap_seconds; - session->gpsdata.sentence_time = t; - session->gpsdata.fix.time = t; - mask |= TIME_SET; - - epx = (double)(getlesl(buf, 7 + 96)/100.0); - epy = (double)(getlesl(buf, 7 + 100)/100.0); - epz = (double)(getlesl(buf, 7 + 104)/100.0); - evx = (double)(getlesl(buf, 7 + 186)/1000.0); - evy = (double)(getlesl(buf, 7 + 190)/1000.0); - evz = (double)(getlesl(buf, 7 + 194)/1000.0); - ecef_to_wgs84fix(&session->gpsdata, epx, epy, epz, evx, evy, evz); - mask |= LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET | CLIMB_SET ; - session->gpsdata.fix.eph = (double)(getlesl(buf, 7 + 252)/100.0); - session->gpsdata.fix.eps = (double)(getlesl(buf, 7 + 254)/100.0); - - session->gpsdata.satellites_used = 0xffff ^ getub(buf, 7 + 16); - mask |= USED_SET ; - - if (flags & FIX_CONV_DOP_VALID){ - session->gpsdata.hdop = (double)(getleuw(buf, 7 + 56)/100.0); - session->gpsdata.gdop = (double)(getleuw(buf, 7 + 58)/100.0); - session->gpsdata.pdop = (double)(getleuw(buf, 7 + 60)/100.0); - session->gpsdata.vdop = (double)(getleuw(buf, 7 + 62)/100.0); - session->gpsdata.tdop = (double)(getleuw(buf, 7 + 64)/100.0); - mask |= HDOP_SET | GDOP_SET | PDOP_SET | VDOP_SET | TDOP_SET; - } - - if ((pflags & FIX_FLAG_MASK_INVALID) == 0 && (flags & FIXINFO_FLAG_VALID) != 0){ - if (pflags & FIX_FLAG_3DFIX) - session->gpsdata.fix.mode = MODE_3D; - else - session->gpsdata.fix.mode = MODE_2D; - - if (pflags & FIX_FLAG_DGPS_CORRECTION) - session->gpsdata.status = STATUS_DGPS_FIX; - else - session->gpsdata.status = STATUS_FIX; - } - - return mask; -} - -static gps_mask_t decode_itk_prnstatus(struct gps_device_t *session, unsigned char *buf, size_t len) -{ - unsigned int i, tow, nsv, nchan, st; - unsigned short gps_week; - double t; - - if (len < 62){ - gpsd_report(LOG_PROG, "ITALK: runt PRN_STATUS (len=%zu)\n", len); - return ERROR_SET; - } - - gps_week = getleuw(buf, 7 + 4); - tow = getleul(buf, 7 + 6); - t = gpstime_to_unix((int)gps_week, tow/1000.0) - session->context->leap_seconds; - session->gpsdata.sentence_time = session->gpsdata.fix.time = t; - - gpsd_zero_satellites(&session->gpsdata); - nchan = (unsigned int)((len - 10 - 52) / 20); - nsv = 0; - for (i = st = 0; i < nchan; i++) { - unsigned int off = 7+ 52 + 20 * i; - unsigned short flags; - - flags = getleuw(buf, off); - session->gpsdata.ss[i] = (int)getleuw(buf, off+2)&0xff; - session->gpsdata.PRN[i] = (int)getleuw(buf, off+4)&0xff; - session->gpsdata.elevation[i] = (int)getlesw(buf, off+6)&0xff; - session->gpsdata.azimuth[i] = (int)getlesw(buf, off+8)&0xff; - if (session->gpsdata.PRN[i]){ - st++; - if (flags & PRN_FLAG_USE_IN_NAV) - session->gpsdata.used[nsv++] = session->gpsdata.PRN[i]; - } - } - session->gpsdata.satellites = (int)st; - session->gpsdata.satellites_used = (int)nsv; - - return USED_SET | SATELLITE_SET | TIME_SET; -} - -static gps_mask_t decode_itk_utcionomodel(struct gps_device_t *session, unsigned char *buf, size_t len) -{ - unsigned int tow; - int leap; - unsigned short gps_week, flags; - double t; - - if (len != 64){ - gpsd_report(LOG_PROG, - "ITALK: bad UTC_IONO_MODEL (len %zu, should be 64)\n", len); - return ERROR_SET; - } - - flags = getleuw(buf, 7); - if (0 == (flags & UTC_IONO_MODEL_UTCVALID)) - return 0; - - leap = (int)getleuw(buf, 7 + 24); - if (session->context->leap_seconds < leap) - session->context->leap_seconds = leap; - - gps_week = getleuw(buf, 7 + 36); - tow = getleul(buf, 7 + 38); - t = gpstime_to_unix((int)gps_week, tow/1000.0) - session->context->leap_seconds; - session->gpsdata.sentence_time = session->gpsdata.fix.time = t; - - return TIME_SET; -} - -/*@ +charint -usedef -compdef @*/ -static ssize_t italk_control_send(struct gps_device_t *session, - char *msg, size_t msglen) -{ - ssize_t status; - - /*@ -mayaliasunique **/ - session->msgbuflen = msglen; - (void)memcpy(session->msgbuf, msg, msglen); - /*@ +mayaliasunique **/ - /* we may need to dump the message */ - gpsd_report(LOG_IO, "writing italk control type %02x:%s\n", - msg[0], gpsd_hexdump_wrapper(msg, msglen, LOG_IO)); -#ifdef ALLOW_RECONFIGURE - status = write(session->gpsdata.gps_fd, msg, msglen); - (void)tcdrain(session->gpsdata.gps_fd); -#else - status = -1; -#endif /* ALLOW_RECONFIGURE */ - return(status); -} -/*@ -charint +usedef +compdef @*/ - -/*@ +charint @*/ -static gps_mask_t italk_parse(struct gps_device_t *session, unsigned char *buf, size_t len) -{ - unsigned int type; - gps_mask_t mask = 0; - - if (len == 0) - return 0; - - type = (uint)getub(buf, 4); - /* we may need to dump the raw packet */ - gpsd_report(LOG_RAW, "raw italk packet type 0x%02x length %zu: %s\n", - type, len, gpsd_hexdump_wrapper(buf, len, LOG_RAW)); - - switch (type) - { - case ITALK_NAV_FIX: - gpsd_report(LOG_IO, "iTalk NAV_FIX len %zu\n", len); - mask = decode_itk_navfix(session, buf, len); - break; - case ITALK_PRN_STATUS: - gpsd_report(LOG_IO, "iTalk PRN_STATUS len %zu\n", len); - mask = decode_itk_prnstatus(session, buf, len); - break; - case ITALK_UTC_IONO_MODEL: - gpsd_report(LOG_IO, "iTalk UTC_IONO_MODEL len %zu\n", len); - mask = decode_itk_utcionomodel(session, buf, len); - break; - - case ITALK_ACQ_DATA: - gpsd_report(LOG_IO, "iTalk ACQ_DATA len %zu\n", len); - break; - case ITALK_TRACK: - gpsd_report(LOG_IO, "iTalk TRACK len %zu\n", len); - break; - case ITALK_PSEUDO: - gpsd_report(LOG_IO, "iTalk PSEUDO len %zu\n", len); - break; - case ITALK_RAW_ALMANAC: - gpsd_report(LOG_IO, "iTalk RAW_ALMANAC len %zu\n", len); - break; - case ITALK_RAW_EPHEMERIS: - gpsd_report(LOG_IO, "iTalk RAW_EPHEMERIS len %zu\n", len); - break; - case ITALK_SUBFRAME: - gpsd_report(LOG_IO, "iTalk SUBFRAME len %zu\n", len); - break; - case ITALK_BIT_STREAM: - gpsd_report(LOG_IO, "iTalk BIT_STREAM len %zu\n", len); - break; - - case ITALK_AGC: - case ITALK_SV_HEALTH: - case ITALK_PRN_PRED: - case ITALK_FREQ_PRED: - case ITALK_DBGTRACE: - case ITALK_START: - case ITALK_STOP: - case ITALK_SLEEP: - case ITALK_STATUS: - case ITALK_ITALK_CONF: - case ITALK_SYSINFO: - case ITALK_ITALK_TASK_ROUTE: - case ITALK_PARAM_CTRL: - case ITALK_PARAMS_CHANGED: - case ITALK_START_COMPLETED: - case ITALK_STOP_COMPLETED: - case ITALK_LOG_CMD: - case ITALK_SYSTEM_START: - case ITALK_STOP_SEARCH: - case ITALK_SEARCH: - case ITALK_PRED_SEARCH: - case ITALK_SEARCH_DONE: - case ITALK_TRACK_DROP: - case ITALK_TRACK_STATUS: - case ITALK_HANDOVER_DATA: - case ITALK_CORE_SYNC: - case ITALK_WAAS_RAWDATA: - case ITALK_ASSISTANCE: - case ITALK_PULL_FIX: - case ITALK_MEMCTRL: - case ITALK_STOP_TASK: - gpsd_report(LOG_IO, - "iTalk not processing packet: id 0x%02x length %zu\n", - type, len); - break; - default: - gpsd_report(LOG_IO, "iTalk unknown packet: id 0x%02x length %zu\n", - type, len); - } - if (mask == ERROR_SET) - mask = 0; - else - (void)snprintf(session->gpsdata.tag, sizeof(session->gpsdata.tag), - "ITK-%02x",type); - - return mask | ONLINE_SET; -} -/*@ -charint @*/ - -static gps_mask_t italk_parse_input(struct gps_device_t *session) -{ - gps_mask_t st; - - if (session->packet.type == ITALK_PACKET){ - st = italk_parse(session, session->packet.outbuffer, session->packet.outbuflen); - session->gpsdata.driver_mode = MODE_BINARY; /* binary */ - return st; -#ifdef NMEA_ENABLE - } else if (session->packet.type == NMEA_PACKET) { - st = nmea_parse((char *)session->packet.outbuffer, session); - session->gpsdata.driver_mode = MODE_NMEA; /* NMEA */ - return st; -#endif /* NMEA_ENABLE */ - } else - return 0; -} - -static bool italk_set_mode(struct gps_device_t *session UNUSED, - speed_t speed UNUSED, - char parity UNUSED, int stopbits UNUSED, - bool mode UNUSED) -{ -#if 0 - /*@ +charint @*/ - char msg[] = {0,}; - - /* HACK THE MESSAGE */ - - return (italk_control_send(session, msg, sizeof(msg)) != -1); - /*@ +charint @*/ -#endif - - return false; /* until this actually works */ -} - -static bool italk_speed(struct gps_device_t *session, - speed_t speed, char parity, int stopbits) -{ - return italk_set_mode(session, speed, parity, stopbits, true); -} - -static void italk_mode(struct gps_device_t *session, int mode) -{ - if (mode == MODE_NMEA) { - (void)italk_set_mode(session, - session->gpsdata.baudrate, - (char)session->gpsdata.parity, - (int)session->gpsdata.stopbits, - false); - } -} - -#ifdef ALLOW_RECONFIGURE -static void italk_configurator(struct gps_device_t *session, unsigned int seq) -{ - if (seq == 0 && session->packet.type == NMEA_PACKET) - (void)italk_set_mode(session, - session->gpsdata.baudrate, - (char)session->gpsdata.parity, - (int)session->gpsdata.stopbits, - true); -} -#endif /* ALLOW_RECONFIGURE */ - -#ifdef __not_yet__ -static void italk_ping(struct gps_device_t *session) -/* send a "ping". it may help us detect an itrax more quickly */ -{ - char *ping = ""; - (void)gpsd_write(session, ping, 3); -} -#endif /* __not_yet__ */ - -/* this is everything we export */ -const struct gps_type_t italk_binary = -{ - .type_name = "iTalk binary", /* full name of type */ - .packet_type = ITALK_PACKET, /* associated lexer packet type */ - .trigger = NULL, /* recognize the type */ - .channels = 12, /* consumer-grade GPS */ - .control_send = italk_control_send, /* how to send a control string */ - .probe_wakeup = NULL, /* no wakeup to be done before hunt */ - .probe_detect = NULL, /* how to detect at startup time */ - .probe_subtype = NULL, /* initialize the device */ -#ifdef ALLOW_RECONFIGURE - .configurator = italk_configurator,/* configure the device */ -#endif /* ALLOW_RECONFIGURE */ - .get_packet = generic_get, /* use generic packet grabber */ - .parse_packet = italk_parse_input,/* parse message packets */ - .rtcm_writer = pass_rtcm, /* send RTCM data straight */ - .speed_switcher = italk_speed, /* we can change baud rates */ - .mode_switcher = italk_mode, /* there is a 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 close hook */ - .cycle = 1, /* updates every second */ -}; -#endif /* defined(ITRAX_ENABLE) && defined(BINARY_ENABLE) */ - -#ifdef ANCIENT_ITRAX_ENABLE -/************************************************************************** - * - * The NMEA mode of the iTrax chipset, as used in the FastTrax and others. - * - * As described by v1.31 of the NMEA Protocol Specification for the - * iTrax02 Evaluation Kit, 2003-06-12. - * v1.18 of the manual, 2002-19-6, describes effectively - * the same protocol, but without ZDA. - * - **************************************************************************/ - -/* - * Enable GGA=0x2000, RMC=0x8000, GSA=0x0002, GSV=0x0001, ZDA=0x0004. - * Disable GLL=0x1000, VTG=0x4000, FOM=0x0020, PPS=0x0010. - * This is 82+75+67+(3*60)+34 = 438 characters - * - * 1200 => at most 1 fix per 4 seconds - * 2400 => at most 1 fix per 2 seconds - * 4800 => at most 1 fix per 1 seconds - * 9600 => at most 2 fixes per second - * 19200 => at most 4 fixes per second - * 57600 => at most 13 fixes per second - * 115200 => at most 26 fixes per second - * - * We'd use FOM, but they don't specify a confidence interval. - */ -#define ITRAX_MODESTRING "$PFST,NMEA,A007,%d\r\n" - -static int literal_send(int fd, const char *fmt, ... ) -/* ship a raw command to the GPS */ -{ - ssize_t status; - va_list ap; - - va_start(ap, fmt) ; - (void)vsnprintf(session->msgbuf, sizeof(session->msgbuf), fmt, ap); - va_end(ap); - session->msgbuflen = strlen(session->msgbuf); - return gpsd_write(fd, session->msgbuf, session->msgbuflen); -} - -static void itrax_probe_subtype(struct gps_device_t *session, unsigned int seq) -/* start it reporting */ -{ - if (seq == 0) { - /* initialize GPS clock with current system time */ - struct tm when; - double integral, fractional; - time_t intfixtime; - char buf[31], frac[6]; - fractional = modf(timestamp(), &integral); - intfixtime = (time_t)integral; - (void)gmtime_r(&intfixtime, &when); - /* FIXME: so what if my local clock is wrong? */ - (void)strftime(buf, sizeof(buf), "$PFST,INITAID,%H%M%S.XX,%d%m%y\r\n", &when); - (void)snprintf(frac, sizeof(frac), "%.2f", fractional); - buf[21] = frac[2]; buf[22] = frac[3]; - (void)literal_send(session->gpsdata.gps_fd, buf); - /* maybe this should be considered a reconfiguration? */ - (void)literal_send(session->gpsdata.gps_fd, "$PFST,START\r\n"); - } -} - -#ifdef ALLOW_RECONFIGURE -static void itrax_configurator(struct gps_device_t *session, int seq) -/* set synchronous mode */ -{ - if (seq == 0) { - (void)literal_send(session->gpsdata.gps_fd, "$PFST,SYNCMODE,1\r\n"); - (void)literal_send(session->gpsdata.gps_fd, - ITRAX_MODESTRING, session->gpsdata.baudrate); - } -} -#endif /* ALLOW_RECONFIGURE */ - -static bool itrax_speed(struct gps_device_t *session, - speed_t speed, char parity UNUSED, int stopbits UNUSED) -/* change the baud rate */ -{ -#ifdef ALLOW_RECONFIGURE - return literal_send(session->gpsdata.gps_fd, ITRAX_MODESTRING, speed) >= 0; -#else - return false; -#endif /* ALLOW_RECONFIGURE */ -} - -static bool itrax_rate(struct gps_device_t *session, double rate) -/* change the sample rate of the GPS */ -{ -#ifdef ALLOW_RECONFIGURE - return literal_send(session->gpsdata.gps_fd, "$PSFT,FIXRATE,%d\r\n", rate) >= 0; -#else - return false; -#endif /* ALLOW_RECONFIGURE */ -} - -static void itrax_wrap(struct gps_device_t *session) -/* stop navigation, this cuts the power drain */ -{ -#ifdef ALLOW_RECONFIGURE - (void)literal_send(session->gpsdata.gps_fd, "$PFST,SYNCMODE,0\r\n"); -#endif /* ALLOW_RECONFIGURE */ - (void)literal_send(session->gpsdata.gps_fd, "$PFST,STOP\r\n"); -} - -/*@ -redef @*/ -const static struct gps_type_t itrax = { - .type_name = "iTrax", /* full name of type */ - .packet_type = NMEA_PACKET; /* associated lexer packet type */ - .trigger = "$PFST,OK", /* tells us to switch to Itrax */ - .channels = 12, /* consumer-grade GPS */ - .control_send = italk_control_send,/* how to send a control string */ - .probe_wakeup = NULL, /* no wakeup to be done before hunt */ - .probe_detect = NULL, /* no probe */ - .probe_subtype = itrax_probe_subtype, /* initialize */ -#ifdef ALLOW_RECONFIGURE - .configurator = itrax_configurator,/* set synchronous mode */ -#endif /* ALLOW_RECONFIGURE */ - .get_packet = generic_get, /* how to get a packet */ - .parse_packet = nmea_parse_input, /* how to interpret a packet */ - .rtcm_writer = NULL, /* iTrax doesn't support DGPS/WAAS/EGNOS */ - .speed_switcher= itrax_speed, /* how to change speeds */ - .mode_switcher = NULL, /* no mode switcher */ - .rate_switcher = itrax_rate, /* there's a sample-rate switcher */ - .cycle_chars = 438, /* not relevant, no rate switch */ -#ifdef ALLOW_RECONFIGURE - .revert = NULL, /* no setting-reversion method */ -#endif /* ALLOW_RECONFIGURE */ - .wrapup = itrax_wrap, /* sleep the receiver */ - .cycle = 1, /* updates every second */ -}; -/*@ -redef @*/ -#endif /* ITRAX_ENABLE */ - diff --git a/italk.h b/italk.h deleted file mode 100644 index ec7ee111..00000000 --- a/italk.h +++ /dev/null @@ -1,498 +0,0 @@ -/* $Id$ */ -#ifndef _GPSD_ITALK_H_ -#define _GPSD_ITALK_H_ - -/* 0 and 1 are responses to the ping for iTalk and NMEA respectively */ -#define PROTO_ITALK 0 -#define PROTO_NMEA 1 - -/* - * Assistance from Timo Ylhainen of Fastrax is acknowledged and appreciated. - * - * iTalk is a messaging system which communicates between tasks, which may - * be running on different devices (nodes). For our purposes (receiver - * configuration), we will probably be sending to the SYSTEM task. - */ - -#define TASK_MASK 0x1f /* 5 low bits of src/dst fields */ -#define NODE_MASK 0xe0 /* 3 high bits of src/dst fields */ -#define NODE_UNDEF 0x00 /* Used in message routing */ -#define NODE_ITRAX 0x20 /* The receiver */ -#define NODE_HOST 0x40 /* Software on your computer */ -#define NODE_GPSWB 0x60 /* GPSWorkbench seems to be HOST|ITRAX */ - -/* XXX These defines will likely be replaced by an enum - * once I map every message to the task that sent it. - */ -/* System controller on the receiver */ -#define TASK_SYSTEM 0 -/* Acquisition & Tracking messages (PD) */ -#define TASK_TRACK1 2 -#define TASK_TRACK2 3 -/* Data decoding messages (PD) */ -#define TASK_DATA 4 -/* Navigation messages are sent by these tasks (PD) */ -#define TASK_NAV1 7 -#define TASK_NAV2 8 -#define TASK_NAV3 9 -/* Host controller software (PD) */ -#define TASK_HOST 31 - -/* iTalk Message IDs - isuite.fastrax.fi/sdk/331/Protocols/PRO_MsgId.html */ -#define ITALK_ACQ_DATA 1 -#define ITALK_PRN_STATUS 2 -#define ITALK_TRACK 3 -#define ITALK_PSEUDO 4 -#define ITALK_AGC 6 -#define ITALK_NAV_FIX 7 -#define ITALK_RAW_ALMANAC 9 -#define ITALK_RAW_EPHEMERIS 10 -#define ITALK_SV_HEALTH 11 -#define ITALK_UTC_IONO_MODEL 12 -#define ITALK_PRN_PRED 13 -#define ITALK_FREQ_PRED 14 -#define ITALK_SUBFRAME 15 -#define ITALK_BIT_STREAM 18 -#define ITALK_DBGTRACE 19 -#define ITALK_START 64 -#define ITALK_STOP 65 -#define ITALK_SLEEP 66 -#define ITALK_STATUS 67 -#define ITALK_ITALK_CONF 68 -#define ITALK_SYSINFO 69 -#define ITALK_ITALK_TASK_ROUTE 70 -#define ITALK_PARAM_CTRL 71 -#define ITALK_PARAMS_CHANGED 72 -#define ITALK_START_COMPLETED 73 -#define ITALK_STOP_COMPLETED 74 -#define ITALK_LOG_CMD 75 -#define ITALK_SYSTEM_START 76 -#define ITALK_STOP_SEARCH 79 -#define ITALK_SEARCH 80 -#define ITALK_PRED_SEARCH 81 -#define ITALK_SEARCH_DONE 82 -#define ITALK_TRACK_DROP 88 -#define ITALK_TRACK_STATUS 90 -#define ITALK_HANDOVER_DATA 92 -#define ITALK_CORE_SYNC 93 -#define ITALK_WAAS_RAWDATA 96 -#define ITALK_ASSISTANCE 98 -#define ITALK_PULL_FIX 99 -#define ITALK_MEMCTRL 112 -#define ITALK_STOP_TASK 255 - -/* NAV_FIX */ -#define FIX_CONV_VEL_VALID 0x0002 -#define FIX_CONV_ACC_VALID 0x0004 -#define FIX_CONV_DOP_VALID 0x0010 -#define FIX_CONV_ERR_VALID 0x0020 -#define FIX_CONV_UTC_VALID 0x0040 -#define FIX_CONV_UND_VALID 0x0080 -#define FIX_CONV_MAG_VALID 0x0100 -#define FIX_CONV_GRID_VALID 0x0200 -#define FIX_CONV_VEL_ESTIMATED 0x0400 - -#define FIX_FLAG_POS_REJECT_FOM 0x0003 -#define FIX_FLAG_POS_REJECT_DOP 0x0004 -#define FIX_FLAG_POS_PINNING 0x0020 - -#define FIX_FLAG_VEL_REJECT_RES 0x0003 -#define FIX_FLAG_ACCELERATION 0x4000 -#define FIX_FLAG_VEL_RELIABLE 0x0020 -#define FIX_FLAG_VEL_RELIABLE_3D 0x0040 - -#define FIX_FLAG_MASK_INVALID 0x0007 -#define FIX_FLAG_REJECT_NUM_SV 0x0001 -#define FIX_FLAG_REJECT_POSTRAIM 0x0002 -#define FIX_FLAG_REJECT_OTHER 0x0007 -#define FIX_FLAG_RELIABLE 0x0008 -#define FIX_FLAG_PF_RAIM 0x0010 -#define FIX_FLAG_3DFIX 0x0100 -#define FIX_FLAG_DGPS_CORRECTION 0x0200 -#define FIX_FLAG_TROPO 0x0400 -#define FIX_FLAG_IONO 0x0800 -#define FIX_FLAG_INS 0x2000 - -#define FIXINFO_FLAG_VALID 0x0002 -#define FIXINFO_FLAG_NEW_FIX 0x0004 -#define FIXINFO_FLAG_SKY_FIX 0x0008 -#define FIXINFO_FLAG_AID_GPSTIME 0x0010 -#define FIXINFO_FLAG_AID_TIMESTAMP 0x0020 -#define FIXINFO_FLAG_AID_EPHEMERIS 0x0040 -#define FIXINFO_FLAG_AID_ALTITUDE 0x0080 -#define FIXINFO_FLAG_KALMAN 0x1000 -#define FIXINFO_FLAG_INTERNAL 0x2000 -#define FIXINFO_FLAG_FIRSTFIX 0x4000 - -/* PRN_STATUS */ -#define PRN_FLAG_FOUND 0x0001 -#define PRN_FLAG_TRACKING 0x0002 -#define PRN_FLAG_USE_IN_NAV 0x0004 - -/* UTC_IONO_MODEL */ -#define UTC_IONO_MODEL_UTCVALID 0x0001 -#define UTC_IONO_MODEL_IONOVALID 0x0002 - - -/* MEMCTRL */ -#define MEM_WRITE 0x0002 -#define MEM_READD 0x0003 -#define MEM_BOOT 0x0004 -#define MEM_ERASE 0x0006 -#define MEM_XTAL_CALIBRATE 0x000a -/* BOOT flags based on isuite.fastrax.fi/sdk/331/Protocols/PRO_NMEA.html */ -#define MEM_BOOT_NORMAL 0x0000 -#define MEM_BOOT_INT_FWLOADER 0x0001 -#define MEM_BOOT_DL_FWLOADER 0x0002 -#define MEM_BOOT_RELOC_ALTFW 0x0003 - -/* Config Parameters - isuite.fastrax.fi/sdk/331/System/SYS_Parameters.html */ -/* System parameters */ -#define SYS_SET_ID 0x0001 -#define SYS_FACTORY_SET_ID 0x0002 -#define SYS_AUTOSTART 0x0380 -#define START_MODE_AUTO 0x0301 -#define SYS_LKG_SAVE_TIME_LIMIT 0x0008 -#define SYS_LKG_SAVE_DIST_LIMIT 0x0009 -#define SYS_LKG_SAVE_STOP_TIME_LIMIT 0x000a -#define SYS_WATCHDOG 0x0028 -#define SYS_WATCHDOG_TIMEOUT 0x0029 -#define SYS_BOOT_ERASE_PARAMS 0x0080 -#define SYS_ENABLE_UI_LEDS 0x0081 - -/* Protocols parameters */ -#define SYS_ITALK_PORT 0x0010 -#define SYS_ITALK_SPEED 0x0011 -#define SYS_ITALK_MASK 0x0012 -#define SYS_NMEA_PORT 0x0020 -#define SYS_NMEA_SPEED 0x0021 -#define SYS_NMEA_MASK 0x0022 -#define TRACK_ALT_MSG_ROUTING 0x047f -#define OBS_ALT_MSG_ROUTING 0x047e - -/* Fix Conversion parameters */ -#define NAV_DATUM_ID 0x0b08 -#define NAV_GRID_ID 0x0b09 -#define NAV_GRID_NUMBER 0x0b0a -#define NAV_HEAD_VEL_THR 0x0b0b -#define NAV_HEAD_VEL_FILTER 0x0b0c -#define NAV_HEAD_VEL_THRMAX 0x0b0d -#define NAV_HEAD_VEL_THR_PLL 0x0b0e -#define NAV_HEAD_VEL_THRMAX_PLL 0x0b0f -#define NAV_HOLD_HEADING_IF_NO_FIX 0x0bd0 - -/* General navigation parameters */ -#define NAV_MODE 0x0b01 -#define NAV_FIX_INTERVAL 0x0b02 -#define NAV_OUTPUT_INTERVAL 0x0b03 -#define NAV_FOM_LIMIT 0x0b10 -#define NAV_VEL_FOM_LIMIT 0x0b15 -#define NAV_HDOP_LIMIT 0x0b11 -#define NAV_VDOP_LIMIT 0x0b12 -#define NAV_ALT_LIMIT 0x0b13 -#define NAV_VEL_LIMIT 0x0b14 -#define NAV_EXT_AIDING_ALT 0x0b20 -#define NAV_CS_INIT_VAR 0x0b30 -#define NAV_CS_PROC_VAR 0x0b31 -#define NAV_CS_MEAS_VAR 0x0b32 -#define NAV_FILTER_VEL_LOW 0x0b33 -#define NAV_FILTER_VEL_HIGH 0x0b34 -#define NAV_MAX_LKGAGE 0x0b40 -#define NAV_MAX_2D_FIX_SEC 0x0b41 -#define NAV_CARRIERSMOOTHING_ENA 0x0b81 -#define NAV_OLD_DATA_ENA 0x0b82 -#define NAV_SNR_WEIGHTING_ENA 0x0b83 -#define NAV_NORMAL_ENV_ENA 0x0b84 -#define NAV_IONO_ENA 0x0b85 -#define NAV_TROPO_ENA 0x0b87 -#define NAV_DGPS_ENA 0x0b88 -#define NAV_VEL_FILTER_ENA 0x0b8b -#define NAV_ALT_LIMIT_ENA 0x0b8c -#define NAV_VEL_LIMIT_ENA 0x0b8d -#define NAV_EXT_AIDING_ALT_ENA 0x0b8e -#define NAV_FOM_ENA 0x0b8f -#define NAV_HDOP_ENA 0x0b90 -#define NAV_VDOP_ENA 0x0b91 -#define NAV_TENTATIVE_ENA 0x0b96 -#define NAV_PULLFIX_ENA 0x0b97 -#define NAV_2D_FIX_ENA 0x0ba0 -#define NAV_RESERVED_001 0x0ba1 -#define NAV_OUTPUT_LAST_POS_IF_NO_FIX 0x0bb0 -#define NAV_ESTIMATE_VEL_WITHOUT_PLL 0x0bb1 -#define NAV_OUTPUT_LAST_VEL_IF_NO_FIX 0x0bb2 - -/* Position pinning parameters */ -#define NAV_PIN_VEL 0x0b35 -#define NAV_PIN_DRIFT_ERR 0x0b36 -#define NAV_PIN_XYZ_ERR 0x0b37 -#define NAV_PIN_TIMEOUT 0x0b38 -#define NAV_PIN_START_DELAY 0x0b39 -#define NAV_PINNING_ENA 0x0b8a - -/* Interval mode parameters */ -#define NAV_INTMODE_NBR_FIXES 0x0b22 -#define NAV_INTMODE_FIX_INTERVAL 0x0b23 -#define NAV_INTMODE_TRY_FIND_SV 0x0b24 -#define NAV_INTMODE_TRY_GET_FIX 0x0b25 -#define NAV_INTMODE_MAX_STAY_UP 0x0b26 -#define NAV_INTMODE_NUM_IGNORED_FIXES 0x0b27 -#define NAV_INTERVAL_MODE_ENA 0x0ba2 - -/* Kalman navigation parameters */ -#define KLM_MODE 0x0801 -#define KLM_MAX_NUM_STATES 0x0802 -#define KLM_START_FLAGS 0x0803 -#define KLM_OUTPUT_FLAGS 0x0804 -#define KLM_NUM_OBS_LIMIT 0x0805 -#define KLM_MEAS_FLAGS 0x0806 -#define KLM_COV_LIMIT 0x0807 -#define KLM_DOPPLER_NOISE 0x0810 -#define KLM_RANGE_NOISE 0x0811 -#define KLM_DOPPLER_NOISE_LOW 0x0812 -#define KLM_RANGE_NOISE_LOW 0x0813 -#define KLM_NOISE_SNR_LOW 0x0814 -#define KLM_DOPPLER_NOISE_PLL 0x0815 -#define KLM_RANGE_NOISE_PLL 0x0816 -#define KLM_CLOCK_OFFSET_NOISE 0x0820 -#define KLM_CLOCK_DRIFT_NOISE 0x0821 -#define KLM_POS_NOISE 0x0822 -#define KLM_POS_NOISE_VERT 0x0823 -#define KLM_VEL_NOISE 0x0824 -#define KLM_VEL_NOISE_VERT 0x0825 -#define KLM_ACC_NOISE 0x0826 -#define KLM_ACC_NOISE_VERT 0x0827 -#define KLM_ACC_NOISE_PARAM 0x0828 -#define KLM_POS_INIT_UNC 0x0830 -#define KLM_VEL_INIT_UNC 0x0831 -#define KLM_CLOCK_OFFSET_INIT_UNC 0x0832 -#define KLM_CLOCK_DRIFT_INIT_UNC 0x0833 -#define KLM_RESERVED_001 0x0841 -#define KLM_RESERVED_002 0x0842 -#define KLM_RESERVED_003 0x0843 -#define KLM_RESERVED_004 0x0844 -#define KLM_RESERVED_005 0x0845 -#define KLM_RESERVED_006 0x0846 -#define KLM_RESERVED_007 0x0847 -#define KLM_RESERVED_008 0x0848 - -/* Observation parameters */ -#define TRACK_MEAS_INTERVAL 0x0420 -#define TRACK_CHANNELS 0x041d -#define OBS_ELEV_LIMIT 0x0101 -#define OBS_SNR_LIMIT 0x0102 -#define OBS_SNR_RAIM_LIMIT 0x0103 -#define OBS_CROSS_CORR_SNR_DIFF 0x0120 -#define OBS_MAX_SNR 0x0121 -#define OBS_PLL_CROSS_CORR_THR 0x0122 -#define OBS_FLL_CROSS_CORR_THR 0x0123 -#define OBS_FREQ_CROSS_CORR_THR 0x0124 -#define OBS_EPOCH_LIMIT 0x0130 -#define OBS_ELEV_LIMIT_ENA 0x0181 -#define OBS_SNR_LIMIT_ENA 0x0182 -#define OBS_SNR_RAIM_ENA 0x0183 -#define SAT_ORBIT_FIT_UPDATE 0x0203 -#define SAT_FIRST_WEEK 0x0204 -#define SAT_NUM_LEAP 0x0205 -#define SAT_PRED_MAX_LKGAGE 0x0220 -#define SAT_PRED_PHASE_TIMEOUT 0x0221 -#define SAT_PRED_LKG_TIMEOUT 0x0222 -#define SAT_ORBIT_CHECK 0x0281 - -/* Unav Tracking parameters */ -#define TRACK_DLL_ALPHA 0x0401 -#define TRACK_DLL_BETA 0x0402 -#define TRACK_DLL_THR_HIGH 0x0403 -#define TRACK_DLL_THR_LOW 0x0404 -#define TRACK_DLL_POW_NARROW 0x0405 -#define TRACK_DLL_POW_WIDE 0x0406 -#define TRACK_FLL_RESPONSE_TIME 0x0407 -#define TRACK_POW_CALIBRATION 0x0408 -#define TRACK_FLL_THR 0x0409 -#define TRACK_FLL_POW_NARROW 0x040b -#define TRACK_FLL_POW_WIDE 0x040c -#define TRACK_PLL_CTH 0x040d -#define TRACK_PLL_CDTH 0x040e -#define TRACK_PLL_CD2TH 0x040f -#define TRACK_RESERVED_000 0x0410 -#define TRACK_RESERVED_001 0x0411 -#define TRACK_RESERVED_002 0x0412 -#define TRACK_RESERVED_003 0x0413 -#define TRACK_RESERVED_004 0x0414 -#define TRACK_RESERVED_005 0x0415 -#define TRACK_RESERVED_006 0x0416 -#define TRACK_RESERVED_007 0x0417 -#define TRACK_RESERVED_008 0x0418 -#define TRACK_RESERVED_009 0x0419 -#define TRACK_RESERVED_010 0x0425 -#define TRACK_RESERVED_011 0x0426 -#define TRACK_RESERVED_012 0x0427 -#define TRACK_RESERVED_013 0x0428 -#define TRACK_RESERVED_014 0x0429 -#define TRACK_RESERVED_016 0x042a -#define TRACK_RESERVED_017 0x042b -#define TRACK_RESERVED_015 0x0483 -#define SUBF_CHECK_FLAGS 0x0432 - -/* Unav Track task parameters */ -#define TRACK_GROUP_1 0x041a -#define TRACK_GROUP_2 0x041b -#define TRACK_GROUP_2_DELAY 0x041c -#define TRACK_CC_DELAY 0x041e -#define TRACK_CC_THR 0x041f -#define TRACK_PLL_ENA 0x0480 -#define TRACK_NAVAID_ENA 0x0482 -#define TRACK_SHIFT_REG 0x0421 - -/* Agc config parameters */ -#define TRACK_AGC_LO 0x0422 -#define TRACK_AGC_HI 0x0423 -#define TRACK_AGC_MAX_HI 0x0424 -#define TRACK_AGC_ENA 0x0481 - -/* PPS parameters */ -#define PPS_DUTYCYCLE 0x0440 -#define PPS_FREQ 0x0441 -#define PPS_DELAY 0x0442 -#define PPS_SURVEYLEN 0x0443 -#define PPS_MEAS_MS 0x0444 -#define PPS_ENA 0x0490 -#define PPS_SYNC_TRACK 0x0491 -#define PPS_ENA_PRED 0x0492 -#define PPS_INVERT 0x0493 - -/* Frequency plan parameters */ -#define FREQ_XTAL 0x0501 -#define FREQ_MCLK_NOM 0x0502 -#define FREQ_MCLK_DENOM 0x0503 -#define FREQ_RF_NOM 0x0504 -#define FREQ_RF_DENOM 0x0505 -#define FREQ_MIXER_OFFSET 0x0506 -#define FREQ_TME2 0x0507 -#define FREQ_PARAM_ENA 0x0581 - -/* Search parameters */ -#define SEARCH_XTAL_UNC 0x0701 -#define SEARCH_DOPPLER_UNC 0x0702 -#define SEARCH_WIN_PRED_EVEN 0x0703 -#define SEARCH_WIN_PRED_ODD 0x0704 -#define SEARCH_SENS_FULL_R1 0x0705 -#define SEARCH_SENS_FULL_R2 0x0706 -#define SEARCH_SENS_FULL_R3 0x0707 -#define SEARCH_SENS_PRED_EVEN 0x0708 -#define SEARCH_SENS_PRED_ODD 0x0709 -#define SEARCH_PRED_ROUNDS 0x070a -#define SEARCH_BACK_PRNS 0x070b -#define SEARCH_GPS_MASK 0x070c -#define SEARCH_WAAS_MASK 0x070d -#define SEARCH_AUTO_PD_ROUNDS 0x070e -#define SEARCH_FLAGS 0x070f -#define SEARCH_PREC_PRED_TIMEOUT 0x0710 -#define SEARCH_PRED_TIMEOUT 0x0711 -#define SEARCH_FERRY_COND 0x0712 -#define SEARCH_IFFERRY_PRED_COND 0x0713 -#define SEARCH_TUNNEL_IN_SNR 0x0714 -#define SEARCH_TUNNEL_OUT_SNR 0x0715 -#define SEARCH_PRED_ENA 0x0781 -#define SEARCH_BITSYNC_ENA 0x0782 -#define SEARCH_AUTO_PRED_ENA 0x0783 -#define SEARCH_AUTO_PD_ENA 0x0784 -#define SEARCH_SE_PD 0x0785 - -/* Unav Acquisition parameters */ -#define ACQ_SENS_9_COH 0x0901 -#define ACQ_SENS_9_NONCOH 0x0902 -#define ACQ_SENS_9_THR 0x0903 -#define ACQ_SENS_9_BIN 0x0904 -#define ACQ_SENS_10_COH 0x0905 -#define ACQ_SENS_10_NONCOH 0x0906 -#define ACQ_SENS_10_THR 0x0907 -#define ACQ_SENS_10_BIN 0x0908 -#define ACQ_MSG_ENA 0x0981 -#define ACQ_QUICK_SEARCH_ENA 0x0982 -#define SE_NONCOH_SHIFT 0x0940 -#define SE_IR_SHIFT 0x0941 -#define SE_THR 0x0942 -#define SE_INT_ENA 0x09a0 - -/* Logging parameters */ -#define LOG_MODE 0x0d01 -#define LOG_INTERVAL_MIN 0x0d02 -#define LOG_INTERVAL_MAX 0x0d03 -#define LOG_MOVE_MIN 0x0d04 -#define LOG_MOVE_MAX 0x0d05 -#define LOG_VELOCITY_MIN 0x0d06 -#define LOG_VELOCITY_MAX 0x0d07 -#define LOG_MAXITEMS 0x0d08 -#define LOG_STORE_LAT_LONG 0x0d80 -#define LOG_STORE_ALT 0x0d81 -#define LOG_STORE_ALT_FULL 0x0d82 -#define LOG_STORE_GPSTIME 0x0d83 -#define LOG_STORE_GPSTIME_MS 0x0d84 -#define LOG_STORE_DIRECTION 0x0d85 -#define LOG_STORE_VEL 0x0d86 -#define LOG_STORE_VEL_VERT 0x0d87 -#define LOG_STORE_FIXINFO 0x0d88 - -/* SBAS parameters */ -#define WAAS_TIMEOUT_MODE 0x0b60 -#define WAAS_MAX_CHANNELS 0x0b61 -#define WAAS_ENA 0x0bc0 -#define WAAS_MSG_0_ENA 0x0bc1 -#define WAAS_STRICT_ENA 0x0bc2 - -/* Sony Track parameters */ -#define TRACK_DLL_COEFF_GPS 0x0f01 -#define TRACK_DLL_COEFF_DISCR 0x0f02 -#define TRACK_DLL_LIM_GPS 0x0f03 -#define TRACK_DLL4_COEFF_A 0x0f04 -#define TRACK_DLL4_COEFF_B 0x0f05 -#define TRACK_DLL4_COEFF_C 0x0f06 -#define TRACK_DLL4_COEFF_D 0x0f07 -#define TRACK_DLL4_FASTADJ_THRES 0x0f08 -#define TRACK_ELGATE_NARROW 0x0f09 -#define TRACK_COSTASLF_GPS 0x0f0a -#define TRACK_COSTASLF_WAAS 0x0f0b -#define TRACK_LPF_GPS_ACQ 0x0f0c -#define TRACK_LPF_GPS_LOCK 0x0f0d -#define TRACK_LPF_WAAS_LOCK 0x0f0e -#define TRACK_LPF_NOISE 0x0f0f -#define TRACK_SIGDETECT_TH 0x0f10 -#define TRACK_SIGDETECT_TH_HS 0x0f11 -#define TRACK_TIMEOUT_ACQ 0x0f12 -#define TRACK_TIMEOUT_ACQHS 0x0f13 -#define TRACK_TIMEOUT_REACQ 0x0f14 -#define TRACK_HANDOVER_OFFSET 0x0f15 -#define TRACK_CROSSCORR_THRES 0x0f16 -#define TRACK_DLLCTRL_INTERVAL 0x0f17 -#define TRACK_BITEXTRACT 0x0f18 -#define TRACK_RESERVED001 0x0f19 -#define TRACK_RESERVED002 0x0f1a -#define TRACK_WAAS_PRN_BITSTREAM 0x0f1b -#define TRACK_COSTAS_ERROR_TH 0x0f1d -#define TRACK_CARRFLT_OUT_TH 0x0f1e -#define TRACK_CARRFLT_MIDDLE_TH 0x0f1f -#define TRACK_CARRFLT_OUT_DIV 0x0f20 -#define TRACK_CARRFLT_MIDDLE_DIV 0x0f21 -#define TRACK_CARRFLT_INBAND_DIV 0x0f22 -#define TRACK_LATCHTIME_OFFSET 0x0f23 -#define TRACK_DIRECTHANDOVER_OFFSET 0x0f24 -#define TRACK_EN_HS 0x0f80 -#define TRACK_CARR_AID 0x0f81 -#define WAAS_EN_DECODE 0x0f82 -#define TRACK_CARRCHKATLOCK 0x0f83 -#define TRACK_BL_REACQ 0x0f84 - -/* Sony Test parameters */ -#define SONYTEST_DISABLE_PORTS 0x0f85 - -/* Sony Acq parameters */ -#define SACQ_SEARCH_CH_NUM 0x0f30 -#define SACQ_NOISE_COUNT_NUM 0x0f31 -#define SACQ_NOISE_VALID_TIME 0x0f32 -#define SACQ_NOISE_K 0x0f33 -#define SACQ_PEAK_FD 0x0f34 -#define SACQ_PEAK_NFD 0x0f35 -#define SACQ_RESERVE 0x0f36 -#define SACQ_SEARCH_CH_NUM_VALID 0x0f96 - -#endif /* _GPSD_ITALK_H_ */ diff --git a/navcom.c b/navcom.c deleted file mode 100644 index b24b39aa..00000000 --- a/navcom.c +++ /dev/null @@ -1,1276 +0,0 @@ -/* - * Driver for Navcom receivers using propietary NCT messages, a binary protocol. - * - * Vendor website: http://www.navcomtech.com/ - * Technical references: http://www.navcomtech.com/support/docs.cfm - * - * Tested with two SF-2040G models - * - * At this stage, this driver implements the following commands: - * - * 0x20: Data Request (tell the unit which responses you want) - * 0x3f: LED Configuration (controls the front panel LEDs -- for testing) - * 0x1c: Test Support Block (again, blinks the front panel lights) - * - * and it understands the following responses: - * - * 0x06: Acknowledgement (without error) - * 0x15: Negative Acknowledge - * 0x86: Channel Status - * 0xae: Identification Block - * 0xb0: Raw Meas. Data Block - * 0xb1: PVT Block - * 0xb5: Pseudorange Noise Statistics - * 0xd3: LBM DSP Status Block - * 0xef: Clock Drift and Offset - * - * FIXME - I'm not too sure of the way I have computed the vertical positional error - * I have used FOM as a scaling factor for VDOP, thusly VRMS = FOM/HDOP*VDOP - * - * By Diego Berge. Contact via web form at http://www.navlost.eu/contact - * - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "gpsd_config.h" -#include "gpsd.h" - -#if defined(NAVCOM_ENABLE) && defined(BINARY_ENABLE) -#include "bits.h" - -/* Have data which is 24 bits long */ -#define getlesl24(buf,off) (int32_t)(((u_int32_t)getub((buf), (off)+2)<<24 | (u_int32_t)getub((buf), (off)+1)<<16 | (u_int32_t)getub((buf), (off))<<8)>>8) -#define getleul24(buf,off) (u_int32_t)(((u_int32_t)getub((buf), (off)+2)<<24 | (u_int32_t)getub((buf), (off)+1)<<16 | (u_int32_t)getub((buf), (off))<<8)>>8) - -/* And just to be difficult, Navcom is little endian but the GPS data stream - is big endian. Some messages contain raw GPS data */ -#define getlesw_be(buf, off) (int16_t)((((u_int16_t)getub(buf, (off)) << 8) \ - | (u_int16_t)getub(buf, (off)+1))) -#define getleuw_be(buf, off) (u_int16_t)((((u_int16_t)getub(buf, (off)) << 8) \ - | (u_int16_t)getub(buf, (off)+1))) -#define getlesl_be(buf, off) (int32_t)((((u_int16_t)getleuw_be(buf, (off)) << 16) \ - | getleuw_be(buf, (off)+2))) -#define getleul_be(buf, off) (u_int32_t)((((u_int16_t)getleuw_be(buf, (off)) << 16) \ - | getleuw_be(buf, (off)+2))) -#define getlesL_be(buf, off) (int64_t)((((u_int64_t)getleul_be(buf, (off)) << 32) \ - | getleul_be(buf, (off)+4))) -#define getleuL_be(buf, off) (u_int64_t)((((u_int64_t)getleul_be(buf, (off)) << 32) \ - | getleul_be(buf, (off)+4))) -#define getlesl24_be(buf,off) (int32_t)(((u_int32_t)getub((buf), (off))<<24 \ - | (u_int32_t)getub((buf), (off)+1)<<16 \ - | (u_int32_t)getub((buf), (off)+2)<<8)>>8) - -#define NAVCOM_CHANNELS 12 - -static u_int8_t checksum(unsigned char *buf, size_t len) -{ - size_t n; - u_int8_t csum = (u_int8_t)0x00; - for(n = 0; n < len; n++) - csum ^= buf[n]; - return csum; -} - -static ssize_t navcom_control_send(struct gps_device_t *session, - char *buf, size_t len) -{ - /*@ +ignoresigns -mayaliasunique @*/ - putbyte(session->msgbuf, 0, 0x02); - putbyte(session->msgbuf, 1, 0x99); - putbyte(session->msgbuf, 2, 0x66); - putbyte(session->msgbuf, 3, buf[0]); /* Cmd ID */ - putleword(session->msgbuf, 4, len+4); /* Length */ - memcpy(session->msgbuf, buf+6, len-1); - putbyte(session->msgbuf, 6 + len, checksum((unsigned char *)session->msgbuf+3, len+5)); - putbyte(session->msgbuf, 7 + len, 0x03); - session->msgbuflen = len+9; - /*@ -ignoresigns +mayaliasunique @*/ - gpsd_report(LOG_RAW, "Navcom: control dump: %s\n", - gpsd_hexdump_wrapper(session->msgbuf, session->msgbuflen, LOG_RAW)); - return gpsd_write(session, session->msgbuf, session->msgbuflen); -} - -static bool navcom_send_cmd(struct gps_device_t *session, unsigned char *cmd, size_t len) -{ - gpsd_report(LOG_RAW, "Navcom: command dump: %s\n", - gpsd_hexdump_wrapper(cmd, len, LOG_RAW)); - return (gpsd_write(session, cmd, len) == (ssize_t)len); -} - -/* Data Request */ -static void navcom_cmd_0x20(struct gps_device_t *session, u_int8_t block_id, u_int16_t rate) -{ - unsigned char msg[18]; - putbyte(msg, 0, 0x02); - putbyte(msg, 1, 0x99); - putbyte(msg, 2, 0x66); - putbyte(msg, 3, 0x20); /* Cmd ID */ - putleword(msg, 4, 0x000e); /* Length */ - putbyte(msg, 6, 0x00); /* Action */ - putbyte(msg, 7, 0x01); /* Count of blocks */ - putbyte(msg, 8, block_id); /* Data Block ID */ - putbyte(msg, 9, 0x02); /* Logical Ports */ - putleword(msg, 10, rate); /* Data rate */ - putbyte(msg, 12, 0x71); - putbyte(msg, 13, 0x00); - putleword(msg, 14, 0x0000); - putbyte(msg, 16, checksum(msg+3, 13)); - putbyte(msg, 17, 0x03); - (void)navcom_send_cmd(session, msg, 18); - gpsd_report(LOG_PROG, - "Navcom: sent command 0x20 (Data Request) " - "- data block id = %02x at rate %02x\n", - block_id, rate); -} - -/*@ unused @*/ -/* Changes the LED settings in the receiver */ -static void UNUSED navcom_cmd_0x3f(struct gps_device_t *session) -{ - unsigned char msg[12]; - putbyte(msg, 0, 0x02); - putbyte(msg, 1, 0x99); - putbyte(msg, 2, 0x66); - putbyte(msg, 3, 0x3f); /* Cmd ID */ - putleword(msg, 4, 0x0008); - putbyte(msg, 6, 0x01); /* Action */ - putbyte(msg, 7, 0x00); /* Reserved */ - putbyte(msg, 8, 0x02); /* Link LED setting */ - putbyte(msg, 9, 0x0a); /* Battery LED setting */ - putbyte(msg, 10, checksum(msg+3, 7)); - putbyte(msg, 11, 0x03); - (void)navcom_send_cmd(session, msg, 12); - gpsd_report(LOG_PROG, - "Navcom: sent command 0x3f (LED Configuration Block)\n"); -} - -/* Test Support Block - Blinks the LEDs */ -static void navcom_cmd_0x1c(struct gps_device_t *session, u_int8_t mode, u_int8_t length) -{ - unsigned char msg[12]; - putbyte(msg, 0, 0x02); - putbyte(msg, 1, 0x99); - putbyte(msg, 2, 0x66); - putbyte(msg, 3, 0x1c); /* Cmd ID */ - putleword(msg, 4, 0x0008); - putbyte(msg, 6, 0x04); /* Use ACK/NAK */ - putbyte(msg, 7, mode); /* 0x01 or 0x02 */ - putbyte(msg, 8, length); /* Only if mode == 0x01 */ - putbyte(msg, 9, 0x00); - putbyte(msg, 10, checksum(msg+3, 7)); - putbyte(msg, 11, 0x03); - (void)navcom_send_cmd(session, msg, 12); - gpsd_report(LOG_PROG, - "Navcom: sent command 0x1c (Test Support Block)\n"); - gpsd_report(LOG_IO, - "Navcom: command 0x1c mode = %02x, length = %u\n", - mode, length); -} - -/* Serial Port Configuration */ -static void navcom_cmd_0x11(struct gps_device_t *session, u_int8_t port_selection) -{ - /* NOTE - We only allow changing one port at a time, - although the message supports doing both at once. */ - unsigned char msg[12]; - putbyte(msg, 0, 0x02); - putbyte(msg, 1, 0x99); - putbyte(msg, 2, 0x66); - putbyte(msg, 3, 0x11); /* Cmd ID */ - putleword(msg, 4, 0x0008); /* Length */ - putbyte(msg, 6, 0x04); /* Action - Use ACK/NAK) */ - putbyte(msg, 7, port_selection); - putbyte(msg, 8, 0x00); /* Reserved */ - putbyte(msg, 9, 0x00); /* Reserved */ - putbyte(msg, 10, checksum(msg+3, 7)); - putbyte(msg, 11, 0x03); - (void)navcom_send_cmd(session, msg, 12); - gpsd_report(LOG_PROG, - "Navcom: sent command 0x11 (Serial Port Configuration)\n"); - gpsd_report(LOG_IO, - "Navcom: serial port selection: 0x%02x\n", port_selection); -} - -static void navcom_probe_subtype(struct gps_device_t *session, unsigned int seq) -{ - /* Request the following messages: */ - if (seq==0) { - /*@ +charint @*/ - navcom_cmd_0x1c(session, 0x01, 5); /* Blink LEDs on receiver */ - navcom_cmd_0x20(session, 0xae, 0x1770); /* Identification Block - send every 10 min*/ - navcom_cmd_0x20(session, 0xb1, 0x4000); /* PVT Block */ - navcom_cmd_0x20(session, 0xb5, 0x00c8); /* Pseudorange Noise Statistics - send every 20s */ - navcom_cmd_0x20(session, 0xb0, 0x4000); /* Raw Meas Data Block */ - navcom_cmd_0x20(session, 0x81, 0x0000); /* Packed Ephemeris Data - send once */ - navcom_cmd_0x20(session, 0x81, 0x4000); /* Packed Ephemeris Data */ - navcom_cmd_0x20(session, 0x86, 0x4000); /* Channel Status */ - navcom_cmd_0x20(session, 0x83, 0x4000); /* Ionosphere and UTC Data */ - navcom_cmd_0x20(session, 0xef, 0x0bb8); /* Clock Drift - send every 5 min */ - /*@ -charint @*/ - } -} - -static void navcom_ping(struct gps_device_t *session) -{ -/* NOTE - This allows us to know into which of the unit's various - serial ports we are connected. - Its value gets updated every time we receive a 0x06 (Ack) - message. Note that if commands are being fed into the - unit from more than one port (which is entirely possible - although not necessarily a bright idea), there is a good - chance that we might misidentify our port */ - /*@ -type @*/ - session->driver.navcom.physical_port = 0xFF; - - navcom_cmd_0x1c(session, 0x02, 0); /* Test Support Block */ - navcom_cmd_0x20(session, 0xae, 0x0000); /* Identification Block */ - navcom_cmd_0x20(session, 0x86, 0x000a); /* Channel Status */ - /*@ +type @*/ -} - -static bool navcom_speed(struct gps_device_t *session, - unsigned int speed, char parity, int stopbits) -{ -#ifdef ALLOW_RECONFIGURE - /* parity and stopbit switching aren't implemented */ - if (parity!=(char)session->gpsdata.parity || stopbits!=(int)session->gpsdata.parity) { - return false; - } else { - u_int8_t port_selection; - u_int8_t baud; - if (session->driver.navcom.physical_port == (unsigned char)0xFF) { - /* We still don't know which port we're connected to */ - return false; - } - /*@ +charint @*/ - switch (speed) { - /* NOTE - The spec says that certain baud combinations - on ports A and B are not allowed, those are - 1200/115200, 2400/57600, and 2400/115200. - To try and minimise the possibility of those - occurring, we do not allow baud rates below - 4800. We could also disallow 57600 and 115200 - to totally prevent this, but I do not consider - that reasonable. Finding which baud speed the - other port is set at would also be too much - trouble, so we do not do it. */ - case 4800: - baud = 0x04; - break; - case 9600: - baud = 0x06; - break; - case 19200: - baud = 0x08; - break; - case 38400: - baud = 0x0a; - break; - case 57600: - baud = 0x0c; - break; - case 115200: - baud = 0x0e; - break; - default: - /* Unsupported speed */ - return false; - } - /*@ -charint @*/ - - /* Proceed to construct our message */ - port_selection = session->driver.navcom.physical_port | baud; - - /* Send it off */ - navcom_cmd_0x11(session, port_selection); - - /* And cheekily return true, even though we have - no way to know if the speed change succeeded - until and if we receive an ACK (message 0x06), - which will be at the new baud speed if the - command was successful. Bottom line, the client - should requery gpsd to see if the new speed is - different than the old one */ - return true; - } -#else - return false; -#endif /* ALLOW_RECONFIGURE */ -} - -/* Ionosphere and UTC Data */ -static gps_mask_t handle_0x83(struct gps_device_t *session) -{ - /* NOTE - At the present moment this is only being used - for determining the GPS-UTC time difference, - for which the iono data is not needed as far - as we are concerned. However, I am still - reporting it (if debuglevel >= LOG_IO) as a - matter of interest */ -/* 2^-30 */ -#define SF_A0 (0.000000000931322574615478515625) -/* 2^-50 */ -#define SF_A1 (0.000000000000000888178419700125) -/* 2^12 */ -#define SF_TOT (4096) -/* 2^-30 */ -#define SF_ALPHA0 (0.000000000931322574615478515625) -/* 2^-27 */ -#define SF_ALPHA1 (0.000000007450580596923828125) -/* 2^-24 */ -#define SF_ALPHA2 (0.000000059604644775390625) -/* 2^-24 */ -#define SF_ALPHA3 (0.000000059604644775390625) -/* 2^11 */ -#define SF_BETA0 (2048) -/* 2^14 */ -#define SF_BETA1 (16384) -/* 2^16 */ -#define SF_BETA2 (65536) -/* 2^16 */ -#define SF_BETA3 (65536) - unsigned char *buf = session->packet.outbuffer + 3; - u_int16_t week = getleuw(buf, 3); - u_int32_t tow = getleul(buf, 5); - int8_t alpha0 = getsb(buf, 9); - int8_t alpha1 = getsb(buf, 10); - int8_t alpha2 = getsb(buf, 11); - int8_t alpha3 = getsb(buf, 12); - int8_t beta0 = getsb(buf, 13); - int8_t beta1 = getsb(buf, 14); - int8_t beta2 = getsb(buf, 15); - int8_t beta3 = getsb(buf, 16); - int32_t a1 = getlesl(buf, 17); - int32_t a0 = getlesl(buf, 21); - u_int8_t tot = getub(buf, 25); - u_int8_t wnt = getub(buf, 26); - int8_t dtls = getsb(buf, 27); - u_int8_t wnlsf = getub(buf, 28); - u_int8_t dn = getub(buf, 29); - int8_t dtlsf = getsb(buf, 30); - - /*@ +charint +relaxtypes @*/ - /* Ref.: ICD-GPS-200C 20.3.3.5.2.4 */ - if ((week%256)*604800+tow/1000.0 < wnlsf*604800+dn*86400) { - /* Effectivity time is in the future, use dtls */ - session->context->leap_seconds = (int)dtls; - } else { - /* Effectivity time is not in the future, use dtlsf */ - session->context->leap_seconds = (int)dtlsf; - } - /*@ -relaxtypes -charint @*/ - - session->gpsdata.sentence_time = gpstime_to_unix((int)week, tow/1000.0) - - session->context->leap_seconds; - - gpsd_report(LOG_PROG, - "Navcom: received packet type 0x83 (Ionosphere and UTC Data)\n"); - gpsd_report(LOG_IO, - "Navcom: Scaled parameters follow:\n"); - gpsd_report(LOG_IO, - "Navcom: GPS Week: %u, GPS Time of Week: %u (GPS Time: %f)\n", - week, tow, week*604800+tow/1000.0); - gpsd_report(LOG_IO, - "Navcom: a0: %12.4E, a1: %12.4E, a2: %12.4E, a3: %12.4E, " - "b0: %12.4E, b1: %12.4E, b2: %12.4E, b3: %12.4E\n", - (double)alpha0*SF_ALPHA0, (double)alpha1*SF_ALPHA1, - (double)alpha2*SF_ALPHA2, (double)alpha3*SF_ALPHA3, - (double)beta0*SF_BETA0, (double)beta1*SF_BETA1, - (double)beta2*SF_BETA2, (double)beta3*SF_BETA3); - gpsd_report(LOG_IO, - "Navcom: A0: %19.12E, A1: %19.12E\n", (double)a0*SF_A0, (double)a1*SF_A1); - gpsd_report(LOG_IO, - "Navcom: UTC Ref. Time: %lu, UTC Ref. Week: %u, dTls: %d\n", - (unsigned long)tot*SF_TOT, wnt, dtls); - gpsd_report(LOG_IO, - "Navcom: Week of leap seconds: %u, Day number of leap seconds: %u, dTlsf: %d\n", - wnlsf, dn, dtlsf); - - return 0; /* No flag for update of leap seconds (Not part of a fix) */ - -#undef SF_A0 -#undef SF_A1 -#undef SF_TOT -#undef SF_ALPHA0 -#undef SF_ALPHA1 -#undef SF_ALPHA2 -#undef SF_ALPHA3 -#undef SF_BETA0 -#undef SF_BETA1 -#undef SF_BETA2 -#undef SF_BETA3 -} - -/* Acknowledgement (without error) */ -static gps_mask_t handle_0x06(struct gps_device_t *session) -{ - unsigned char *buf = session->packet.outbuffer + 3; - u_int8_t cmd_id = getub(buf, 3); - u_int8_t port = getub(buf, 4); - session->driver.navcom.physical_port = port; /* This tells us which serial port was used last */ - gpsd_report(LOG_PROG, - "Navcom: received packet type 0x06 (Acknowledgement (without error))\n"); - /*@ -type @*/ - gpsd_report(LOG_IO, - "Navcom: acknowledged command id 0x%02x on port %c\n", - cmd_id, (port==0?'A':(port==1?'B':'?'))); - /*@ +type @*/ - return 0; /* Nothing updated */ -} - -/* Negative Acknowledge */ -static gps_mask_t handle_0x15(struct gps_device_t *session) -{ - size_t n; - unsigned char *buf = session->packet.outbuffer + 3; - size_t msg_len = (size_t)getleuw(buf, 1); - /*@ -type @*/ - u_int8_t port, cmd_id = getub(buf, 3); - gpsd_report(LOG_PROG, - "Navcom: received packet type 0x15 (Negative Acknowledge)\n"); - for (n=4; n<(msg_len-2); n+=2) { - u_int8_t err_id = getub(buf, n); - u_int8_t err_desc = getub(buf, n+1); - gpsd_report(LOG_IO, - "Navcom: error id = 0x%02x, error description = 0x%02x\n", - err_id, err_desc); - } - port = getub(buf, n); - gpsd_report(LOG_IO, - "Navcom: negative acknowledge was for command id 0x%02x on port %c\n", - cmd_id, (port==0?'A':(port==1?'B':'?'))); - /*@ -type @*/ - return 0; /* Nothing updated */ -} - -/* PVT Block */ -static gps_mask_t handle_0xb1(struct gps_device_t *session) -{ - unsigned int n; - unsigned char *buf = session->packet.outbuffer + 3; - uint16_t week; - uint32_t tow; - uint32_t sats_used; - int32_t lat, lon; - /* Resolution of lat/lon values (2^-11) */ -#define LL_RES (0.00048828125) - uint8_t lat_fraction, lon_fraction; - /* Resolution of lat/lon fractions (2^-15) */ -#define LL_FRAC_RES (0.000030517578125) - uint8_t nav_mode; - int32_t ellips_height, altitude; - /* Resolution of height and altitude values (2.0^-10) */ -#define EL_RES (0.0009765625) - double vel_north, vel_east, vel_up; - /* Resolution of velocity values (2.0^-10) */ -#define VEL_RES (0.0009765625) - double track; - uint8_t fom, gdop, pdop, hdop, vdop, tdop, tfom; - /* This value means "undefined" */ -#define DOP_UNDEFINED (255) - - int16_t ant_height_adj; - int32_t set_delta_up; - /* Resolution of delta north, east, and up, - and ant. height adjustment values (1mm) */ -#define D_RES (0.001) - -#ifdef __UNUSED__ - /* Other values provided by the PVT block which we - may want to provide in the future. At the present - moment, the gpsd protocol does not have a mechanism - to make this available to the user */ - uint8_t dgps_conf; - uint16_t max_dgps_age; - uint8_t ext_nav_mode; - int32_t set_delta_north, set_delta_east; - uint8_t nav_failure_code; -#endif /* __UNUSED__ */ - - /* Timestamp */ - week = (uint16_t)getleuw(buf, 3); - tow = (uint32_t)getleul(buf, 5); - session->gpsdata.fix.time = session->gpsdata.sentence_time = gpstime_to_unix((int)week, tow/1000.0) - session->context->leap_seconds; - - /* Satellites used */ - sats_used = (uint32_t)getleul(buf, 9); - session->gpsdata.satellites_used = 0; - for (n = 0; n < 31; n++) { - if ((sats_used & (0x01 << n)) != 0) - session->gpsdata.used[session->gpsdata.satellites_used++] = (int)(n+1); - } - - /* Get latitude, longitude */ - lat = getlesl(buf, 13); - lon = getlesl(buf, 17); - lat_fraction = (uint8_t)(getub(buf, 21) >> 4); - lon_fraction = (uint8_t)(getub(buf, 21) & 0x0f); - - session->gpsdata.fix.latitude = (double)(lat * LL_RES + lat_fraction * LL_FRAC_RES ) / 3600; - session->gpsdata.fix.longitude = (double)(lon * LL_RES + lon_fraction * LL_FRAC_RES ) / 3600; - - /* Nav mode */ - nav_mode = (uint8_t)getub(buf, 22); - if (-nav_mode & 0x80) { - session->gpsdata.status = STATUS_NO_FIX; - session->gpsdata.fix.mode = MODE_NO_FIX; - } else { - session->gpsdata.fix.mode = (nav_mode & 0x40 ? MODE_3D : MODE_2D); - session->gpsdata.status = (nav_mode & 0x03 ? STATUS_DGPS_FIX : STATUS_FIX); - } - - /* Height Data */ - ellips_height = getlesl(buf, 23); - altitude = getlesl(buf, 27); - - ant_height_adj = getlesw(buf, 51); - set_delta_up = getlesl(buf, 79); - - session->gpsdata.fix.altitude = (double)(altitude * EL_RES) - + (ant_height_adj * D_RES) + (set_delta_up * D_RES); - session->gpsdata.separation = (double)(ellips_height - altitude)*EL_RES - + (ant_height_adj * D_RES) + (set_delta_up * D_RES); - - /* Speed Data */ - vel_north = (double)getlesl24(buf, 31); - vel_east = (double)getlesl24(buf, 34); - /* vel_up = getlesl24(buf, 37); */ - vel_up = (double)getlesl24(buf, 37); - - track = atan2(vel_east, vel_north); - if (track < 0) - track += 2 * GPS_PI; - session->gpsdata.fix.track = track * RAD_2_DEG; - /*@ -evalorder @*/ - session->gpsdata.fix.speed = sqrt(pow(vel_east,2) + pow(vel_north,2)) * VEL_RES; - /*@ +evalorder @*/ - session->gpsdata.fix.climb = vel_up * VEL_RES; - - /* Quality indicators */ - /*@ -type @*/ - fom = getub(buf, 40); - gdop = getub(buf, 41); - pdop = getub(buf, 42); - hdop = getub(buf, 43); - vdop = getub(buf, 44); - tdop = getub(buf, 45); - tfom = getub(buf, 46); - /*@ +type @*/ - - /* splint apparently gets confused about C promotion rules. */ - /* "Assignment of arbitrary unsigned integral type to double" on these */ -#ifndef S_SPLINT_S - session->gpsdata.fix.eph = fom/100.0*1.96/*Two sigma*/; - /* FIXME - Which units is tfom in (spec doesn't say) and - which units does gpsd require? (docs don't say) */ - session->gpsdata.fix.ept = tfom*1.96/*Two sigma*/; - /* FIXME This cannot possibly be right */ - /* I cannot find where to get VRMS from in the Navcom output, though, - and this value seems to agree with the output from other software */ - session->gpsdata.fix.epv = (double)fom/(double)hdop*(double)vdop/100.0*1.96/*Two sigma*/; - - if (gdop == DOP_UNDEFINED) - session->gpsdata.gdop = NAN; - else - session->gpsdata.gdop = gdop/10.0; - if (pdop == DOP_UNDEFINED) - session->gpsdata.pdop = NAN; - else - session->gpsdata.pdop = pdop/10.0; - if (hdop == DOP_UNDEFINED) - session->gpsdata.hdop = NAN; - else - session->gpsdata.hdop = hdop/10.0; - if (vdop == DOP_UNDEFINED) - session->gpsdata.vdop = NAN; - else - session->gpsdata.vdop = vdop/10.0; - if (tdop == DOP_UNDEFINED) - session->gpsdata.tdop = NAN; - else - session->gpsdata.tdop = tdop/10.0; -#endif /* S_SPLINT_S */ - - gpsd_report(LOG_PROG, "Navcom: received packet type 0xb1 (PVT Report)\n"); - gpsd_report(LOG_IO, "Navcom: navigation mode %s (0x%02x) - %s - %s\n", - (-nav_mode&0x80?"invalid":"valid"), nav_mode, - (nav_mode&0x40?"3D":"2D"), (nav_mode&0x03?"DGPS":"GPS")); - gpsd_report(LOG_IO, "Navcom: latitude = %f, longitude = %f, altitude = %f, geoid = %f\n", - session->gpsdata.fix.latitude, session->gpsdata.fix.longitude, - session->gpsdata.fix.altitude, session->gpsdata.separation); - gpsd_report(LOG_IO, - "Navcom: velocities: north = %f, east = %f, up = %f (track = %f, speed = %f)\n", - vel_north*VEL_RES, vel_east*VEL_RES, vel_up*VEL_RES, - session->gpsdata.fix.track, session->gpsdata.fix.speed); - gpsd_report(LOG_IO, - "Navcom: hrms = %f, vrms = %f, gdop = %f, pdop = %f, " - "hdop = %f, vdop = %f, tdop = %f\n", - session->gpsdata.fix.eph, session->gpsdata.fix.epv, - session->gpsdata.gdop, session->gpsdata.pdop, - session->gpsdata.hdop, session->gpsdata.vdop, - session->gpsdata.tdop); -#undef D_RES -#undef LL_RES -#undef LL_FRAC_RES -#undef EL_RES -#undef VEL_RES -#undef DOP_UNDEFINED - - return LATLON_SET | ALTITUDE_SET | CLIMB_SET | SPEED_SET | TRACK_SET - | TIME_SET | STATUS_SET | MODE_SET | USED_SET | HERR_SET | VERR_SET - | TIMERR_SET | DOP_SET | CYCLE_START_SET; -} - -/* Packed Ephemeris Data */ -static gps_mask_t handle_0x81(struct gps_device_t *session) -{ - /* Scale factors for everything */ - /* 2^-31 */ -#define SF_TGD (.000000000465661287307739257812) - /* 2^4 */ -#define SF_TOC (16) - /* 2^-55 */ -#define SF_AF2 (.000000000000000027755575615628) - /* 2^-43 */ -#define SF_AF1 (.000000000000113686837721616029) - /* 2^-31 */ -#define SF_AF0 (.000000000465661287307739257812) - /* 2^-5 */ -#define SF_CRS (.031250000000000000000000000000) - /* 2^-43 */ -#define SF_DELTA_N (.000000000000113686837721616029) - /* 2^-31 */ -#define SF_M0 (.000000000465661287307739257812) - /* 2^-29 */ -#define SF_CUC (.000000001862645149230957031250) - /* 2^-33 */ -#define SF_E (.000000000116415321826934814453) - /* 2^-29 */ -#define SF_CUS (.000000001862645149230957031250) - /* 2^-19 */ -#define SF_SQRT_A (.000001907348632812500000000000) - /* 2^4 */ -#define SF_TOE (16) - /* 2^-29 */ -#define SF_CIC (.000000001862645149230957031250) - /* 2^-31 */ -#define SF_OMEGA0 (.000000000465661287307739257812) - /* 2^-29 */ -#define SF_CIS (.000000001862645149230957031250) - /* 2^-31 */ -#define SF_I0 (.000000000465661287307739257812) - /* 2^-5 */ -#define SF_CRC (.031250000000000000000000000000) - /* 2^-31 */ -#define SF_OMEGA (.000000000465661287307739257812) - /* 2^-43 */ -#define SF_OMEGADOT (.000000000000113686837721616029) - /* 2^-43 */ -#define SF_IDOT (.000000000000113686837721616029) - - unsigned char *buf = session->packet.outbuffer + 3; - u_int8_t prn = getub(buf, 3); - u_int16_t week = getleuw(buf, 4); - u_int32_t tow = getleul(buf, 6); - u_int16_t iodc = getleuw(buf, 10); - /* And now the fun starts... everything that follows is - raw GPS data minus parity */ - /* Subframe 1, words 3 to 10 minus parity */ - u_int16_t wn = (getleuw_be(buf, 12)&0xffc0)>>6; - u_int8_t cl2 = (getub(buf, 13)&0x30)>>4; - u_int8_t ura = getub(buf, 13)&0x0f; - u_int8_t svh = (getub(buf, 14)&0xfc)>>2; - /* We already have IODC from earlier in the message, so - we do not decode again */ -/* u_int16_t iodc = (getub(buf, 14)&0x03)<<8;*/ - u_int8_t l2pd = (getub(buf, 15)&0x80)>>7; - int8_t tgd = getsb(buf, 26); -/* iodc |= getub(buf, 27);*/ - u_int16_t toc = getleuw_be(buf, 28); - int8_t af2 = getsb(buf, 30); - int16_t af1 = getlesw_be(buf, 31); - /*@ -shiftimplementation @*/ - int32_t af0 = getlesl24_be(buf, 33)>>2; - /*@ +shiftimplementation @*/ - /* Subframe 2, words 3 to 10 minus parity */ - u_int8_t iode = getub(buf, 36); - int16_t crs = getlesw_be(buf, 37); - int16_t delta_n = getlesw_be(buf, 39); - int32_t m0 = getlesl_be(buf, 41); - int16_t cuc = getlesw_be(buf, 45); - u_int32_t e = getleul_be(buf, 47); - int16_t cus = getlesw_be(buf, 51); - u_int32_t sqrt_a = getleul_be(buf, 53); - u_int16_t toe = getleuw_be(buf, 57); - /* NOTE - Fit interval & AODO not collected */ - /* Subframe 3, words 3 to 10 minus parity */ - int16_t cic = getlesw_be(buf, 60); - int32_t Omega0 = getlesl_be(buf, 62); - int16_t cis = getlesw_be(buf, 66); - int32_t i0 = getlesl_be(buf, 68); - int16_t crc = getlesw_be(buf, 72); - int32_t omega = getlesl_be(buf, 74); - int32_t Omegadot = getlesl24_be(buf, 78); - /*@ -predboolothers @*/ - /* Question: What is the proper way of shifting a signed int 2 bits to - * the right, preserving sign? Answer: integer division by 4. */ - int16_t idot = (int16_t)(((getlesw_be(buf, 82)&0xfffc)/4)|(getub(buf, 82)&80?0xc000:0x0000)); - /*@ +predboolothers @*/ - char time_str[24]; - (void)unix_to_iso8601(gpstime_to_unix((int)wn, (double)(toc*SF_TOC)), time_str, sizeof(time_str)); - - gpsd_report(LOG_PROG, - "Navcom: received packet type 0x81 (Packed Ephemeris Data)\n"); - gpsd_report(LOG_IO, - "Navcom: PRN: %u, Epoch: %u (%s), SV clock bias/drift/drift rate: %#19.12E/%#19.12E/%#19.12E\n", - prn, toc*SF_TOC, time_str, ((double)af0)*SF_AF0, ((double)af1)*SF_AF1, ((double)af2)*SF_AF2); - gpsd_report(LOG_IO, - "Navcom: IODE (!AODE): %u Crs: %19.12e, Delta n: %19.12e, M0: %19.12e\n", - iode, (double)crs*SF_CRS, (double)delta_n*SF_DELTA_N*GPS_PI, (double)m0*SF_M0*GPS_PI); - gpsd_report(LOG_IO, - "Navcom: Cuc: %19.12e, Eccentricity: %19.12e, Cus: %19.12e, A^1/2: %19.12e\n", - (double)cuc*SF_CUC, (double)e*SF_E, (double)cus*SF_CUS, (double)sqrt_a*SF_SQRT_A); - gpsd_report(LOG_IO, - "Navcom: TOE: %u, Cic: %19.12e, Omega %19.12e, Cis: %19.12e\n", - toe*SF_TOE, (double)cic*SF_CIC, (double)Omega0*SF_OMEGA0*GPS_PI, - (double)cis*SF_CIS); - gpsd_report(LOG_IO, - "Navcom: i0: %19.12e, Crc: %19.12e, omega: %19.12e, Omega dot: %19.12e\n", - (double)i0*SF_I0*GPS_PI, (double)crc*SF_CRC, (double)omega*SF_OMEGA*GPS_PI, - (double)Omegadot*SF_OMEGADOT*GPS_PI); - gpsd_report(LOG_IO, - "Navcom: IDOT: %19.12e, Codes on L2: 0x%x, GPS Week: %u, L2 P data flag: %x\n", - (double)idot*SF_IDOT*GPS_PI, cl2, week-(week%1024)+wn, l2pd); - gpsd_report(LOG_IO, - "Navcom: SV accuracy: 0x%x, SV health: 0x%x, TGD: %f, IODC (!AODC): %u\n", - ura, svh, (double)tgd*SF_TGD, iodc); - gpsd_report(LOG_IO, - "Navcom: Transmission time: %u\n", - tow); - -#undef SF_TGD -#undef SF_TOC -#undef SF_AF2 -#undef SF_AF1 -#undef SF_AF0 -#undef SF_CRS -#undef SF_DELTA_N -#undef SF_M0 -#undef SF_CUC -#undef SF_E -#undef SF_CUS -#undef SF_SQRT_A -#undef SF_TOE -#undef SF_CIC -#undef SF_OMEGA0 -#undef SF_CIS -#undef SF_I0 -#undef SF_CRC -#undef SF_OMEGA -#undef SF_OMEGADOT -#undef SF_IDOT - - return 0; -} - -/* Channel Status */ -static gps_mask_t handle_0x86(struct gps_device_t *session) -{ - size_t n, i; - u_int8_t prn, tracking_status, ele, ca_snr, p2_snr, log_channel, hw_channel, s; - u_int16_t azm, dgps_age; - unsigned char *buf = session->packet.outbuffer + 3; - size_t msg_len = (size_t)getleuw(buf, 1); - u_int16_t week = getleuw(buf, 3); - u_int32_t tow = getleul(buf, 5); - u_int8_t eng_status = getub(buf, 9); - u_int16_t sol_status = getleuw(buf, 10); - u_int8_t sats_visible = getub(buf, 12); - u_int8_t sats_tracked = getub(buf, 13); - u_int8_t sats_used = getub(buf, 14); - u_int8_t pdop = getub(buf, 15); - - /* Timestamp and PDOP */ - session->gpsdata.sentence_time = gpstime_to_unix((int)week, tow/1000.0) - - session->context->leap_seconds; - session->gpsdata.pdop = (int)pdop / 10.0; - - /* Satellite count */ - session->gpsdata.satellites = (int)sats_visible; - session->gpsdata.satellites_used = (int)sats_used; - - /* Fix mode */ - switch(sol_status & 0x05) - { - case 0x05: - session->gpsdata.status = STATUS_DGPS_FIX; - break; - case 0x01: - session->gpsdata.status = STATUS_FIX; - break; - default: - session->gpsdata.status = STATUS_NO_FIX; - } - - /*@ -predboolothers @*/ - gpsd_report(LOG_PROG, - "Navcom: received packet type 0x86 (Channel Status) " - "- satellites: visible = %u, tracked = %u, used = %u\n", - sats_visible, sats_tracked, sats_used); - gpsd_report(LOG_IO, - "Navcom: engine status = 0x%x, almanac = %s, time = 0x%x, pos = 0x%x\n", - eng_status&0x07, (eng_status&0x08?"valid":"invalid"), - eng_status&0x30>>4, eng_status&0xc0>>6); - /*@ +predboolothers @*/ - - /* Satellite details */ - i = 0; - for(n = 17; n < msg_len; n += 14) { - if(i >= MAXCHANNELS) { - gpsd_report(LOG_ERROR, - "Navcom: packet type 0x86: too many satellites!\n"); - gpsd_zero_satellites(&session->gpsdata); - return ERROR_SET; - } - prn = getub(buf, n); - tracking_status = getub(buf, n+1); - log_channel = getub(buf, n+2); - ele = getub(buf, n+5); - azm = getleuw(buf, n+6); - ca_snr = getub(buf, n+8); - p2_snr = getub(buf, n+10); - dgps_age = getleuw(buf, n+11); - hw_channel = getub(buf, n+13); - s = (unsigned char)0; - /*@ -predboolothers +charint @*/ - /* NOTE - In theory, I think one would check for hw channel number to - see if one is dealing with a GPS or other satellite, but the - channel numbers reported bear no resemblance to what the spec - says should be. So I check for the fact that if all three - values below are zero, one is not interested on this satellite */ - if (!(ele == 0 && azm == 0 && dgps_age == 0)) { - session->gpsdata.PRN[i] = (int)prn; - session->gpsdata.elevation[i] = (int)ele; - session->gpsdata.azimuth[i] = (int)azm; - /*@i1@*/s = session->gpsdata.ss[i++] = (p2_snr ? p2_snr : ca_snr) / 4; - } - gpsd_report(LOG_IO, - "Navcom: prn = %3u, ele = %02u, azm = %03u, snr = %d (%s), " - "dgps age = %.1fs, log ch = %d, hw ch = 0x%02x\n", - prn, ele, azm, s, (p2_snr?"P2":"C/A"), - (double)dgps_age*0.1, log_channel&0x3f, hw_channel); - gpsd_report(LOG_IO, - "Navcom: sol. valid = %c, clock = %s, pos. = %s, " - "height = %s, err. code = 0x%x\n", - (sol_status&0x01?'Y':'N'), (sol_status&0x02?"stable":"unstable"), - (sol_status&0x04?"dgps":"unaided"), (sol_status&0x08?"solved":"constrained"), - (sol_status&0x01?0x00:sol_status&0x0f00>>8)); - /*@ +predboolothers -charint @*/ - } - - return PDOP_SET | SATELLITE_SET | STATUS_SET; -} - -/* Raw Meas. Data Block */ -static gps_mask_t handle_0xb0(struct gps_device_t *session) -{ - /* L1 wavelength (299792458m/s / 1575420000Hz) */ -#define LAMBDA_L1 (.190293672798364880476317426464) - size_t n; - unsigned char *buf = session->packet.outbuffer + 3; - size_t msg_len = (size_t)getleuw(buf, 1); - u_int16_t week = getleuw(buf, 3); - u_int32_t tow = getleul(buf, 5); - u_int8_t tm_slew_acc = getub(buf, 9); - u_int8_t status = getub(buf, 10); - - char time_str[24]; - (void)unix_to_iso8601(gpstime_to_unix((int)week, (double)tow/1000.0), time_str, sizeof(time_str)); - - gpsd_report(LOG_PROG, - "Navcom: received packet type 0xb0 (Raw Meas. Data Block)\n"); - /*@ -predboolothers @*/ - gpsd_report(LOG_IO, - "Navcom: Epoch = %s, time slew accumulator = %u (1/1023mS), status = 0x%02x " - "(%sclock %s - %u blocks follow)\n", - time_str, - tm_slew_acc, status, (status&0x80?"channel time set - ":""), - (status&0x40?"stable":"not stable"), status&0x0f); - /*@ +predboolothers @*/ - for (n=11; n> sv status = 0x%02x (PRN %u - C/A & L1 %s - P1 %s - P2 & L2 %s)\n", - sv_status, (sv_status&0x1f), (sv_status&0x80?"valid":"invalid"), - (sv_status&0x40?"valid":"invalid"), (sv_status&0x20?"valid":"invalid")); - gpsd_report(LOG_IO+1, - "Navcom: >>> ch status = 0x%02x (Logical channel: %u - CA C/No: %u dBHz) " - "sL1: %u, sL2: %u\n", - ch_status, ch_status&0x0f, ((ch_status&0xf0)>>4)+35, l1_slips, l2_slips); - gpsd_report(LOG_IO+1, - "Navcom: >>> C1: %14.3f, L1: %14.3f, L2: %14.3f, P1: %14.3f, P2: %14.3f\n", - c1, l1, l2, p1, p2); - /*@ +predboolothers -charint @*/ - } -#undef LAMBDA_L1 - return 0; /* Raw measurements not yet implemented in gpsd */ -} - -/* Pseudorange Noise Statistics */ -static gps_mask_t handle_0xb5(struct gps_device_t *session) -{ - if(sizeof(double) == 8) { - union long_double l_d; - unsigned char *buf = session->packet.outbuffer + 3; - u_int16_t week = getleuw(buf, 3); - u_int32_t tow = getleul(buf, 5); - double rms = getled(buf, 9); -#ifdef __UNUSED__ - /* Reason why it's unused is these figures do not agree - with those obtained from the PVT report (handle_0xb1). - The figures from 0xb1 do agree with the values reported - by Navcom's PC utility */ - double ellips_maj = getled(buf, 17); - double ellips_min = getled(buf, 25); - double ellips_azm = getled(buf, 33); - double lat_sd = getled(buf, 41); - double lon_sd = getled(buf, 49); - double alt_sd = getled(buf, 57); - double hrms = sqrt(pow(lat_sd, 2) + pow(lon_sd, 2)); -#endif /* __UNUSED__ */ - session->gpsdata.epe = rms*1.96; -#ifdef __UNUSED__ - session->gpsdata.fix.eph = hrms*1.96; - session->gpsdata.fix.epv = alt_sd*1.96; -#endif /* __UNUSED__ */ - session->gpsdata.sentence_time = gpstime_to_unix((int)week, tow/1000.0) - - session->context->leap_seconds; - gpsd_report(LOG_PROG, - "Navcom: received packet type 0xb5 (Pseudorange Noise Statistics)\n"); - gpsd_report(LOG_IO, - "Navcom: epe = %f\n", session->gpsdata.epe); - return TIME_SET | PERR_SET; - } else { - /* Ignore this message block */ - if (!session->driver.navcom.warned) { - gpsd_report(LOG_WARN, - "Navcom: received packet type 0xb5 (Pseudorange Noise Statistics) ignored " - " - sizeof(double) == 64 bits required\n"); - session->driver.navcom.warned = true; - } - return 0; /* Block ignored - wrong sizeof(double) */ - } -} - -/* LBM DSP Status Block */ -static gps_mask_t handle_0xd3(struct gps_device_t *session UNUSED) -{ - /* This block contains status information about the - unit's L-band (Inmarsat) module. There is nothing - interesting in it for our purposes so we do not deal - with it. This callback is purely to a) stop - "unrecognised packet" messages appearing in the log - and b) explain what it is for the curious */ - return 0; /* Nothing updated */ -} - -/* Identification Block */ -static gps_mask_t handle_0xae(struct gps_device_t *session) -{ - char *engconfstr, *asicstr; - unsigned char *buf = session->packet.outbuffer + 3; - size_t msg_len = (size_t)getleuw(buf, 1); - u_int8_t engconf = getub(buf, 3); - u_int8_t asic = getub(buf, 4); - u_int8_t swvermaj = getub(buf, 5); - u_int8_t swvermin = getub(buf, 6); - u_int16_t dcser = getleuw(buf, 7); - u_int8_t dcclass = getub(buf, 9); - u_int16_t rfcser = getleuw(buf, 10); - u_int8_t rfcclass= getub(buf, 12); - /*@ -stringliteralnoroomfinalnull -type @*/ - u_int8_t softtm[17] = "\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0"; - u_int8_t bootstr[17] = "\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0"; - u_int8_t ioptm[17] = "\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0"; - /*@ +stringliteralnoroomfinalnull +type @*/ - u_int8_t iopvermaj = (u_int8_t)0x00; - u_int8_t iopvermin = (u_int8_t)0x00; - u_int8_t picver = (u_int8_t)0x00; - u_int8_t slsbn = (u_int8_t)0x00; - u_int8_t iopsbn = (u_int8_t)0x00; - memcpy(softtm, &buf[13], 16); - memcpy(bootstr, &buf[29], 16); - if (msg_len == 0x0037) { /* No IOP */ - slsbn = getub(buf, 53); - } else { /* IOP Present */ - iopvermaj = getub(buf, 53); - iopvermin = getub(buf, 54); - memcpy(ioptm, &buf[55], 16); - picver = getub(buf, 71); - slsbn = getub(buf, 72); - iopsbn = getub(buf, 73); - } - - switch(engconf) - { - case 0x00: - engconfstr = "Unknown/Undefined"; - break; - case 0x01: - engconfstr = "NCT 2000 S"; - break; - case 0x02: - engconfstr = "NCT 2000 D"; - break; - case 0x03: - engconfstr = "Startfire Single"; - break; - case 0x04: - engconfstr = "Starfire Dual"; - break; - case 0x05: - engconfstr = "Pole Mount RTK (Internal Radio)"; - break; - case 0x06: - engconfstr = "Pole Mount GIS (LBM)"; - break; - case 0x07: - engconfstr = "Black Box RTK (Internal Radio)"; - break; - case 0x08: - engconfstr = "Black Box GIS (LBM)"; - break; - case 0x80: - engconfstr = "R100"; - break; - case 0x81: - engconfstr = "R200"; - break; - case 0x82: - engconfstr = "R210"; - break; - case 0x83: - engconfstr = "R300"; - break; - case 0x84: - engconfstr = "R310"; - break; - default: - engconfstr = "?"; - } - - switch(asic) - { - case 0x01: - asicstr = "A-ASIC"; - break; - case 0x02: - asicstr = "B-ASIC"; - break; - case 0x03: - asicstr = "C-ASIC"; - break; - case 0x04: - asicstr = "M-ASIC"; - break; - default: - asicstr = "?"; - } - - gpsd_report(LOG_PROG, "Navcom: received packet type 0xae (Identification Block)\n"); - if(msg_len == 0x0037) { - gpsd_report(LOG_INF, "Navcom: ID Data: " - "%s %s Ver. %u.%u.%u, DC S/N: %u.%u, RF S/N: %u.%u, " - "Build ID: %s, Boot software: %s\n", - engconfstr, asicstr, swvermaj, swvermin, slsbn, dcser, dcclass, - rfcser, rfcclass, softtm, bootstr); - } else { - gpsd_report(LOG_INF, "Navcom: ID Data: " - "%s %s Ver. %u.%u.%u, DC S/N: %u.%u, RF S/N: %u.%u, " - "Build ID: %s, Boot software: %s, " - "IOP Ver.: %u.%u.%u, PIC: %u, IOP Build ID: %s\n", - engconfstr, asicstr, swvermaj, swvermin, slsbn, dcser, dcclass, - rfcser, rfcclass, softtm, bootstr, iopvermaj, iopvermin, iopsbn, - picver, ioptm); - } - - /*@ -formattype @*/ - (void)snprintf(session->subtype, sizeof(session->subtype), - "%s %s Ver. %u.%u.%u S/N %u.%u %u.%u", - engconfstr, asicstr, swvermaj, swvermin, slsbn, dcser, dcclass, - rfcser, rfcclass); - /*@ +formattype @*/ - return DEVICEID_SET; -} - -/* Clock Drift and Offset */ -static gps_mask_t handle_0xef(struct gps_device_t *session) -{ - unsigned char *buf = session->packet.outbuffer + 3; - u_int16_t week = getleuw(buf, 3); - u_int32_t tow = getleul(buf, 5); - int8_t osc_temp = getsb(buf, 9); - u_int8_t nav_status = getub(buf, 10); - union long_double l_d; - double nav_clock_offset; - union int_float i_f; - float nav_clock_drift; - float osc_filter_drift_est; - int32_t time_slew = (int32_t)getlesl(buf, 27); - if (sizeof(double) == 8) { - nav_clock_offset = getled(buf, 11); - } else { - nav_clock_offset = NAN; - } - if (sizeof(float) == 4) { - nav_clock_drift = getlef(buf, 19); - osc_filter_drift_est = getlef(buf, 23); - } else { - nav_clock_drift = NAN; - osc_filter_drift_est = NAN; - } - - session->gpsdata.sentence_time = gpstime_to_unix((int)week, tow/1000.0) - - session->context->leap_seconds; - - gpsd_report(LOG_PROG, - "Navcom: received packet type 0xef (Clock Drift and Offset)\n"); - gpsd_report(LOG_IO, - "Navcom: oscillator temp. = %d, nav. status = 0x%02x, " - "nav. clock offset = %f, nav. clock drift = %f, " - "osc. filter drift est. = %f, acc.time slew value = %d\n", - osc_temp, nav_status, nav_clock_offset, nav_clock_drift, - osc_filter_drift_est, time_slew); - return TIME_SET; -} - - -/*@ +charint @*/ -gps_mask_t navcom_parse(struct gps_device_t *session, unsigned char *buf, size_t len) -{ - unsigned char cmd_id; - unsigned char *payload; - unsigned int msg_len; - - if (len == 0) - return 0; - - cmd_id = getub(buf, 3); - payload = &buf[6]; - msg_len = getleuw(buf, 4); - - /*@ -usedef -compdef @*/ - gpsd_report(LOG_RAW, "Navcom: packet type 0x%02x, length %d: %s\n", - cmd_id, msg_len, gpsd_hexdump_wrapper(buf, len, LOG_RAW)); - /*@ +usedef +compdef @*/ - - (void)snprintf(session->gpsdata.tag, sizeof(session->gpsdata.tag), - "0x%02x",cmd_id); - - switch (cmd_id) - { - case 0x06: - return handle_0x06(session); - case 0x15: - return handle_0x15(session); - case 0x81: - return handle_0x81(session); - case 0x83: - return handle_0x83(session); - case 0x86: - return handle_0x86(session); - case 0xae: - return handle_0xae(session); - case 0xb0: - return handle_0xb0(session); - case 0xb1: - return handle_0xb1(session); - case 0xb5: - return handle_0xb5(session); - case 0xd3: - return handle_0xd3(session); - case 0xef: - return handle_0xef(session); - default: - gpsd_report(LOG_PROG, - "Navcom: received packet type 0x%02x, length %d - unknown or unimplemented\n", - cmd_id, msg_len); - return 0; - } -} -/*@ -charint @*/ - -static gps_mask_t navcom_parse_input(struct gps_device_t *session) -{ - gps_mask_t st; - - if (session->packet.type == NAVCOM_PACKET){ - st = navcom_parse(session, session->packet.outbuffer, session->packet.outbuflen); - session->gpsdata.driver_mode = MODE_BINARY; /* binary */ - return st; -#ifdef NMEA_ENABLE - } else if (session->packet.type == NMEA_PACKET) { - st = nmea_parse((char *)session->packet.outbuffer, session); - session->gpsdata.driver_mode = MODE_NMEA; /* NMEA */ - return st; -#endif /* NMEA_ENABLE */ - } else - return 0; -} - - -/* this is everything we export */ -const struct gps_type_t navcom_binary = -{ - .type_name = "Navcom binary", /* full name of type */ - .packet_type = NAVCOM_PACKET, /* lexer packet type */ - .trigger = "\x02\x99\x66", /* packet leader */ - .channels = NAVCOM_CHANNELS, /* 12 L1 + 12 L2 + 2 Inmarsat L-Band */ - .control_send = navcom_control_send, /* no control sender yet */ - .probe_wakeup = navcom_ping, /* wakeup to be done before hunt */ - .probe_detect = NULL, /* no probe */ - .probe_subtype = navcom_probe_subtype, /* subtype probing */ -#ifdef ALLOW_RECONFIGURE - .configurator = NULL, /* no reconfigure */ -#endif /* ALLOW_RECONFIGURE */ - .get_packet = generic_get, /* use generic one */ - .parse_packet = navcom_parse_input, /* parse message packets */ - .rtcm_writer = pass_rtcm, /* send RTCM data straight */ - .speed_switcher = navcom_speed, /* we do change baud rates */ - .mode_switcher = NULL, /* there is not a mode switcher */ - .rate_switcher = NULL, /* no sample-rate switcher */ - .cycle_chars = -1, /* ignore, no rate switch */ -#ifdef ALLOW_RECONFIGURE - .revert = NULL, /* no reversion code */ -#endif /* ALLOW_RECONFIGURE */ - .wrapup = NULL, /* ignore, no wrapup */ - .cycle = 1, /* updates every second */ -}; - -#endif /* defined(NAVCOM_ENABLE) && defined(BINARY_ENABLE) */ diff --git a/nmea_parse.c b/nmea_parse.c deleted file mode 100644 index a8a2b556..00000000 --- a/nmea_parse.c +++ /dev/null @@ -1,936 +0,0 @@ -/* $Id$ */ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "gpsd_config.h" -#include "gpsd.h" -#include "timebase.h" - -#ifdef MKT3301_ENABLE -extern gps_mask_t processMKT3301(int c UNUSED, char *field[], struct gps_device_t *session); -#endif /* MKT3301_ENABLE */ - -#ifdef NMEA_ENABLE -/************************************************************************** - * - * Parser helpers begin here - * - **************************************************************************/ - -static void do_lat_lon(char *field[], struct gps_data_t *out) -/* process a pair of latitude/longitude fields starting at field index BEGIN */ -{ - double lat, lon, d, m; - char str[20], *p; - int updated = 0; - - if (*(p = field[0]) != '\0') { - strncpy(str, p, 20); - (void)sscanf(p, "%lf", &lat); - m = 100.0 * modf(lat / 100.0, &d); - lat = d + m / 60.0; - p = field[1]; - if (*p == 'S') - lat = -lat; - if (out->fix.latitude != lat) - out->fix.latitude = lat; - updated++; - } - if (*(p = field[2]) != '\0') { - strncpy(str, p, 20); - (void)sscanf(p, "%lf", &lon); - m = 100.0 * modf(lon / 100.0, &d); - lon = d + m / 60.0; - - p = field[3]; - if (*p == 'W') - lon = -lon; - if (out->fix.longitude != lon) - out->fix.longitude = lon; - updated++; - } -} - -/************************************************************************** - * - * Scary timestamp fudging begins here - * - * Four sentences, GGA and GLL and RMC and ZDA, contain timestamps. - * GGA/GLL/RMC timestamps look like hhmmss.ss, with the trailing .ss part - * optional. RMC has a date field, in the format ddmmyy. ZDA has - * separate fields for day/month/year, with a 4-digit year. This - * means that for RMC we must supply a century and for GGA and GLL we - * must supply a century, year, and day. We get the missing data from - * a previous RMC or ZDA; century in RMC is supplied by a constant if - * there has been no previous ZDA. - * - **************************************************************************/ - -#define DD(s) ((int)((s)[0]-'0')*10+(int)((s)[1]-'0')) - -static void merge_ddmmyy(char *ddmmyy, struct gps_device_t *session) -/* sentence supplied ddmmyy, but no century part */ -{ - if (session->driver.nmea.date.tm_year == 0) - session->driver.nmea.date.tm_year = (CENTURY_BASE + DD(ddmmyy+4)) - 1900; - session->driver.nmea.date.tm_mon = DD(ddmmyy+2)-1; - session->driver.nmea.date.tm_mday = DD(ddmmyy); -} - -static void merge_hhmmss(char *hhmmss, struct gps_device_t *session) -/* update from a UTC time */ -{ - int old_hour = session->driver.nmea.date.tm_hour; - - session->driver.nmea.date.tm_hour = DD(hhmmss); - if (session->driver.nmea.date.tm_hour < old_hour) /* midnight wrap */ - session->driver.nmea.date.tm_mday++; - session->driver.nmea.date.tm_min = DD(hhmmss+2); - session->driver.nmea.date.tm_sec = DD(hhmmss+4); - session->driver.nmea.subseconds = atof(hhmmss+4) - session->driver.nmea.date.tm_sec; -} - -#undef DD - -/************************************************************************** - * - * Compare GPS timestamps for equality. Depends on the fact that the - * timestamp granularity of GPS is 1/100th of a second. Use this to avoid - * naive float comparisons. - * - **************************************************************************/ - -#define GPS_TIME_EQUAL(a, b) (fabs((a) - (b)) < 0.01) - -/************************************************************************** - * - * NMEA sentence handling begins here - * - **************************************************************************/ - -static gps_mask_t processGPRMC(int count, char *field[], struct gps_device_t *session) -/* Recommend Minimum Course Specific GPS/TRANSIT Data */ -{ - /* - RMC,225446.33,A,4916.45,N,12311.12,W,000.5,054.7,191194,020.3,E,A*68 - 1 225446.33 Time of fix 22:54:46 UTC - 2 A Status of Fix: A = Autonomous, valid; - D = Differential, valid; V = invalid - 3,4 4916.45,N Latitude 49 deg. 16.45 min North - 5,6 12311.12,W Longitude 123 deg. 11.12 min West - 7 000.5 Speed over ground, Knots - 8 054.7 Course Made Good, True north - 9 181194 Date of fix 18 November 1994 - 10,11 020.3,E Magnetic variation 20.3 deg East - 12 A FAA mode indicator (NMEA 2.3 and later) - A=autonomous, D=differential, E=Estimated, - N=not valid, S=Simulator, M=Manual input mode - *68 mandatory nmea_checksum - - * SiRF chipsets don't return either Mode Indicator or magnetic variation. - */ - gps_mask_t mask = 0; - - if (strcmp(field[2], "V")==0) { - /* copes with Magellan EC-10X, see below */ - if (session->gpsdata.status != STATUS_NO_FIX) { - session->gpsdata.status = STATUS_NO_FIX; - mask |= STATUS_SET; - } - if (session->gpsdata.fix.mode >= MODE_2D) { - session->gpsdata.fix.mode = MODE_NO_FIX; - mask |= MODE_SET; - } - /* set something nz, so it won't look like an unknown sentence */ - mask |= ONLINE_SET; - } else if (strcmp(field[2], "A")==0) { - if (count > 9) { - merge_hhmmss(field[1], session); - merge_ddmmyy(field[9], session); - mask |= TIME_SET; - session->gpsdata.fix.time = (double)mkgmtime(&session->driver.nmea.date)+session->driver.nmea.subseconds; - if (!GPS_TIME_EQUAL(session->gpsdata.sentence_time, session->gpsdata.fix.time)) { - mask |= CYCLE_START_SET; - gpsd_report(LOG_PROG, "GPRMC starts a reporting cycle.\n"); - } - session->gpsdata.sentence_time = session->gpsdata.fix.time; - } - do_lat_lon(&field[3], &session->gpsdata); - mask |= LATLON_SET; - session->gpsdata.fix.speed = atof(field[7]) * KNOTS_TO_MPS; - session->gpsdata.fix.track = atof(field[8]); - mask |= (TRACK_SET | SPEED_SET); - /* - * This copes with GPSes like the Magellan EC-10X that *only* emit - * GPRMC. In this case we set mode and status here so the client - * code that relies on them won't mistakenly believe it has never - * received a fix. - */ - if (session->gpsdata.status == STATUS_NO_FIX) { - session->gpsdata.status = STATUS_FIX; /* could be DGPS_FIX, we can't tell */ - mask |= STATUS_SET; - } - if (session->gpsdata.fix.mode < MODE_2D) { - session->gpsdata.fix.mode = MODE_2D; - mask |= MODE_SET; - } - } - - gpsd_report(LOG_PROG, "GPRMC sets mode %d\n", session->gpsdata.fix.mode); - return mask; -} - -static gps_mask_t processGPGLL(int count, char *field[], struct gps_device_t *session) -/* Geographic position - Latitude, Longitude */ -{ - /* Introduced in NMEA 3.0. - - $GPGLL,4916.45,N,12311.12,W,225444,A,A*5C - - 1,2: 4916.46,N Latitude 49 deg. 16.45 min. North - 3,4: 12311.12,W Longitude 123 deg. 11.12 min. West - 5: 225444 Fix taken at 22:54:44 UTC - 6: A Data valid - 7: A Autonomous mode - 8: *5C Mandatory NMEA checksum - - 1,2 Latitude, N (North) or S (South) - 3,4 Longitude, E (East) or W (West) - 5 UTC of position - 6 A=Active, V=Void - 7 Mode Indicator - A = Autonomous mode - D = Differential Mode - E = Estimated (dead-reckoning) mode - M = Manual Input Mode - S = Simulated Mode - N = Data Not Valid - - I found a note at - indicating that the Garmin 65 does not return time and status. - SiRF chipsets don't return the Mode Indicator. - This code copes gracefully with both quirks. - - Unless you care about the FAA indicator, this sentence supplies nothing - that GPRMC doesn't already. But at least one Garmin GPS -- the 48 - actually ships updates in GPLL that aren't redundant. - */ - char *status = field[7]; - gps_mask_t mask = ERROR_SET; - - if (strcmp(field[6], "A")==0 && (count < 8 || *status != 'N')) { - int newstatus = session->gpsdata.status; - - mask = 0; - merge_hhmmss(field[5], session); - if (session->driver.nmea.date.tm_year == 0) - gpsd_report(LOG_WARN, "can't use GGL time until after ZDA or RMC has supplied a year.\n"); - else { - mask = TIME_SET; - session->gpsdata.fix.time = (double)mkgmtime(&session->driver.nmea.date)+session->driver.nmea.subseconds; - if (!GPS_TIME_EQUAL(session->gpsdata.sentence_time, session->gpsdata.fix.time)) { - mask |= CYCLE_START_SET; - gpsd_report(LOG_PROG, "GPGLL starts a reporting cycle.\n"); - } - session->gpsdata.sentence_time = session->gpsdata.fix.time; - } - do_lat_lon(&field[1], &session->gpsdata); - mask |= LATLON_SET; - if (count >= 8 && *status == 'D') - newstatus = STATUS_DGPS_FIX; /* differential */ - else - newstatus = STATUS_FIX; - /* - * This is a bit dodgy. Technically we shouldn't set the mode - * bit until we see GSA. But it may be later in the cycle, - * some devices like the FV-18 don't send it by default, and - * elsewhere in the code we want to be able to test for the - * presence of a valid fix with mode > MODE_NO_FIX. - */ - if (session->gpsdata.fix.mode < MODE_2D) { - session->gpsdata.fix.mode = MODE_2D; - mask |= MODE_SET; - } - session->gpsdata.status = newstatus; - mask |= STATUS_SET; - gpsd_report(LOG_PROG, "GPGLL sets status %d\n", session->gpsdata.status); - } - - return mask; -} - -static gps_mask_t processGPGGA(int c UNUSED, char *field[], struct gps_device_t *session) -/* Global Positioning System Fix Data */ -{ - /* - GGA,123519,4807.038,N,01131.324,E,1,08,0.9,545.4,M,46.9,M, , *42 - 1 123519 Fix taken at 12:35:19 UTC - 2,3 4807.038,N Latitude 48 deg 07.038' N - 4,5 01131.324,E Longitude 11 deg 31.324' E - 6 1 Fix quality: 0 = invalid, 1 = GPS, 2 = DGPS, - 3=PPS (Precise Position Service), - 4=RTK (Real Time Kinematic) with fixed integers, - 5=Float RTK, 6=Estimated, 7=Manual, 8=Simulator - 7 08 Number of satellites being tracked - 8 0.9 Horizontal dilution of position - 9,10 545.4,M Altitude, Metres above mean sea level - 11,12 46.9,M Height of geoid (mean sea level) above WGS84 - ellipsoid, in Meters - (empty field) time in seconds since last DGPS update - (empty field) DGPS station ID number (0000-1023) - */ - gps_mask_t mask; - - session->gpsdata.status = atoi(field[6]); - mask = STATUS_SET; - if (session->gpsdata.status > STATUS_NO_FIX) { - char *altitude; - double oldfixtime = session->gpsdata.fix.time; - - merge_hhmmss(field[1], session); - if (session->driver.nmea.date.tm_year == 0) - gpsd_report(LOG_WARN, "can't use GGA time until after ZDA or RMC has supplied a year.\n"); - else { - mask |= TIME_SET; - session->gpsdata.fix.time = (double)mkgmtime(&session->driver.nmea.date)+session->driver.nmea.subseconds; - if (!GPS_TIME_EQUAL(session->gpsdata.sentence_time, session->gpsdata.fix.time)) { - mask |= CYCLE_START_SET; - gpsd_report(LOG_PROG, "GPGGA starts a reporting cycle.\n"); - } - session->gpsdata.sentence_time = session->gpsdata.fix.time; - } - do_lat_lon(&field[2], &session->gpsdata); - mask |= LATLON_SET; - session->gpsdata.satellites_used = atoi(field[7]); - altitude = field[9]; - /* - * SiRF chipsets up to version 2.2 report a null altitude field. - * See . - * If we see this, force mode to 2D at most. - */ - if (altitude[0] == '\0') { - if (session->gpsdata.fix.mode == MODE_3D) { - session->gpsdata.fix.mode = session->gpsdata.status ? MODE_2D : MODE_NO_FIX; - mask |= MODE_SET; - } - } else { - double oldaltitude = session->gpsdata.fix.altitude; - - session->gpsdata.fix.altitude = atof(altitude); - mask |= ALTITUDE_SET; - /* - * This is a bit dodgy. Technically we shouldn't set the mode - * bit until we see GSA. But it may be later in the cycle, - * some devices like the FV-18 don't send it by default, and - * elsewhere in the code we want to be able to test for the - * presence of a valid fix with mode > MODE_NO_FIX. - */ - if (session->gpsdata.fix.mode < MODE_3D) { - session->gpsdata.fix.mode = MODE_3D; - mask |= MODE_SET; - } - - /* - * Compute climb/sink in the simplest possible way. - * This substitutes for the climb report provided by - * SiRF and Garmin chips, which might have some smoothing - * going on. - */ - if (isnan(oldaltitude)!=0 || session->gpsdata.fix.time==oldfixtime) - session->gpsdata.fix.climb = 0; - else { - session->gpsdata.fix.climb = (session->gpsdata.fix.altitude-oldaltitude)/(session->gpsdata.fix.time-oldfixtime); - } - mask |= CLIMB_SET; - } - if (strlen(field[11]) > 0) { - session->gpsdata.separation = atof(field[11]); - } else { - session->gpsdata.separation = wgs84_separation(session->gpsdata.fix.latitude,session->gpsdata.fix.longitude); - } - } - gpsd_report(LOG_PROG, "GPGGA sets status %d and mode %d (%s)\n", session->gpsdata.status, session->gpsdata.fix.mode, ((mask&MODE_SET)!=0) ? "changed" : "unchanged"); - return mask; -} - -static gps_mask_t processGPGSA(int count, char *field[], struct gps_device_t *session) -/* GPS DOP and Active Satellites */ -{ - /* - eg1. $GPGSA,A,3,,,,,,16,18,,22,24,,,3.6,2.1,2.2*3C - eg2. $GPGSA,A,3,19,28,14,18,27,22,31,39,,,,,1.7,1.0,1.3*35 - 1 = Mode: - M=Manual, forced to operate in 2D or 3D - A=Automatic, 3D/2D - 2 = Mode: 1=Fix not available, 2=2D, 3=3D - 3-14 = PRNs of satellites used in position fix (null for unused fields) - 15 = PDOP - 16 = HDOP - 17 = VDOP - */ - gps_mask_t mask; - int i; - - /* - * One chipset called the i.Trek M3 issues GPGSA lines that look like - * this: "$GPGSA,A,1,,,,*32" when it has no fix. This is broken - * in at least two ways: it's got the wrong number of fields, and - * it claims to be a valid sentence (A flag) when it isn't. - * Alarmingly, it's possible this error may be generic to SiRFstarIII. - */ - if (count < 17) - return ONLINE_SET; - - session->gpsdata.fix.mode = atoi(field[2]); - /* - * The first arm of this conditional ignores dead-reckoning - * fixes from an Antaris chipset. which returns E in field 2 - * for a dead-reckoning estimate. Fix by Andreas Stricker. - */ - if (session->gpsdata.fix.mode == 0 && field[2][0] == 'E') - mask = 0; - else - mask = MODE_SET; - gpsd_report(LOG_PROG, "GPGSA sets mode %d\n", session->gpsdata.fix.mode); - session->gpsdata.pdop = atof(field[15]); - session->gpsdata.hdop = atof(field[16]); - session->gpsdata.vdop = atof(field[17]); - session->gpsdata.satellites_used = 0; - memset(session->gpsdata.used,0,sizeof(session->gpsdata.used)); - /* the magic 6 here counts the tag, two mode fields, and the DOP fields */ - for (i = 0; i < count - 6; i++) { - int prn = atoi(field[i+3]); - if (prn > 0) - session->gpsdata.used[session->gpsdata.satellites_used++] = prn; - } - mask |= USED_SET; - if (strlen(field[count-3]) > 0) - mask |= HDOP_SET; - if (strlen(field[count-2]) > 0) - mask |= VDOP_SET; - if (strlen(field[count-1]) > 0) - mask |= HDOP_SET; - return mask; -} - -static gps_mask_t processGPGSV(int count, char *field[], struct gps_device_t *session) -/* GPS Satellites in View */ -{ - /* - GSV,2,1,08,01,40,083,46,02,17,308,41,12,07,344,39,14,22,228,45*75 - 2 Number of sentences for full data - 1 Sentence 1 of 2 - 08 Total number of satellites in view - 01 Satellite PRN number - 40 Elevation, degrees - 083 Azimuth, degrees - 46 Signal-to-noise ratio in decibels - - There my be up to three GSV sentences in a data packet - */ - int n, fldnum; - if (count <= 3) { - gpsd_zero_satellites(&session->gpsdata); - session->gpsdata.satellites = 0; - return ERROR_SET; - } - if (count % 4 != 0){ - gpsd_report(LOG_WARN, "malformed GPGSV - fieldcount %d %% 4 != 0\n", count); - gpsd_zero_satellites(&session->gpsdata); - session->gpsdata.satellites = 0; - return ERROR_SET; - } - - session->driver.nmea.await = atoi(field[1]); - if (sscanf(field[2], "%d", &session->driver.nmea.part) < 1) { - gpsd_zero_satellites(&session->gpsdata); - return ERROR_SET; - } else if (session->driver.nmea.part == 1) - gpsd_zero_satellites(&session->gpsdata); - - for (fldnum = 4; fldnum < count; ) { - if (session->gpsdata.satellites >= MAXCHANNELS) { - gpsd_report(LOG_ERROR, "internal error - too many satellites!\n"); - gpsd_zero_satellites(&session->gpsdata); - break; - } - session->gpsdata.PRN[session->gpsdata.satellites] = atoi(field[fldnum++]); - session->gpsdata.elevation[session->gpsdata.satellites] = atoi(field[fldnum++]); - session->gpsdata.azimuth[session->gpsdata.satellites] = atoi(field[fldnum++]); - session->gpsdata.ss[session->gpsdata.satellites] = atoi(field[fldnum++]); - /* - * Incrementing this unconditionally falls afoul of chipsets like - * the Motorola Oncore GT+ that emit empty fields at the end of the - * last sentence in a GPGSV set if the number of satellites is not - * a multiple of 4. - */ - if (session->gpsdata.PRN[session->gpsdata.satellites] != 0) - session->gpsdata.satellites++; - } - if (session->driver.nmea.part == session->driver.nmea.await && atoi(field[3]) != session->gpsdata.satellites) - gpsd_report(LOG_WARN, "GPGSV field 3 value of %d != actual count %d\n", - atoi(field[3]), session->gpsdata.satellites); - - /* not valid data until we've seen a complete set of parts */ - if (session->driver.nmea.part < session->driver.nmea.await) { - gpsd_report(LOG_PROG, "Partial satellite data (%d of %d).\n", session->driver.nmea.part, session->driver.nmea.await); - return ERROR_SET; - } - /* - * This sanity check catches an odd behavior of SiRFstarII receivers. - * When they can't see any satellites at all (like, inside a - * building) they sometimes cough up a hairball in the form of a - * GSV packet with all the azimuth entries 0 (but nonzero - * elevations). This behavior was observed under SiRF firmware - * revision 231.000.000_A2. - */ - for (n = 0; n < session->gpsdata.satellites; n++) - if (session->gpsdata.azimuth[n] != 0) - goto sane; - gpsd_report(LOG_WARN, "Satellite data no good (%d of %d).\n", session->driver.nmea.part, session->driver.nmea.await); - gpsd_zero_satellites(&session->gpsdata); - return ERROR_SET; - sane: - gpsd_report(LOG_PROG, "Satellite data OK (%d of %d).\n", session->driver.nmea.part, session->driver.nmea.await); - return SATELLITE_SET; - } - -static gps_mask_t processPGRME(int c UNUSED, char *field[], struct gps_device_t *session) -/* Garmin Estimated Position Error */ -{ - /* - $PGRME,15.0,M,45.0,M,25.0,M*22 - 1 = horizontal error estimate - 2 = units - 3 = vertical error estimate - 4 = units - 5 = spherical error estimate - 6 = units - * - * Garmin won't say, but the general belief is that these are 50% CEP. - * We follow the advice at . - * If this assumption changes here, it should also change in garmin.c - * where we scale error estimates from Garmin binary packets, and - * in libgpsd_core.c where we generate $PGRME. - */ - if ((strcmp(field[2], "M")!=0) || - (strcmp(field[4], "M")!=0) || - (strcmp(field[6], "M")!=0)){ - session->gpsdata.fix.eph = - session->gpsdata.fix.epv = - session->gpsdata.epe = 100; - return ERROR_SET; - } - - session->gpsdata.fix.eph = atof(field[1]) * (GPSD_CONFIDENCE/CEP50_SIGMA); - session->gpsdata.fix.epv = atof(field[3]) * (GPSD_CONFIDENCE/CEP50_SIGMA); - session->gpsdata.epe = atof(field[5]) * (GPSD_CONFIDENCE/CEP50_SIGMA); - - return HERR_SET | VERR_SET | PERR_SET; -} - -static gps_mask_t processGPZDA(int c UNUSED, char *field[], struct gps_device_t *session) -/* Time & Date */ -{ - gps_mask_t mask = TIME_SET; - /* - $GPZDA,160012.71,11,03,2004,-1,00*7D - 1) UTC time (hours, minutes, seconds, may have fractional subsecond) - 2) Day, 01 to 31 - 3) Month, 01 to 12 - 4) Year (4 digits) - 5) Local zone description, 00 to +- 13 hours - 6) Local zone minutes description, apply same sign as local hours - 7) Checksum - */ - merge_hhmmss(field[1], session); - session->driver.nmea.date.tm_year = atoi(field[4]) - 1900; - session->driver.nmea.date.tm_mon = atoi(field[3])-1; - session->driver.nmea.date.tm_mday = atoi(field[2]); - session->gpsdata.fix.time = (double)mkgmtime(&session->driver.nmea.date)+session->driver.nmea.subseconds; - if (!GPS_TIME_EQUAL(session->gpsdata.sentence_time, session->gpsdata.fix.time)) { - mask |= CYCLE_START_SET; - gpsd_report(LOG_PROG, "GPZDA starts a reporting cycle.\n"); - } - session->gpsdata.sentence_time = session->gpsdata. fix.time; - return mask; -} - -#ifdef TNT_ENABLE -static gps_mask_t processTNTHTM(int c UNUSED, char *field[], struct gps_device_t *session) -{ - /* - * Proprietary sentence for True North Technologies Magnetic Compass. - * This may also apply to some Honeywell units since they may have been - * designed by True North. - - HTM,x.x,a,x.x,a,x.x,a,x.x,x.x*hh - Fields in order: - 1. True heading in degrees - 2. magnetometer status character: - C = magnetometer calibration alarm - L = low alarm - M = low warning - N = normal - O = high warning - P = high alarm - V = magnetometer voltage level alarm - 3. pitch angle - 4. pitch status character - see field 2 above - 5. roll angle - 6. roll status character - see field 2 above - 7. dip angle - 8. relative magnitude horizontal component of earth's magnetic field - *hh mandatory nmea_checksum - */ - gps_mask_t mask; - mask = ONLINE_SET; - - //gpsd_zero_satellites(&session->gpsdata); - - /* - * Heading maps to track. - * Pitch maps to climb. - * Roll maps to speed. - * Dip maps to altitude. - */ - session->gpsdata.fix.time = timestamp(); - session->gpsdata.fix.track = atof(field[1]); - session->gpsdata.headingStatus = *field[2]; - session->gpsdata.fix.climb = atof(field[3]); - session->gpsdata.pitchStatus = *field[4]; - session->gpsdata.fix.speed = atof(field[5]); - session->gpsdata.rollStatus = *field[6]; - session->gpsdata.fix.altitude = atof(field[7]); - session->gpsdata.horzField = atof(field[8]); - session->gpsdata.fix.mode = MODE_3D; - mask |= (STATUS_SET | MODE_SET | TRACK_SET | SPEED_SET | CLIMB_SET | ALTITUDE_SET); - session->gpsdata.status = STATUS_FIX; /* could be DGPS_FIX */ - - gpsd_report(LOG_RAW, "Heading %lf %c.\n", session->gpsdata.fix.track, session->gpsdata.headingStatus); - return mask; -} -#endif /* TNT_ENABLE */ - -#ifdef OCEANSERVER_ENABLE -static gps_mask_t processOHPR(int c UNUSED, char *field[], struct gps_device_t *session) -{ - /* - * Proprietary sentence for OceanServer Magnetic Compass. - - OHPR,x.x,x.x,x.x,x.x,x.x,x.x,x.x,x.x,x.x,x.x,x.x,x.x,x.x,x.x,x.x,x.x,x.x,x.x*hh - Fields in order: - 1. Azimuth - 2. Pitch Angle - 3. Roll Angle - 4. Temperature - 5. Depth (feet) - 6. Magnetic Vector Length - 7-9. 3 axis Magnetic Field readings x,y,z - 10. Acceleration Vector Length - 11-13. 3 axis Acceleration Readings x,y,z - 14. Reserved - 15-16. 2 axis Gyro Output, X,y - 17. Reserved - 18. Reserved - *hh mandatory nmea_checksum - */ - gps_mask_t mask; - mask = ONLINE_SET; - - //gpsd_zero_satellites(&session->gpsdata); - - /* - * Heading maps to track. - * Pitch maps to climb. - * Roll maps to speed. - * Depth maps to altitude. - */ - session->gpsdata.fix.time = timestamp(); - session->gpsdata.fix.track = atof(field[1]); - session->gpsdata.fix.climb = atof(field[2]); - session->gpsdata.fix.speed = atof(field[3]); - session->gpsdata.temperature = atof(field[4]); - session->gpsdata.fix.altitude = atof(field[5]); - session->gpsdata.magnetic_length = atof(field[6]); - session->gpsdata.magnetic_field_x = atof(field[7]); - session->gpsdata.magnetic_field_y = atof(field[8]); - session->gpsdata.magnetic_field_z = atof(field[9]); - session->gpsdata.acceleration_length = atof(field[10]); - session->gpsdata.acceleration_field_x = atof(field[11]); - session->gpsdata.acceleration_field_y = atof(field[12]); - session->gpsdata.acceleration_field_z = atof(field[13]); - session->gpsdata.gyro_output_x = atof(field[15]); - session->gpsdata.gyro_output_y = atof(field[16]); - session->gpsdata.fix.mode = MODE_3D; - mask |= (STATUS_SET | MODE_SET | TRACK_SET | SPEED_SET | CLIMB_SET | ALTITUDE_SET); - session->gpsdata.status = STATUS_FIX; /* could be DGPS_FIX */ - - gpsd_report(LOG_RAW, "Heading %lf.\n", session->gpsdata.fix.track); - return mask; -} -#endif /* OCEANSERVER_ENABLE */ - -#ifdef ASHTECH_ENABLE -static gps_mask_t processPASHR(int c UNUSED, char *field[], struct gps_device_t *session) -{ - gps_mask_t mask; - mask = ONLINE_SET; - - if (0 == strcmp("RID", field[1])){ /* Receiver ID */ - (void)snprintf(session->subtype, sizeof(session->subtype)-1, - "%s ver %s", field[2], field[3]); - return 0; - } else if (0 == strcmp("POS", field[1])){ /* 3D Position */ - mask |= MODE_SET | STATUS_SET | CYCLE_START_SET; - if (0 == strlen(field[2])){ - /* empty first field means no 3D fix is available */ - session->gpsdata.status = STATUS_NO_FIX; - session->gpsdata.fix.mode = MODE_NO_FIX; - return mask; - } - - /* if we make it this far, we at least have a 3D fix */ - session->gpsdata.fix.mode = MODE_3D; - if (1 == atoi(field[2])) - session->gpsdata.status = STATUS_DGPS_FIX; - else - session->gpsdata.status = STATUS_FIX; - - session->gpsdata.satellites_used = atoi(field[3]); - merge_hhmmss(field[4], session); - do_lat_lon(&field[5], &session->gpsdata); - session->gpsdata.fix.altitude = atof(field[9]); - session->gpsdata.fix.track = atof(field[11]); - session->gpsdata.fix.speed = atof(field[12]) / MPS_TO_KPH; - session->gpsdata.fix.climb = atof(field[13]); - session->gpsdata.pdop = atof(field[14]); - session->gpsdata.hdop = atof(field[15]); - session->gpsdata.vdop = atof(field[16]); - session->gpsdata.tdop = atof(field[17]); - mask |= (TIME_SET | LATLON_SET | ALTITUDE_SET); - mask |= (SPEED_SET | TRACK_SET | CLIMB_SET); - mask |= (PDOP_SET | HDOP_SET | VDOP_SET | TDOP_SET); - } else if (0 == strcmp("SAT", field[1])){ /* Satellite Status */ - int i, n, p, u; - n = session->gpsdata.satellites = atoi(field[2]); - u = 0; - for (i = 0; i < n; i++){ - session->gpsdata.PRN[i] = p = atoi(field[3+i*5+0]); - session->gpsdata.azimuth[i] = atoi(field[3+i*5+1]); - session->gpsdata.elevation[i] = atoi(field[3+i*5+2]); - session->gpsdata.ss[i] = atoi(field[3+i*5+3]); - if (field[3+i*5+4][0] == 'U') - session->gpsdata.used[u++] = p; - } - session->gpsdata.satellites_used = u; - mask |= SATELLITE_SET | USED_SET; - } - return mask; -} -#endif /* ASHTECH_ENABLE */ - -#ifdef __UNUSED__ -static short nmea_checksum(char *sentence, unsigned char *correct_sum) -/* is the checksum on the specified sentence good? */ -{ - unsigned char sum = '\0'; - char c, *p = sentence, csum[3]; - - while ((c = *p++) != '*' && c != '\0') - sum ^= c; - if (correct_sum) - *correct_sum = sum; - (void)snprintf(csum, sizeof(csum), "%02X", sum); - return(csum[0]==toupper(p[0])) && (csum[1]==toupper(p[1])); -} -#endif /* __ UNUSED__ */ - -/************************************************************************** - * - * Entry points begin here - * - **************************************************************************/ - -/*@ -mayaliasunique @*/ -gps_mask_t nmea_parse(char *sentence, struct gps_device_t *session) -/* parse an NMEA sentence, unpack it into a session structure */ -{ - typedef gps_mask_t (*nmea_decoder)(int count, char *f[], struct gps_device_t *session); - static struct { - char *name; - int nf; /* minimum number of fields required to parse */ - nmea_decoder decoder; - } nmea_phrase[] = { - /*@ -nullassign @*/ - {"RMC", 8, processGPRMC}, - {"GGA", 13, processGPGGA}, - {"GLL", 7, processGPGLL}, - {"GSA", 17, processGPGSA}, - {"GSV", 0, processGPGSV}, - {"VTG", 0, NULL}, /* ignore Velocity Track made Good */ - {"ZDA", 7, processGPZDA}, - {"PGRMC", 0, NULL}, /* ignore Garmin Sensor Config */ - {"PGRME", 7, processPGRME}, - {"PGRMI", 0, NULL}, /* ignore Garmin Sensor Init */ - {"PGRMO", 0, NULL}, /* ignore Garmin Sentence Enable */ -#ifdef TNT_ENABLE - {"PTNTHTM", 9, processTNTHTM}, -#endif /* TNT_ENABLE */ -#ifdef ASHTECH_ENABLE - {"PASHR", 3, processPASHR}, /* general handler for Ashtech */ -#endif /* ASHTECH_ENABLE */ -#ifdef OCEANSERVER_ENABLE - {"OHPR", 18, processOHPR}, -#endif /* OCEANSERVER_ENABLE */ - /*@ +nullassign @*/ - }; - - int count; - gps_mask_t retval = 0; - unsigned int i; - char *p, *s, *e; -#ifndef USE_OLD_SPLIT - volatile char *t; -#endif -#ifdef __UNUSED__ - unsigned char sum; - - if (!nmea_checksum(sentence+1, &sum)) { - gpsd_report(LOG_ERROR, "Bad NMEA checksum: '%s' should be %02X\n", - sentence, sum); - return 0; - } -#endif /* __ UNUSED__ */ - - /* - * We've had reports that on the Garmin GPS-10 the device sometimes - * (1:1000 or so) sends garbage packets that have a valid checksum - * but are like 2 successive NMEA packets merged together in one - * with some fields lost. Usually these are much longer than the - * legal limit for NMEA, so we can cope by just tossing out overlong - * packets. This may be a generic bug of all Garmin chipsets. - */ - if (strlen(sentence) > NMEA_MAX) { - gpsd_report(LOG_WARN, "Overlong packet rejected.\n"); - return ONLINE_SET; - } - -#ifdef BREAK_REGRESSIONS - /* trim trailing CR/LF */ - for (i = 0; i < strlen(sentence); i++) - if ((sentence[i] == '\r') || (sentence[i] == '\n')){ - sentence[i] = '\0'; - break; - } -#endif - /*@ -usedef @*//* splint 3.1.1 seems to have a bug here */ - /* make an editable copy of the sentence */ - strncpy((char *)session->driver.nmea.fieldcopy, sentence, NMEA_MAX); - /* discard the checksum part */ - for (p = (char *)session->driver.nmea.fieldcopy; (*p != '*') && (*p >= ' '); ) ++p; - if (*p == '*') - *p++ = ','; /* otherwise we drop the last field */ - *p = '\0'; - e = p; - /* split sentence copy on commas, filling the field array */ -#ifdef USE_OLD_SPLIT - for (count = 0, p = (char *)session->driver.nmea.fieldcopy; p != NULL && *p != '\0'; ++count, p = strchr(p, ',')) { - *p = '\0'; - session->driver.nmea.field[count] = ++p; - } -#else - count = 0; - t = p; /* end of sentence */ - p = (char *)session->driver.nmea.fieldcopy + 1; /* beginning of tag, 'G' not '$' */ - /* while there is a search string and we haven't run off the buffer... */ - while((p != NULL) && (p <= t)){ - session->driver.nmea.field[count] = p; /* we have a field. record it */ - /*@ -compdef @*/ - if ((p = strchr(p, ',')) != NULL){ /* search for the next delimiter */ - *p = '\0'; /* replace it with a NUL */ - count++; /* bump the counters and continue */ - p++; - } - /*@ +compdef @*/ - } -#endif - /* point remaining fields at empty string, just in case */ - for (i = (unsigned int)count; - i < (unsigned)(sizeof(session->driver.nmea.field)/sizeof(session->driver.nmea.field[0])); - i++) - session->driver.nmea.field[i] = e; - /* dispatch on field zero, the sentence tag */ - for (i = 0; i < (unsigned)(sizeof(nmea_phrase)/sizeof(nmea_phrase[0])); ++i) { - s = session->driver.nmea.field[0]; - if (strlen(nmea_phrase[i].name) == 3) - s += 2; /* skip talker ID */ - if (strcmp(nmea_phrase[i].name, s) == 0) { - if (nmea_phrase[i].decoder!=NULL && (count >= nmea_phrase[i].nf)) { - retval = (nmea_phrase[i].decoder)(count, session->driver.nmea.field, session); - strncpy(session->gpsdata.tag, nmea_phrase[i].name, MAXTAGLEN); - session->gpsdata.sentence_length = strlen(sentence); - } else - retval = ONLINE_SET; /* unknown sentence */ - break; - } - } -#ifdef MKT3301_ENABLE - if (strncmp("PMTK", session->driver.nmea.field[0], 4) == 0) /* general handler for MKT3301 vendor specifics */ - retval = processMKT3301(count, session->driver.nmea.field, session); -#endif /* MKT3301_ENABLE */ - /*@ +usedef @*/ - return retval; -} -/*@ +mayaliasunique @*/ -#endif /* NMEA_ENABLE */ - -void nmea_add_checksum(char *sentence) -/* add NMEA checksum to a possibly *-terminated sentence */ -{ - unsigned char sum = '\0'; - char c, *p = sentence; - - if (*p == '$') { - p++; - } else { - gpsd_report(LOG_ERROR, "Bad NMEA sentence: '%s'\n", sentence); - } - while ( ((c = *p) != '*') && (c != '\0')) { - sum ^= c; - p++; - } - *p++ = '*'; - (void)snprintf(p, 5, "%02X\r\n", (unsigned)sum); -} - -ssize_t nmea_write(struct gps_device_t *session, char *buf, size_t len UNUSED) -/* ship a command to the GPS, adding * and correct checksum */ -{ - (void)strlcpy(session->msgbuf, buf, sizeof(session->msgbuf)); - if (session->msgbuf[0] == '$') { - (void)strlcat(session->msgbuf, "*", sizeof(session->msgbuf)); - nmea_add_checksum(session->msgbuf); - } else - (void)strlcat(session->msgbuf, "\r\n", sizeof(session->msgbuf)); - session->msgbuflen = strlen(session->msgbuf); - return gpsd_write(session, session->msgbuf, session->msgbuflen); -} - -ssize_t nmea_send(struct gps_device_t *session, const char *fmt, ... ) -{ - char buf[BUFSIZ]; - va_list ap; - - va_start(ap, fmt) ; - (void)vsnprintf(buf, sizeof(buf)-5, fmt, ap); - va_end(ap); - return nmea_write(session, buf, strlen(buf)); -} diff --git a/rtcm2.c b/rtcm2.c deleted file mode 100644 index 2181cec5..00000000 --- a/rtcm2.c +++ /dev/null @@ -1,739 +0,0 @@ -/* $Id$ */ -/***************************************************************************** - -This is a decoder for RTCM-104 2.x, an obscure and complicated serial -protocol used for broadcasting pseudorange corrections from -differential-GPS reference stations. The applicable -standard is - -RTCM RECOMMENDED STANDARDS FOR DIFFERENTIAL NAVSTAR GPS SERVICE, -RTCM PAPER 194-93/SC 104-STD - -Ordering instructions are accessible from -under "Publications". This describes version 2.1 of the RTCM specification. -RTCM-104 was later incrementally revised up to a 2.3 level before being -completely redesigned as level 3.0. - -Also applicable is ITU-R M.823: "Technical characteristics of -differential transmissions for global navigation satellite systems -from maritime radio beacons in the frequency band 283.5 - 315 kHz in -region 1 and 285 - 325 kHz in regions 2 & 3." - -The RTCM 2.x protocol uses as a transport layer the GPS satellite downlink -protocol described in IS-GPS-200, the Navstar GPS Interface -Specification. This code relies on the lower-level packet-assembly -code for that protocol in isgps.c. - -The lower layer's job is done when it has assembled a message of up to -33 words of clean parity-checked data. At this point this upper layer -takes over. struct rtcm2_msg_t is overlaid on the buffer and the bitfields -are used to extract pieces of it. Those pieces are copied and (where -necessary) reassembled into a struct rtcm2_t. - -This code and the contents of isgps.c are evolved from code by Wolfgang -Rupprecht. Wolfgang's decoder was loosely based on one written by -John Sager in 1999 (in particular the dump function emits a close -descendant of Sager's dump format). Here are John Sager's original -notes: - -The RTCM decoder prints a legible representation of the input data. -The RTCM SC-104 specification is copyrighted, so I cannot -quote it - in fact, I have never read it! Most of the information -used to develop the decoder came from publication ITU-R M.823. -This is a specification of the data transmitted from LF DGPS -beacons in the 300kHz band. M.823 contains most of those parts of -RTCM SC-104 directly relevant to the air interface (there -are one or two annoying and vital omissions!). Information -about the serial interface format was gleaned from studying -the output of a beacon receiver test program made available on -Starlink's website. - -*****************************************************************************/ - -#include -#include -#include -#include -#include -#include -#include /* for round() */ - -#include "gpsd_config.h" -#include "gpsd.h" -#include "rtcm2.h" - -#ifdef RTCM104V2_ENABLE - -#define PREAMBLE_PATTERN 0x66 - -static unsigned int tx_speed[] = { 25, 50, 100, 110, 150, 200, 250, 300 }; - -#define DIMENSION(a) (unsigned)(sizeof(a)/sizeof(a[0])) - -void rtcm2_unpack(/*@out@*/struct rtcm2_t *tp, char *buf) -/* break out the raw bits into the content fields */ -{ - int len; - unsigned int n, w; - struct rtcm2_msg_t *msg = (struct rtcm2_msg_t *)buf; - - tp->type = msg->w1.msgtype; - tp->length = msg->w2.frmlen; - tp->zcount = msg->w2.zcnt * ZCOUNT_SCALE; - tp->refstaid = msg->w1.refstaid; - tp->seqnum = msg->w2.sqnum; - tp->stathlth = msg->w2.stathlth; - - len = (int)tp->length; - n = 0; - switch (tp->type) { - case 1: - case 9: - { - struct b_correction_t *m = &msg->msg_type.type1.corrections[0]; - - while (len >= 0) { - if (len >= 2) { - tp->msg_data.ranges.sat[n].ident = m->w3.satident1; - tp->msg_data.ranges.sat[n].udre = m->w3.udre1; - tp->msg_data.ranges.sat[n].issuedata = m->w4.issuedata1; - tp->msg_data.ranges.sat[n].rangerr = m->w3.pc1 * - (m->w3.scale1 ? PCLARGE : PCSMALL); - tp->msg_data.ranges.sat[n].rangerate = m->w4.rangerate1 * - (m->w3.scale1 ? RRLARGE : RRSMALL); - n++; - } - if (len >= 4) { - tp->msg_data.ranges.sat[n].ident = m->w4.satident2; - tp->msg_data.ranges.sat[n].udre = m->w4.udre2; - tp->msg_data.ranges.sat[n].issuedata = m->w6.issuedata2; - tp->msg_data.ranges.sat[n].rangerr = m->w5.pc2 * - (m->w4.scale2 ? PCLARGE : PCSMALL); - tp->msg_data.ranges.sat[n].rangerate = m->w5.rangerate2 * - (m->w4.scale2 ? RRLARGE : RRSMALL); - n++; - } - if (len >= 5) { - tp->msg_data.ranges.sat[n].ident = m->w6.satident3; - tp->msg_data.ranges.sat[n].udre = m->w6.udre3; - tp->msg_data.ranges.sat[n].issuedata = m->w7.issuedata3; - /*@ -shiftimplementation @*/ - tp->msg_data.ranges.sat[n].rangerr = ((m->w6.pc3_h<<8)|(m->w7.pc3_l)) * - (m->w6.scale3 ? PCLARGE : PCSMALL); - tp->msg_data.ranges.sat[n].rangerate = m->w7.rangerate3 * - (m->w6.scale3 ? RRLARGE : RRSMALL); - /*@ +shiftimplementation @*/ - n++; - } - len -= 5; - m++; - } - tp->msg_data.ranges.nentries = n; - } - break; - case 3: - { - struct rtcm2_msg3 *m = &msg->msg_type.type3; - - if ((tp->msg_data.ecef.valid = len >= 4)) { - tp->msg_data.ecef.x = ((m->w3.x_h<<8)|(m->w4.x_l))*XYZ_SCALE; - tp->msg_data.ecef.y = ((m->w4.y_h<<16)|(m->w5.y_l))*XYZ_SCALE; - tp->msg_data.ecef.z = ((m->w5.z_h<<24)|(m->w6.z_l))*XYZ_SCALE; - } - } - break; - case 4: - if ((tp->msg_data.reference.valid = len >= 2)){ - struct rtcm2_msg4 *m = &msg->msg_type.type4; - - tp->msg_data.reference.system = - (m->w3.dgnss==0) ? gps : - ((m->w3.dgnss==1) ? glonass : unknown); - tp->msg_data.reference.sense = (m->w3.dat != 0) ? global : local; - if (m->w3.datum_alpha_char1){ - tp->msg_data.reference.datum[n++] = (char)(m->w3.datum_alpha_char1); - } - if (m->w3.datum_alpha_char2){ - tp->msg_data.reference.datum[n++] = (char)(m->w3.datum_alpha_char2); - } - if (m->w4.datum_sub_div_char1){ - tp->msg_data.reference.datum[n++] = (char)(m->w4.datum_sub_div_char1); - } - if (m->w4.datum_sub_div_char2){ - tp->msg_data.reference.datum[n++] = (char)(m->w4.datum_sub_div_char2); - } - if (m->w4.datum_sub_div_char3){ - tp->msg_data.reference.datum[n++] = (char)(m->w4.datum_sub_div_char3); - } - tp->msg_data.reference.datum[n++] = '\0'; - if (len >= 4) { - tp->msg_data.reference.dx = m->w5.dx * DXYZ_SCALE; - tp->msg_data.reference.dy = ((m->w5.dy_h << 8) | m->w6.dy_l) * DXYZ_SCALE; - tp->msg_data.reference.dz = m->w6.dz * DXYZ_SCALE; - } else - tp->msg_data.reference.sense = invalid; - } - break; - case 5: - for (n = 0; n < (unsigned)len; n++) { - struct consat_t *csp = &tp->msg_data.conhealth.sat[n]; - struct b_health_t *m = &msg->msg_type.type5.health[n]; - - csp->ident = m->sat_id; - csp->iodl = m->issue_of_data_link!=0; - csp->health = m->data_health; - /*@i@*/csp->snr = (m->cn0?(m->cn0+CNR_OFFSET):SNR_BAD); - csp->health_en = m->health_enable; - csp->new_data = m->new_nav_data!=0; - csp->los_warning = m->loss_warn!=0; - csp->tou = m->time_unhealthy*TU_SCALE; - } - tp->msg_data.conhealth.nentries = n; - break; - case 7: - for (w = 0; w < (unsigned)len; w++) { - struct station_t *np = &tp->msg_data.almanac.station[n]; - struct b_station_t *mp = &msg->msg_type.type7.almanac[w]; - - np->latitude = mp->w3.lat * LA_SCALE; - /*@i@*/np->longitude = ((mp->w3.lon_h << 8) | mp->w4.lon_l) * LO_SCALE; - np->range = mp->w4.range; - np->frequency = (((mp->w4.freq_h << 6) | mp->w5.freq_l) * FREQ_SCALE) + FREQ_OFFSET; - np->health = mp->w5.health; - np->station_id = mp->w5.station_id, - np->bitrate = tx_speed[mp->w5.bit_rate]; - n++; - } - tp->msg_data.almanac.nentries = (unsigned)(len/3); - break; - case 16: - /*@ -boolops @*/ - for (w = 0; w < (unsigned)len; w++){ - if (!msg->msg_type.type16.txt[w].byte1) { - break; - } - tp->msg_data.message[n++] = (char)(msg->msg_type.type16.txt[w].byte1); - if (!msg->msg_type.type16.txt[w].byte2) { - break; - } - tp->msg_data.message[n++] = (char)(msg->msg_type.type16.txt[w].byte2); - if (!msg->msg_type.type16.txt[w].byte3) { - break; - } - tp->msg_data.message[n++] = (char)(msg->msg_type.type16.txt[w].byte3); - } - /*@ +boolops @*/ - tp->msg_data.message[n++] = '\0'; - break; - - default: - memcpy(tp->msg_data.words, msg->msg_type.rtcm2_msgunk, (RTCM2_WORDS_MAX-2)*sizeof(isgps30bits_t)); - break; - } -} - -bool rtcm2_repack(struct rtcm2_t *tp, isgps30bits_t *buf) -/* repack the content fields into the raw bits */ -{ - int len, sval; - unsigned int n, w, uval; - struct rtcm2_msg_t *msg = (struct rtcm2_msg_t *)buf; - struct rtcm2_msghw1 *wp = (struct rtcm2_msghw1 *)buf; - - msg->w1.msgtype = tp->type; - msg->w2.frmlen = tp->length; - msg->w2.zcnt = (unsigned) round(tp->zcount / ZCOUNT_SCALE); - msg->w1.refstaid = tp->refstaid; - msg->w2.sqnum = tp->seqnum; - msg->w2.stathlth = tp->stathlth; - - len = (int)tp->length; - n = 0; - switch (tp->type) { - case 1: /* S */ - case 9: - { - struct b_correction_t *m = &msg->msg_type.type1.corrections[0]; - - while (len >= 0) { - if (len >= 2) { - struct rangesat_t *ssp = &tp->msg_data.ranges.sat[n]; - m->w3.satident1 = ssp->ident; - m->w3.udre1 = ssp->udre; - m->w4.issuedata1 = ssp->issuedata; - m->w3.scale1 = (unsigned)((ssp->rangerr > MAXPCSMALL) || - (ssp->rangerr < (-MAXPCSMALL)) || - (ssp->rangerate > MAXRRSMALL) || - (ssp->rangerate < (-MAXRRSMALL))); - m->w3.pc1 = (int) round(ssp->rangerr / (m->w3.scale1 ? PCLARGE : PCSMALL)); - m->w4.rangerate1 = (int) round(ssp->rangerate / (m->w3.scale1 ? RRLARGE : RRSMALL)); - n++; - } - if (len >= 4) { - struct rangesat_t *ssp = &tp->msg_data.ranges.sat[n]; - m->w4.satident2 = ssp->ident; - m->w4.udre2 = ssp->udre; - m->w6.issuedata2 = ssp->issuedata; - m->w4.scale2 = (unsigned)((ssp->rangerr > MAXPCSMALL) || - (ssp->rangerr < (-MAXPCSMALL)) || - (ssp->rangerate > MAXRRSMALL) || - (ssp->rangerate < (-MAXRRSMALL))); - m->w5.pc2 = (int) round(ssp->rangerr / (m->w4.scale2 ? PCLARGE : PCSMALL)); - m->w5.rangerate2 = (int) round(ssp->rangerate / (m->w4.scale2 ? RRLARGE : RRSMALL)); - n++; - } - if (len >= 5) { - struct rangesat_t *ssp = &tp->msg_data.ranges.sat[n]; - m->w6.satident3 = ssp->ident; - m->w6.udre3 = ssp->udre; - m->w7.issuedata3 = ssp->issuedata; - m->w6.scale3 = (unsigned)((ssp->rangerr > MAXPCSMALL) || - (ssp->rangerr < (-MAXPCSMALL)) || - (ssp->rangerate > MAXRRSMALL) || - (ssp->rangerate < (-MAXRRSMALL))); - sval = (int) round(ssp->rangerr / (m->w6.scale3 ? PCLARGE : PCSMALL)); - /*@ -shiftimplementation @*/ - m->w6.pc3_h = sval >> 8; - /*@ +shiftimplementation @*/ - m->w7.pc3_l = (unsigned)sval & 0xff; - m->w7.rangerate3 = (int) round(ssp->rangerate / (m->w6.scale3 ? RRLARGE : RRSMALL)); - n++; - } - len -= 5; - m++; - } - tp->msg_data.ranges.nentries = n; - } - break; - case 3: /* R */ - if (tp->msg_data.ecef.valid) { - struct rtcm2_msg3 *m = &msg->msg_type.type3; - unsigned x = (unsigned) round(tp->msg_data.ecef.x / XYZ_SCALE); - unsigned y = (unsigned) round(tp->msg_data.ecef.y / XYZ_SCALE); - unsigned z = (unsigned) round(tp->msg_data.ecef.z / XYZ_SCALE); - - m->w4.x_l = x & 0xff; - m->w3.x_h = x >> 8; - m->w5.y_l = y & 0xffff; - m->w4.y_h = y >> 16; - m->w6.z_l = z & 0xffffff; - m->w5.z_h = z >> 24; - } - break; - case 4: /* D */ - if (tp->msg_data.reference.valid) { - struct rtcm2_msg4 *m = &msg->msg_type.type4; - - m->w3.dgnss = tp->msg_data.reference.system; - m->w3.dat = (unsigned)(tp->msg_data.reference.sense == global); - /*@ -predboolothers -type @*/ - if (tp->msg_data.reference.datum[0]) - m->w3.datum_alpha_char1 = tp->msg_data.reference.datum[0]; - else - m->w3.datum_alpha_char1 = 0; - if (tp->msg_data.reference.datum[1]) - m->w3.datum_alpha_char2 = tp->msg_data.reference.datum[1]; - else - m->w3.datum_alpha_char2 = 0; - if (tp->msg_data.reference.datum[2]) - m->w4.datum_sub_div_char1 = tp->msg_data.reference.datum[2]; - else - m->w4.datum_sub_div_char1 = 0; - if (tp->msg_data.reference.datum[3]) - m->w4.datum_sub_div_char2 = tp->msg_data.reference.datum[3]; - else - m->w4.datum_sub_div_char2 = 0; - if (tp->msg_data.reference.datum[4]) - m->w4.datum_sub_div_char3 = tp->msg_data.reference.datum[4]; - else - m->w4.datum_sub_div_char3 = 0; - /*@ +predboolothers +type @*/ - if (tp->msg_data.reference.system != unknown) { - m->w5.dx = (uint)round(tp->msg_data.reference.dx / DXYZ_SCALE); - uval = (uint)round(tp->msg_data.reference.dy / DXYZ_SCALE); - m->w5.dy_h = uval >> 8; - m->w6.dy_l = uval & 0xff; - m->w6.dz = (uint)round(tp->msg_data.reference.dz / DXYZ_SCALE); - } - } - break; - case 5: /* C */ - for (n = 0; n < (unsigned)len; n++) { - struct consat_t *csp = &tp->msg_data.conhealth.sat[n]; - struct b_health_t *m = &msg->msg_type.type5.health[n]; - - m->sat_id = csp->ident; - m->issue_of_data_link = (unsigned)csp->iodl; - m->data_health = csp->health; - m->cn0 = (csp->snr == SNR_BAD) ? 0 : (unsigned)csp->snr-CNR_OFFSET; - m->health_enable = csp->health_en; - m->new_nav_data = (unsigned)csp->new_data; - m->loss_warn = (unsigned)csp->los_warning; - m->time_unhealthy = (unsigned)(csp->tou / TU_SCALE); - } - break; - case 7: /* A */ - for (w = 0; w < (RTCM2_WORDS_MAX - 2)/ 3; w++) { - struct station_t *np = &tp->msg_data.almanac.station[n++]; - struct b_station_t *mp = &msg->msg_type.type7.almanac[w]; - - mp->w3.lat = (int) round(np->latitude / LA_SCALE); - sval = (int) round(np->longitude / LO_SCALE); - /*@ -shiftimplementation @*/ - mp->w3.lon_h = sval >> 8; - /*@ +shiftimplementation @*/ - mp->w4.lon_l = (unsigned)sval & 0xff; - mp->w4.range = np->range; - uval = (unsigned) round(((np->frequency-FREQ_OFFSET) / FREQ_SCALE)); - mp->w4.freq_h = uval >> 6; - mp->w5.freq_l = uval & 0x3f; - mp->w5.health = np->health; - mp->w5.station_id = np->station_id; - mp->w5.bit_rate = 0; - for (uval = 0; uval < (unsigned)(sizeof(tx_speed)/sizeof(tx_speed[0])); uval++) - if (tx_speed[uval] == np->bitrate) { - mp->w5.bit_rate = uval; - break; - } - if (mp->w5.bit_rate == 0) - return false; - } - tp->msg_data.almanac.nentries = n; - break; - case 16: /* T */ - /*@ -boolops @*/ - for (w = 0; w < RTCM2_WORDS_MAX - 2; w++){ - if (!tp->msg_data.message[n]) { - break; - } - msg->msg_type.type16.txt[w].byte1 = (unsigned)tp->msg_data.message[n++]; - if (!tp->msg_data.message[n]) { - break; - } - msg->msg_type.type16.txt[w].byte2 = (unsigned)tp->msg_data.message[n++]; - if (!tp->msg_data.message[n]) { - break; - } - msg->msg_type.type16.txt[w].byte3 = (unsigned)tp->msg_data.message[n++]; - } - msg->w2.frmlen = w+1; - /*@ +boolops @*/ - break; - - default: /* U */ - memcpy(msg->msg_type.rtcm2_msgunk, tp->msg_data.words, (RTCM2_WORDS_MAX-2)*sizeof(isgps30bits_t)); - break; - } - - /* compute parity for each word in the message */ - for (w = 0; w < tp->length; w++) - wp[w].parity = isgps_parity(buf[w]); - - /* FIXME: must do inversion here */ - return true; -} - -static bool preamble_match(isgps30bits_t *w) -{ - return (((struct rtcm2_msghw1 *)w)->preamble == PREAMBLE_PATTERN); -} - -static bool length_check(struct gps_packet_t *lexer) -{ - return lexer->isgps.bufindex >= 2 - && lexer->isgps.bufindex >= ((struct rtcm2_msg_t *)lexer->isgps.buf)->w2.frmlen + 2u; -} - -enum isgpsstat_t rtcm2_decode(struct gps_packet_t *lexer, unsigned int c) -{ - return isgps_decode(lexer, - preamble_match, - length_check, - RTCM2_WORDS_MAX, - c); -} - -void rtcm2_dump(struct rtcm2_t *rtcm, /*@out@*/char buf[], size_t buflen) -/* dump the contents of a parsed RTCM104 message */ -{ - unsigned int n; - - (void)snprintf(buf, buflen, "H\t%u\t%u\t%0.1f\t%u\t%u\t%u\n", - rtcm->type, - rtcm->refstaid, - rtcm->zcount, - rtcm->seqnum, - rtcm->length, - rtcm->stathlth); - - switch (rtcm->type) { - case 1: - case 9: - for (n = 0; n < rtcm->msg_data.ranges.nentries; n++) { - struct rangesat_t *rsp = &rtcm->msg_data.ranges.sat[n]; - (void)snprintf(buf + strlen(buf), buflen - strlen(buf), - "S\t%u\t%u\t%u\t%0.1f\t%0.3f\t%0.3f\n", - rsp->ident, - rsp->udre, - rsp->issuedata, - rtcm->zcount, - rsp->rangerr, - rsp->rangerate); - } - break; - - case 3: - if (rtcm->msg_data.ecef.valid) - (void)snprintf(buf + strlen(buf), buflen - strlen(buf), - "R\t%.2f\t%.2f\t%.2f\n", - rtcm->msg_data.ecef.x, - rtcm->msg_data.ecef.y, - rtcm->msg_data.ecef.z); - break; - - case 4: - if (rtcm->msg_data.reference.valid) - (void)snprintf(buf + strlen(buf), buflen - strlen(buf), - "D\t%s\t%1d\t%s\t%.1f\t%.1f\t%.1f\n", - (rtcm->msg_data.reference.system==gps) ? "GPS" - : ((rtcm->msg_data.reference.system==glonass) ? "GLONASS" - : "UNKNOWN"), - rtcm->msg_data.reference.sense, - rtcm->msg_data.reference.datum, - rtcm->msg_data.reference.dx, - rtcm->msg_data.reference.dy, - rtcm->msg_data.reference.dz); - break; - - case 5: - for (n = 0; n < rtcm->msg_data.conhealth.nentries; n++) { - struct consat_t *csp = &rtcm->msg_data.conhealth.sat[n]; - (void)snprintf(buf + strlen(buf), buflen - strlen(buf), - "C\t%2u\t%1u\t%1u\t%2d\t%1u\t%1u\t%1u\t%2u\n", - csp->ident, - (unsigned)csp->iodl, - (unsigned)csp->health, - csp->snr, - (unsigned)csp->health_en, - (unsigned)csp->new_data, - (unsigned)csp->los_warning, - csp->tou); - } - break; - - case 6: /* NOP msg */ - (void)strlcat(buf, "N\n", buflen); - break; - - case 7: - for (n = 0; n < rtcm->msg_data.almanac.nentries; n++) { - struct station_t *ssp = &rtcm->msg_data.almanac.station[n]; - (void)snprintf(buf + strlen(buf), buflen - strlen(buf), - "A\t%.4f\t%.4f\t%u\t%.1f\t%u\t%u\t%u\n", - ssp->latitude, - ssp->longitude, - ssp->range, - ssp->frequency, - ssp->health, - ssp->station_id, - ssp->bitrate); - } - break; - case 16: - (void)snprintf(buf + strlen(buf), buflen - strlen(buf), - "T\t\"%s\"\n", rtcm->msg_data.message); - break; - - default: - for (n = 0; n < rtcm->length; n++) - (void)snprintf(buf + strlen(buf), buflen - strlen(buf), - "U\t0x%08x\n", rtcm->msg_data.words[n]); - break; - } - - (void)strlcat(buf, ".\n", buflen); -} - -int rtcm2_undump(/*@out@*/struct rtcm2_t *rtcmp, char *buf) -/* merge a line of data into an RTCM structure, return 0 if done */ -{ - int fldcount, v; - unsigned n; - char buf2[BUFSIZ]; /* stdio.h says BUFSIZ=1024. True everywhere? */ - - /*@ -usedef @*/ - switch (rtcmp->type) { - case 0: - fldcount = sscanf(buf, "H\t%u\t%u\t%lf\t%u\t%u\t%u\n", - &rtcmp->type, - &rtcmp->refstaid, - &rtcmp->zcount, - &rtcmp->seqnum, - &rtcmp->length, - &rtcmp->stathlth); - if (fldcount != 6) - return -1; - else - return 1; - //break; - - case 1: - case 9: - { - struct rangesat_t *rsp = &rtcmp->msg_data.ranges.sat[rtcmp->msg_data.ranges.nentries++]; - /* we ignore the third (zcount) field, it's in the parent */ - fldcount = sscanf(buf, - "S\t%u\t%u\t%u\t%*f\t%lf\t%lf\n", - &rsp->ident, - &rsp->udre, - &rsp->issuedata, - &rsp->rangerr, - &rsp->rangerate); - if (fldcount != 5 || (rtcmp->type != 1 && rtcmp->type != 9)) - return (int)(-rtcmp->type-1); - else if (rtcmp->msg_data.ranges.nentries != rtcmp->length*3/5) - return (int)(rtcmp->type+1); - else - return 0; - } - //break; - - case 3: - fldcount = sscanf(buf, - "R\t%lf\t%lf\t%lf\n", - &rtcmp->msg_data.ecef.x, - &rtcmp->msg_data.ecef.y, - &rtcmp->msg_data.ecef.z); - if (fldcount != 3 || rtcmp->type != 3) - return -4; - else { - rtcmp->msg_data.ecef.valid = true; - return 0; - } - //break; - - case 4: - fldcount = sscanf(buf, - "D\t%1023s\t%1d\t%5s\t%lf\t%lf\t%lf\n", - buf2, - &v, - (char *)&rtcmp->msg_data.reference.datum, - &rtcmp->msg_data.reference.dx, - &rtcmp->msg_data.reference.dy, - &rtcmp->msg_data.reference.dz); - if (fldcount != 6 || rtcmp->type != 4) - return -5; - else { - if (strcmp(buf2, "GPS") == 0) - rtcmp->msg_data.reference.system = gps; - else if (strcmp(buf2, "GLONASS") == 0) - rtcmp->msg_data.reference.system = glonass; - else - rtcmp->msg_data.reference.system = unknown; - rtcmp->msg_data.reference.sense = (v == 1) ? global : ((v == 0) ? local : invalid); - rtcmp->msg_data.reference.valid = true; - return 0; - } - //break; - - case 5: - { - struct consat_t *csp = &rtcmp->msg_data.conhealth.sat[rtcmp->msg_data.conhealth.nentries++]; - unsigned int iodl, new_data, los_warning; - - fldcount = sscanf(buf, - "C\t%2u\t%1u\t%1u\t%2d\t%1u\t%1u\t%1u\t%2u\n", - &csp->ident, - &iodl, - &csp->health, - &csp->snr, - &csp->health_en, - &new_data, - &los_warning, - &csp->tou); - csp->iodl = iodl > 0; - csp->new_data = new_data > 0; - csp->los_warning = los_warning > 0; - if (fldcount != 8 || rtcmp->type != 5) - return -6; - else if (rtcmp->msg_data.conhealth.nentries < rtcmp->length) - return 6; - else - return 0; - } - //break; - - case 6: /* NOP msg */ - if (buf[0] != 'N') - return -7; - else - return 0; - //break; - - case 7: - { - struct station_t *ssp = &rtcmp->msg_data.almanac.station[rtcmp->msg_data.almanac.nentries++]; - fldcount = sscanf(buf, - "A\t%lf\t%lf\t%u\t%lf\t%u\t%u\t%u\n", - &ssp->latitude, - &ssp->longitude, - &ssp->range, - &ssp->frequency, - &ssp->health, - &ssp->station_id, - &ssp->bitrate); - if (fldcount != 7 || rtcmp->type != 7) - return 8; - else if (rtcmp->msg_data.almanac.nentries < rtcmp->length/3) - return 8; - else - return 0; - } - //break; - - case 16: - fldcount = sscanf(buf, "T\t\"%[^\"]\"\n", rtcmp->msg_data.message); - if (fldcount != 1) - return 16; - else - return 0; - //break; - - default: - for (n = 0; n < DIMENSION(rtcmp->msg_data.words); n++) - if (rtcmp->msg_data.words[n] == 0) - break; - if (n >= DIMENSION(rtcmp->msg_data.words)) - return 0; - else { - unsigned int u; - fldcount = sscanf(buf, "U\t0x%08x\n", &u); - if (fldcount != 1) - return (int)(-rtcmp->type-1); - else { - rtcmp->msg_data.words[n] = (isgps30bits_t)u; - if (n == rtcmp->length-1) - return 0; - else - return (int)(rtcmp->type+1); - } - } - //break; - } - /*@ +usedef @*/ -} - -#ifdef __UNUSED__ -void rtcm2_output_magnavox(isgps30bits_t *ip, FILE *fp) -/* ship an RTCM message in the format emitted by Magnavox DGPS receivers */ -{ - static uint sqnum = 0; - - ((struct rtcm2_msg_t *) ip)->w2.sqnum = sqnum++; - sqnum &= 0x7; - - isgps_output_magnavox(ip, ((struct rtcm2_msg_t *) ip)->w2.frmlen + 2, fp); -} -#endif /* __UNUSED__ */ - -#endif /* RTCM104V2_ENABLE */ diff --git a/rtcm2.h b/rtcm2.h deleted file mode 100644 index fe2fdfe4..00000000 --- a/rtcm2.h +++ /dev/null @@ -1,497 +0,0 @@ -/* $Id$ */ -#ifndef _GPSD_RTCM2_H_ -#define _GPSD_RTCM2_H_ - -/***************************************************************************** - -This is a decoder for RTCM-104 2.x, an obscure and complicated serial -protocol used for broadcasting pseudorange corrections from -differential-GPS reference stations. The applicable -standard is - -RTCM RECOMMENDED STANDARDS FOR DIFFERENTIAL NAVSTAR GPS SERVICE, -RTCM PAPER 194-93/SC 104-STD - -Ordering instructions are accessible from -under "Publications". This describes version 2.1 of the RTCM specification. -RTCM-104 was later incrementally revised up to a 2.3 level before being -completely redesigned as level 3.0. - -Also applicable is ITU-R M.823: "Technical characteristics of -differential transmissions for global navigation satellite systems -from maritime radio beacons in the frequency band 283.5 - 315 kHz in -region 1 and 285 - 325 kHz in regions 2 & 3." - -The RTCM 2.x protocol uses as a transport layer the GPS satellite -downlink protocol described in IS-GPS-200, the Navstar GPS Interface -Specification. This code relies on the lower-level packet-assembly -code for that protocol in isgps.c. - -The lower layer's job is done when it has assembled a message of up to -33 words of clean parity-checked data. At this point this upper layer -takes over. struct rtcm2_msg_t is overlaid on the buffer and the bitfields -are used to extract pieces of it. Those pieces are copied and (where -necessary) reassembled into a struct rtcm2_t. - -This code and the contents of isgps.c are evolved from code by Wolfgang -Rupprecht. Wolfgang's decoder was loosely based on one written by -John Sager in 1999 (in particular the dump function emits a close -descendant of Sager's dump format). Here are John Sager's original -notes: - -The RTCM decoder prints a legible representation of the input data. -The RTCM SC-104 specification is copyrighted, so I cannot -quote it - in fact, I have never read it! Most of the information -used to develop the decoder came from publication ITU-R M.823. -This is a specification of the data transmitted from LF DGPS -beacons in the 300kHz band. M.823 contains most of those parts of -RTCM SC-104 directly relevant to the air interface (there -are one or two annoying and vital omissions!). Information -about the serial interface format was gleaned from studying -the output of a beacon receiver test program made available on -Starlink's website. - -*****************************************************************************/ - -/* - * Structures for interpreting words in an RTCM-104 2.x message (after - * parity checking and removing inversion). Note, these structures - * are overlayed on the raw data in order to decode them into - * bitfields; this will fail horribly if your C compiler ever - * introduces padding between or before bit fields, or between - * 8-bit-aligned bitfields and character arrays. - * - * (In practice, the only class of machines on which this is likely - * to fail are word-aligned architectures without barrel shifters. - * Very few of these are left in 2008.) - * - * The RTCM 2.1 standard is less explicit than it should be about signed-integer - * representations. Two's complement is specified for prc and rrc (msg1wX), - * but not everywhere. - */ - -#define ZCOUNT_SCALE 0.6 /* sec */ -#define PCSMALL 0.02 /* meters */ -#define PCLARGE 0.32 /* meters */ -#define RRSMALL 0.002 /* meters/sec */ -#define RRLARGE 0.032 /* meters/sec */ - -#define MAXPCSMALL (0x7FFF * PCSMALL) /* 16-bits signed */ -#define MAXRRSMALL (0x7F * RRSMALL) /* 8-bits signed */ - -#define XYZ_SCALE 0.01 /* meters */ -#define DXYZ_SCALE 0.1 /* meters */ -#define LA_SCALE (90.0/32767.0) /* degrees */ -#define LO_SCALE (180.0/32767.0) /* degrees */ -#define FREQ_SCALE 0.1 /* kHz */ -#define FREQ_OFFSET 190.0 /* kHz */ -#define CNR_OFFSET 24 /* dB */ -#define TU_SCALE 5 /* minutes */ - -#pragma pack(1) - -#ifndef WORDS_BIGENDIAN /* little-endian, like x86 */ - -struct rtcm2_msg_t { - struct rtcm2_msghw1 { /* header word 1 */ - uint parity:6; - uint refstaid:10; /* reference station ID */ - uint msgtype:6; /* RTCM message type */ - uint preamble:8; /* fixed at 01100110 */ - uint _pad:2; - } w1; - - struct rtcm2_msghw2 { /* header word 2 */ - uint parity:6; - uint stathlth:3; /* station health */ - uint frmlen:5; - uint sqnum:3; - uint zcnt:13; - uint _pad:2; - } w2; - - union { - /* msg 1 - differential gps corrections */ - struct rtcm2_msg1 { - struct b_correction_t { - struct { /* msg 1 word 3 */ - uint parity:6; - int pc1:16; - uint satident1:5; /* satellite ID */ - uint udre1:2; - uint scale1:1; - uint _pad:2; - } w3; - - struct { /* msg 1 word 4 */ - uint parity:6; - uint satident2:5; /* satellite ID */ - uint udre2:2; - uint scale2:1; - uint issuedata1:8; - int rangerate1:8; - uint _pad:2; - } w4; - - struct { /* msg 1 word 5 */ - uint parity:6; - int rangerate2:8; - int pc2:16; - uint _pad:2; - } w5; - - struct { /* msg 1 word 6 */ - uint parity:6; - int pc3_h:8; - uint satident3:5; /* satellite ID */ - uint udre3:2; - uint scale3:1; - uint issuedata2:8; - uint _pad:2; - } w6; - - struct { /* msg 1 word 7 */ - uint parity:6; - uint issuedata3:8; - int rangerate3:8; - uint pc3_l:8; /* NOTE: uint for low byte */ - uint _pad:2; - } w7; - } corrections[(RTCM2_WORDS_MAX - 2) / 5]; - } type1; - - /* msg 3 - reference station parameters */ - struct rtcm2_msg3 { - struct { - uint parity:6; - uint x_h:24; - uint _pad:2; - } w3; - struct { - uint parity:6; - uint y_h:16; - uint x_l:8; - uint _pad:2; - } w4; - struct { - uint parity:6; - uint z_h:8; - uint y_l:16; - uint _pad:2; - } w5; - - struct { - uint parity:6; - uint z_l:24; - uint _pad:2; - } w6; - } type3; - - /* msg 4 - reference station datum */ - struct rtcm2_msg4 { - struct { - uint parity:6; - uint datum_alpha_char2:8; - uint datum_alpha_char1:8; - uint spare:4; - uint dat:1; - uint dgnss:3; - uint _pad:2; - } w3; - struct { - uint parity:6; - uint datum_sub_div_char2:8; - uint datum_sub_div_char1:8; - uint datum_sub_div_char3:8; - uint _pad:2; - } w4; - struct { - uint parity:6; - uint dy_h:8; - uint dx:16; - uint _pad:2; - } w5; - struct { - uint parity:6; - uint dz:24; - uint dy_l:8; - uint _pad:2; - } w6; - } type4; - - /* msg 5 - constellation health */ - struct rtcm2_msg5 { - struct b_health_t { - uint parity:6; - uint unassigned:2; - uint time_unhealthy:4; - uint loss_warn:1; - uint new_nav_data:1; - uint health_enable:1; - uint cn0:5; - uint data_health:3; - uint issue_of_data_link:1; - uint sat_id:5; - uint reserved:1; - uint _pad:2; - } health[MAXHEALTH]; - } type5; - - /* msg 6 - null message */ - - /* msg 7 - beacon almanac */ - struct rtcm2_msg7 { - struct b_station_t { - struct { - uint parity:6; - int lon_h:8; - int lat:16; - uint _pad:2; - } w3; - struct { - uint parity:6; - uint freq_h:6; - uint range:10; - uint lon_l:8; - uint _pad:2; - } w4; - struct { - uint parity:6; - uint encoding:1; - uint sync_type:1; - uint mod_mode:1; - uint bit_rate:3; - /* - * ITU-R M.823-2 page 9 and RTCM-SC104 v2.1 pages - * 4-21 and 4-22 are in conflict over the next two - * field sizes. ITU says 9+3, RTCM says 10+2. - * The latter correctly decodes the USCG station - * id's so I'll use that one here. -wsr - */ - uint station_id:10; - uint health:2; - uint freq_l:6; - uint _pad:2; - } w5; - } almanac[(RTCM2_WORDS_MAX - 2)/3]; - } type7; - - /* msg 16 - text msg */ - struct rtcm2_msg16 { - struct { - uint parity:6; - uint byte3:8; - uint byte2:8; - uint byte1:8; - uint _pad:2; - } txt[RTCM2_WORDS_MAX-2]; - } type16; - - /* unknown message */ - isgps30bits_t rtcm2_msgunk[RTCM2_WORDS_MAX-2]; - } msg_type; -}; - -#endif /* LITTLE_ENDIAN */ - -#ifdef WORDS_BIGENDIAN -/* This struct was generated from the above using invert-bitfields.pl */ -#ifndef S_SPLINT_S /* splint thinks it's a duplicate definition */ - -struct rtcm2_msg_t { - struct rtcm2_msghw1 { /* header word 1 */ - uint _pad:2; - uint preamble:8; /* fixed at 01100110 */ - uint msgtype:6; /* RTCM message type */ - uint refstaid:10; /* reference station ID */ - uint parity:6; - } w1; - - struct rtcm2_msghw2 { /* header word 2 */ - uint _pad:2; - uint zcnt:13; - uint sqnum:3; - uint frmlen:5; - uint stathlth:3; /* station health */ - uint parity:6; - } w2; - - union { - /* msg 1 - differential gps corrections */ - struct rtcm2_msg1 { - struct b_correction_t { - struct { /* msg 1 word 3 */ - uint _pad:2; - uint scale1:1; - uint udre1:2; - uint satident1:5; /* satellite ID */ - int pc1:16; - uint parity:6; - } w3; - - struct { /* msg 1 word 4 */ - uint _pad:2; - int rangerate1:8; - uint issuedata1:8; - uint scale2:1; - uint udre2:2; - uint satident2:5; /* satellite ID */ - uint parity:6; - } w4; - - struct { /* msg 1 word 5 */ - uint _pad:2; - int pc2:16; - int rangerate2:8; - uint parity:6; - } w5; - - struct { /* msg 1 word 6 */ - uint _pad:2; - uint issuedata2:8; - uint scale3:1; - uint udre3:2; - uint satident3:5; /* satellite ID */ - int pc3_h:8; - uint parity:6; - } w6; - - struct { /* msg 1 word 7 */ - uint _pad:2; - uint pc3_l:8; /* NOTE: uint for low byte */ - int rangerate3:8; - uint issuedata3:8; - uint parity:6; - } w7; - } corrections[(RTCM2_WORDS_MAX - 2) / 5]; - } type1; - - /* msg 3 - reference station parameters */ - struct rtcm2_msg3 { - struct { - uint _pad:2; - uint x_h:24; - uint parity:6; - } w3; - struct { - uint _pad:2; - uint x_l:8; - uint y_h:16; - uint parity:6; - } w4; - struct { - uint _pad:2; - uint y_l:16; - uint z_h:8; - uint parity:6; - } w5; - - struct { - uint _pad:2; - uint z_l:24; - uint parity:6; - } w6; - } type3; - - /* msg 4 - reference station datum */ - struct rtcm2_msg4 { - struct { - uint _pad:2; - uint dgnss:3; - uint dat:1; - uint spare:4; - uint datum_alpha_char1:8; - uint datum_alpha_char2:8; - uint parity:6; - } w3; - struct { - uint _pad:2; - uint datum_sub_div_char3:8; - uint datum_sub_div_char1:8; - uint datum_sub_div_char2:8; - uint parity:6; - } w4; - struct { - uint _pad:2; - uint dx:16; - uint dy_h:8; - uint parity:6; - } w5; - struct { - uint _pad:2; - uint dy_l:8; - uint dz:24; - uint parity:6; - } w6; - } type4; - - /* msg 5 - constellation health */ - struct rtcm2_msg5 { - struct b_health_t { - uint _pad:2; - uint reserved:1; - uint sat_id:5; - uint issue_of_data_link:1; - uint data_health:3; - uint cn0:5; - uint health_enable:1; - uint new_nav_data:1; - uint loss_warn:1; - uint time_unhealthy:4; - uint unassigned:2; - uint parity:6; - } health[MAXHEALTH]; - } type5; - - /* msg 6 - null message */ - - /* msg 7 - beacon almanac */ - struct rtcm2_msg7 { - struct b_station_t { - struct { - uint _pad:2; - int lat:16; - int lon_h:8; - uint parity:6; - } w3; - struct { - uint _pad:2; - uint lon_l:8; - uint range:10; - uint freq_h:6; - uint parity:6; - } w4; - struct { - uint _pad:2; - uint freq_l:6; - uint health:2; - uint station_id:10; - /* see comments in LE struct above. */ - uint bit_rate:3; - uint mod_mode:1; - uint sync_type:1; - uint encoding:1; - uint parity:6; - } w5; - } almanac[(RTCM2_WORDS_MAX - 2)/3]; - } type7; - - /* msg 16 - text msg */ - struct rtcm2_msg16 { - struct { - uint _pad:2; - uint byte1:8; - uint byte2:8; - uint byte3:8; - uint parity:6; - } txt[RTCM2_WORDS_MAX-2]; - } type16; - - /* unknown message */ - isgps30bits_t rtcm2_msgunk[RTCM2_WORDS_MAX-2]; - } msg_type; -}; - -#endif /* S_SPLINT_S */ -#endif /* BIG ENDIAN */ -#endif /* _GPSD_RTCM2_H_ */ diff --git a/rtcm3.c b/rtcm3.c deleted file mode 100644 index 807816de..00000000 --- a/rtcm3.c +++ /dev/null @@ -1,473 +0,0 @@ -/***************************************************************************** - -This is a decoder for RTCM-104 3.x, a serial protocol used for -broadcasting pseudorange corrections from differential-GPS reference -stations. The applicable specification is RTCM 10403.1: RTCM Paper -177-2006-SC104-STD. This obsolesces the esrlier RTCM-104 2.x -specifications. The specification document is proprietary; ordering -instructions are accessible from -under "Publications". - -Unike the RTCM 2.x protocol, RTCM3.x does not use the strange -sliding-bit-window IS-GPS-200 protocol as a transport layer, but is a -self-contained byte-oriented packet protocol. Packet recognition is -handled in the GPSD packet-getter state machine; this code is -concerned with unpacking the packets into well-behaved C structures, -coping with odd field lengths and fields that may overlap byte -boudaries. These report structures live in gps.h. - -Note that the unpacking this module does is probably useful only for -RTCM reporting and diagnostic tools. It is not necessary when -passing RTCM corrections to a GPS, which normally should just be -passed an entire correction packet for processing by their internal -firmware. - -*****************************************************************************/ - -#include -#include -#include -#include -#include -#include -#include -#include /* for ntohl(3) and friends */ - -#include "gpsd_config.h" -#include "gpsd.h" -#include "bits.h" - -#ifdef RTCM104V3_ENABLE - -/* scaling constants for RTCM3 real number types */ -#define PSEUDORANGE_RESOLUTION 0.2 /* DF011 */ -#define PSEUDORANGE_DIFF_RESOLUTION 0.0005 /* DF012 */ -#define CARRIER_NOISE_RATIO_UNITS 0.25 /* DF015 */ -#define ANTENNA_POSITION_RESOLUTION 0.0001 /* DF025-027 */ -#define ANTENNA_DEGREE_RESOLUTION 25e-6 /* DF062 */ -#define GPS_EPOCH_TIME_RESOLUTION 0.1 /* DF065 */ -#define PHASE_CORRECTION_RESOLUTION 0.5 /* DF069-070 */ - -/* Other magic values */ -#define INVALID_PSEUDORANGE 0x80000 /* DF012 */ - -/*@ -type @*/ /* re-enable when we're ready to take this live */ - -void rtcm3_unpack(/*@out@*/struct rtcm3_t *rtcm, char *buf) -/* break out the raw bits into the scaled report-structure fields */ -{ - unsigned int bitcount = 0; - unsigned i; - signed long temp; - - /*@ -evalorder -sefparams @*/ -#define ugrab(width) (bitcount += width, ubits(buf, bitcount-width, width)) -#define sgrab(width) (bitcount += width, sbits(buf, bitcount-width, width)) - assert(ugrab(8) == 0xD3); - assert(ugrab(6) == 0x00); - - rtcm->length = (uint)ugrab(10); - rtcm->type = (uint)ugrab(12); - - switch(rtcm->type) { - case 1001: - rtcm->rtcmtypes.rtcm3_1001.header.msgnum = (uint)ugrab(12); - rtcm->rtcmtypes.rtcm3_1001.header.station_id = (uint)ugrab(12); - rtcm->rtcmtypes.rtcm3_1001.header.tow = (time_t)ugrab(30); - rtcm->rtcmtypes.rtcm3_1001.header.sync = (bool)ugrab(1); - rtcm->rtcmtypes.rtcm3_1001.header.satcount = (ushort)ugrab(5); - rtcm->rtcmtypes.rtcm3_1001.header.smoothing = (bool)ugrab(1); - rtcm->rtcmtypes.rtcm3_1001.header.interval = (ushort)ugrab(3); - for (i = 0; i < rtcm->rtcmtypes.rtcm3_1001.header.satcount; i++) { - rtcm->rtcmtypes.rtcm3_1001.rtk_data[i].ident = (ushort)ugrab(6); - rtcm->rtcmtypes.rtcm3_1001.rtk_data[i].L1.indicator = (unsigned char)ugrab(1); - temp = (long)sgrab(24); - if (temp == INVALID_PSEUDORANGE) - rtcm->rtcmtypes.rtcm3_1001.rtk_data[i].L1.pseudorange = 0; - else - rtcm->rtcmtypes.rtcm3_1001.rtk_data[i].L1.pseudorange = temp * PSEUDORANGE_RESOLUTION; - if (temp == INVALID_PSEUDORANGE) - rtcm->rtcmtypes.rtcm3_1001.rtk_data[i].L1.rangediff = 0; - else - rtcm->rtcmtypes.rtcm3_1001.rtk_data[i].L1.rangediff = temp * PSEUDORANGE_DIFF_RESOLUTION; - rtcm->rtcmtypes.rtcm3_1001.rtk_data[i].L1.locktime = (unsigned char)sgrab(7); - } - break; - - case 1002: - rtcm->rtcmtypes.rtcm3_1002.header.msgnum = (uint)ugrab(12); - rtcm->rtcmtypes.rtcm3_1002.header.station_id = (uint)ugrab(12); - rtcm->rtcmtypes.rtcm3_1002.header.tow = (time_t)ugrab(30); - rtcm->rtcmtypes.rtcm3_1002.header.sync = (bool)ugrab(1); - rtcm->rtcmtypes.rtcm3_1002.header.satcount = (ushort)ugrab(5); - rtcm->rtcmtypes.rtcm3_1002.header.smoothing = (bool)ugrab(1); - rtcm->rtcmtypes.rtcm3_1002.header.interval = (ushort)ugrab(3); - for (i = 0; i < rtcm->rtcmtypes.rtcm3_1002.header.satcount; i++) { - rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].ident = (ushort)ugrab(6); - rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.indicator = (unsigned char)ugrab(1); - temp = (long)sgrab(24); - if (temp == INVALID_PSEUDORANGE) - rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.pseudorange = 0; - else - rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.pseudorange = temp * PSEUDORANGE_RESOLUTION; - if (temp == INVALID_PSEUDORANGE) - rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.rangediff = 0; - else - rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.rangediff = temp * PSEUDORANGE_DIFF_RESOLUTION; - rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.locktime = (unsigned char)sgrab(7); - rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.ambiguity = (bool)ugrab(8); - rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.CNR = (bool)ugrab(8) * CARRIER_NOISE_RATIO_UNITS; - } - break; - - case 1003: - rtcm->rtcmtypes.rtcm3_1003.header.msgnum = (uint)ugrab(12); - rtcm->rtcmtypes.rtcm3_1003.header.station_id = (uint)ugrab(12); - rtcm->rtcmtypes.rtcm3_1003.header.tow = (time_t)ugrab(30); - rtcm->rtcmtypes.rtcm3_1003.header.sync = (bool)ugrab(1); - rtcm->rtcmtypes.rtcm3_1003.header.satcount = (ushort)ugrab(5); - rtcm->rtcmtypes.rtcm3_1003.header.smoothing = (bool)ugrab(1); - rtcm->rtcmtypes.rtcm3_1003.header.interval = (ushort)ugrab(3); - for (i = 0; i < rtcm->rtcmtypes.rtcm3_1003.header.satcount; i++) { - rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].ident = (ushort)ugrab(6); - rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L1.indicator = (unsigned char)ugrab(1); - temp = (long)sgrab(24); - if (temp == INVALID_PSEUDORANGE) - rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L1.pseudorange = 0; - else - rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L1.pseudorange = temp * PSEUDORANGE_RESOLUTION; - if (temp == INVALID_PSEUDORANGE) - rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L1.rangediff = 0; - else - rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L1.rangediff = temp * PSEUDORANGE_DIFF_RESOLUTION; - rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L1.locktime = (unsigned char)sgrab(7); - rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L2.indicator = (unsigned char)ugrab(2); - temp = (long)sgrab(24); - if (temp == INVALID_PSEUDORANGE) - rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L2.pseudorange = 0; - else - rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L2.pseudorange = temp * PSEUDORANGE_RESOLUTION; - if (temp == INVALID_PSEUDORANGE) - rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L2.rangediff = 0; - else - rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L2.rangediff = temp * PSEUDORANGE_DIFF_RESOLUTION; - rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L2.locktime = (unsigned char)sgrab(7); - } - break; - - case 1004: - rtcm->rtcmtypes.rtcm3_1004.header.msgnum = (uint)ugrab(12); - rtcm->rtcmtypes.rtcm3_1004.header.station_id = (uint)ugrab(12); - rtcm->rtcmtypes.rtcm3_1004.header.tow = (time_t)ugrab(30); - rtcm->rtcmtypes.rtcm3_1004.header.sync = (bool)ugrab(1); - rtcm->rtcmtypes.rtcm3_1004.header.satcount = (ushort)ugrab(5); - rtcm->rtcmtypes.rtcm3_1004.header.smoothing = (bool)ugrab(1); - rtcm->rtcmtypes.rtcm3_1004.header.interval = (ushort)ugrab(3); - for (i = 0; i < rtcm->rtcmtypes.rtcm3_1004.header.satcount; i++) { - rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].ident = (ushort)ugrab(6); - rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L1.indicator = (bool)ugrab(1); - temp = (long)sgrab(24); - if (temp == INVALID_PSEUDORANGE) - rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L1.pseudorange = 0; - else - rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L1.pseudorange = temp * PSEUDORANGE_RESOLUTION; - if (temp == INVALID_PSEUDORANGE) - rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L1.rangediff = 0; - else - rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L1.rangediff = temp * PSEUDORANGE_DIFF_RESOLUTION; - rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L1.locktime = (unsigned char)sgrab(7); - rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L1.ambiguity = (bool)ugrab(8); - rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L1.CNR = (bool)ugrab(8) * CARRIER_NOISE_RATIO_UNITS; - rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.indicator = (unsigned char)ugrab(2); - temp = (long)sgrab(24); - if (temp == INVALID_PSEUDORANGE) - rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.pseudorange = 0; - else - rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.pseudorange = temp * PSEUDORANGE_RESOLUTION; - if (temp == INVALID_PSEUDORANGE) - rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.rangediff = 0; - else - rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.rangediff = temp * PSEUDORANGE_DIFF_RESOLUTION; - rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.locktime = (unsigned char)sgrab(7); - rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.ambiguity = (bool)ugrab(8); - rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.CNR = (bool)ugrab(8) * CARRIER_NOISE_RATIO_UNITS; - } - break; - - case 1005: - rtcm->rtcmtypes.rtcm3_1005.station_id = (unsigned short)ugrab(12); - ugrab(6); /* reserved */ - if ((bool)ugrab(1)) - rtcm->rtcmtypes.rtcm3_1005.system = gps; - else if ((bool)ugrab(1)) - rtcm->rtcmtypes.rtcm3_1005.system = glonass; - else if ((bool)ugrab(1)) - rtcm->rtcmtypes.rtcm3_1005.system = galileo; - rtcm->rtcmtypes.rtcm3_1005.reference_station = (bool)ugrab(1); - rtcm->rtcmtypes.rtcm3_1005.ecef_x = sgrab(38) * ANTENNA_POSITION_RESOLUTION; - rtcm->rtcmtypes.rtcm3_1005.single_receiver = ugrab(1); - ugrab(1); - rtcm->rtcmtypes.rtcm3_1005.ecef_y = sgrab(38) * ANTENNA_POSITION_RESOLUTION; - ugrab(2); - rtcm->rtcmtypes.rtcm3_1005.ecef_z = sgrab(38) * ANTENNA_POSITION_RESOLUTION; - break; - - case 1006: - rtcm->rtcmtypes.rtcm3_1006.station_id = (unsigned short)ugrab(12); - ugrab(6); /* reserved */ - if ((bool)ugrab(1)) - rtcm->rtcmtypes.rtcm3_1006.system = gps; - else if ((bool)ugrab(1)) - rtcm->rtcmtypes.rtcm3_1006.system = glonass; - else if ((bool)ugrab(1)) - rtcm->rtcmtypes.rtcm3_1006.system = galileo; - rtcm->rtcmtypes.rtcm3_1006.reference_station = (bool)ugrab(1); - rtcm->rtcmtypes.rtcm3_1006.ecef_x = sgrab(38) * ANTENNA_POSITION_RESOLUTION; - rtcm->rtcmtypes.rtcm3_1006.single_receiver = ugrab(1); - ugrab(1); - rtcm->rtcmtypes.rtcm3_1006.ecef_y = sgrab(38) * ANTENNA_POSITION_RESOLUTION; - ugrab(2); - rtcm->rtcmtypes.rtcm3_1006.ecef_z = sgrab(38) * ANTENNA_POSITION_RESOLUTION; - rtcm->rtcmtypes.rtcm3_1006.height = ugrab(16) * ANTENNA_POSITION_RESOLUTION; - break; - - case 1007: - break; - - case 1008: - break; - - case 1009: - break; - - case 1010: - break; - - case 1011: - break; - - case 1012: - break; - - case 1013: - break; - - case 1014: - break; - - case 1015: - break; - - case 1016: - break; - - case 1017: - break; - - case 1018: - break; - - case 1019: - break; - - case 1020: - break; - - case 1029: - break; - } -#undef sgrab -#undef ugrab - /*@ +evalorder +sefparams @*/ -} - -void rtcm3_dump(struct rtcm3_t *rtcm, FILE *fp) -/* dump the contents of a parsed RTCM104 message */ -{ - int i; - - char *systems[] = {"GPS", "Glonass", "Galileo", "unknown"}; - - (void)fprintf(fp, "%u (%u):\n", rtcm->type, rtcm->length); - -#define BOOL(c) (c!=0 ? 't' : 'f') -#define CODE(x) (unsigned int)(x) -#define INT(x) (unsigned int)(x) - switch(rtcm->type) { - case 1001: - (void)fprintf(fp, - " #%u station_id=%u, tow=%d sync=%c smoothing=%c interval=%u satcount=%u", - rtcm->rtcmtypes.rtcm3_1001.header.msgnum, - rtcm->rtcmtypes.rtcm3_1001.header.station_id, - (int)rtcm->rtcmtypes.rtcm3_1001.header.tow, - BOOL(rtcm->rtcmtypes.rtcm3_1001.header.sync), - BOOL(rtcm->rtcmtypes.rtcm3_1001.header.smoothing), - rtcm->rtcmtypes.rtcm3_1001.header.interval, - rtcm->rtcmtypes.rtcm3_1001.header.satcount); - for (i = 0; i < rtcm->rtcmtypes.rtcm3_1001.header.satcount; i++) { - (void)fprintf(fp, - " ident=%u\n L1: ind=%u prange=%8.1f delta=%6.4f lockt=%u\n", - rtcm->rtcmtypes.rtcm3_1001.rtk_data[i].ident, - CODE(rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L1.indicator), - rtcm->rtcmtypes.rtcm3_1001.rtk_data[i].L1.pseudorange, - rtcm->rtcmtypes.rtcm3_1001.rtk_data[i].L1.rangediff, - INT(rtcm->rtcmtypes.rtcm3_1001.rtk_data[i].L1.locktime)); - } - break; - - case 1002: - (void)fprintf(fp, - " #%u station_id=%u, tow=%d sync=%c smoothing=%c interval=%u satcount=%u", - rtcm->rtcmtypes.rtcm3_1002.header.msgnum, - rtcm->rtcmtypes.rtcm3_1002.header.station_id, - (int)rtcm->rtcmtypes.rtcm3_1002.header.tow, - BOOL(rtcm->rtcmtypes.rtcm3_1002.header.sync), - BOOL(rtcm->rtcmtypes.rtcm3_1002.header.smoothing), - rtcm->rtcmtypes.rtcm3_1002.header.interval, - rtcm->rtcmtypes.rtcm3_1002.header.satcount); - for (i = 0; i < rtcm->rtcmtypes.rtcm3_1002.header.satcount; i++) { - (void)fprintf(fp, - " ident=%u\n L1: ind=%u prange=%8.1f delta=%6.4f lockt=%u amb=%u CNR=%.2f\n", - rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].ident, - CODE(rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L1.indicator), - rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.pseudorange, - rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.rangediff, - INT(rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.locktime), - INT(rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.ambiguity), - rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.CNR); - } - break; - - case 1003: - (void)fprintf(fp, - " #%u station_id=%u, tow=%d sync=%c smoothing=%c interval=%u satcount=%u", - rtcm->rtcmtypes.rtcm3_1003.header.msgnum, - rtcm->rtcmtypes.rtcm3_1003.header.station_id, - (int)rtcm->rtcmtypes.rtcm3_1003.header.tow, - BOOL(rtcm->rtcmtypes.rtcm3_1003.header.sync), - BOOL(rtcm->rtcmtypes.rtcm3_1003.header.smoothing), - rtcm->rtcmtypes.rtcm3_1003.header.interval, - rtcm->rtcmtypes.rtcm3_1003.header.satcount); - for (i = 0; i < rtcm->rtcmtypes.rtcm3_1003.header.satcount; i++) { - (void)fprintf(fp, - " ident=%u\n L1: ind=%u prange=%8.1f delta=%6.4f lockt=%u\n L2: ind=%u prange=%8.1f delta=%6.4f lockt=%u\n", - rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].ident, - CODE(rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L1.indicator), - rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L1.pseudorange, - rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L1.rangediff, - INT(rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L1.locktime), - CODE(rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L2.indicator), - rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L2.pseudorange, - rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L2.rangediff, - INT(rtcm->rtcmtypes.rtcm3_1003.rtk_data[i].L2.locktime)); - } - break; - - case 1004: - (void)fprintf(fp, - " #%u station_id=%u, tow=%d sync=%c smoothing=%c interval=%u satcount=%u\n", - rtcm->rtcmtypes.rtcm3_1004.header.msgnum, - rtcm->rtcmtypes.rtcm3_1004.header.station_id, - (int)rtcm->rtcmtypes.rtcm3_1004.header.tow, - BOOL(rtcm->rtcmtypes.rtcm3_1004.header.sync), - BOOL(rtcm->rtcmtypes.rtcm3_1004.header.smoothing), - rtcm->rtcmtypes.rtcm3_1004.header.interval, - rtcm->rtcmtypes.rtcm3_1004.header.satcount); - for (i = 0; i < rtcm->rtcmtypes.rtcm3_1004.header.satcount; i++) { - (void)fprintf(fp, - " ident=%u\n L1: ind=%u prange=%8.1f delta=%6.4f lockt=%u amb=%u CNR=%.2f\n L2: ind=%u prange=%8.1f delta=%6.4f lockt=%u amb=%u CNR=%.2f\n", - rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].ident, - CODE(rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L1.indicator), - rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L1.pseudorange, - rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L1.rangediff, - INT(rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L1.locktime), - INT(rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.ambiguity), - rtcm->rtcmtypes.rtcm3_1002.rtk_data[i].L1.CNR, - CODE(rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.indicator), - rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.pseudorange, - rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.rangediff, - INT(rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.locktime), - INT(rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.ambiguity), - rtcm->rtcmtypes.rtcm3_1004.rtk_data[i].L2.CNR); - } - break; - - case 1005: - (void)fprintf(fp, - " station_id=%u, %s refstation=%c sro=%c x=%f y=%f z=%f\n", - rtcm->rtcmtypes.rtcm3_1005.station_id, - systems[rtcm->rtcmtypes.rtcm3_1005.system], - BOOL(rtcm->rtcmtypes.rtcm3_1005.reference_station), - BOOL(rtcm->rtcmtypes.rtcm3_1005.single_receiver), - rtcm->rtcmtypes.rtcm3_1005.ecef_x, - rtcm->rtcmtypes.rtcm3_1005.ecef_y, - rtcm->rtcmtypes.rtcm3_1005.ecef_z); - break; - - case 1006: - (void)fprintf(fp, - " station_id=%u, %s refstation=%c sro=%c x=%f y=%f z=%f a=%f\n", - rtcm->rtcmtypes.rtcm3_1006.station_id, - systems[rtcm->rtcmtypes.rtcm3_1006.system], - BOOL(rtcm->rtcmtypes.rtcm3_1006.reference_station), - BOOL(rtcm->rtcmtypes.rtcm3_1006.single_receiver), - rtcm->rtcmtypes.rtcm3_1006.ecef_x, - rtcm->rtcmtypes.rtcm3_1006.ecef_y, - rtcm->rtcmtypes.rtcm3_1006.ecef_z, - rtcm->rtcmtypes.rtcm3_1006.height); - break; - - case 1007: - break; - - case 1008: - break; - - case 1009: - break; - - case 1010: - break; - - case 1011: - break; - - case 1012: - break; - - case 1013: - break; - - case 1014: - break; - - case 1015: - break; - - case 1016: - break; - - case 1017: - break; - - case 1018: - break; - - case 1019: - break; - - case 1020: - break; - - case 1029: - (void)fprintf(fp, " Unknown content\n"); - break; - } -#undef CODE -#undef BOOL -#undef INT -} - -/*@ +type @*/ - -#endif /* RTCM104V3_ENABLE */ diff --git a/sirf.c b/sirf.c deleted file mode 100644 index dfd184c8..00000000 --- a/sirf.c +++ /dev/null @@ -1,1037 +0,0 @@ -/* $Id$ */ -/* - * This is the gpsd driver for SiRF GPSes operating in binary mode. - * It also handles uBlox, a SiRF derivative. - * - * The advantage: Reports climb/sink rate (raw-mode clients won't see this). - * The disadvantages: Doesn't return PDOP or VDOP, just HDOP. - * - * Chris Kuethe, our SiRF expert, tells us: - * - * "I don't see any indication in any of my material that PDOP, GDOP - * or VDOP are output. There are quantities called Estimated - * {Horizontal Position, Vertical Position, Time, Horizonal Velocity} - * Error, but those are apparently only valid when SiRFDRive is - * active." - * - * "(SiRFdrive is their Dead Reckoning augmented firmware. It - * allows you to feed odometer ticks, gyro and possibly - * accelerometer inputs to the chip to allow it to continue - * to navigate in the absence of satellite information, and - * to improve fixes when you do have satellites.)" - * - * "[When we need RINEX data, we can get it from] SiRF Message #5. - * If it's no longer implemented on your receiver, messages - * 7, 28, 29 and 30 will give you the same information." - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "gpsd_config.h" -#include "gpsd.h" -#include "bits.h" -#if defined(SIRF_ENABLE) && defined(BINARY_ENABLE) - -#define HI(n) ((n) >> 8) -#define LO(n) ((n) & 0xff) - -#ifdef ALLOW_RECONFIGURE -/*@ +charint @*/ -static unsigned char enablesubframe[] = {0xa0, 0xa2, 0x00, 0x19, - 0x80, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x0C, - 0x10, - 0x00, 0x00, 0xb0, 0xb3}; - -static unsigned char disablesubframe[] = {0xa0, 0xa2, 0x00, 0x19, - 0x80, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x0C, - 0x00, - 0x00, 0x00, 0xb0, 0xb3}; - -static unsigned char modecontrol[] = {0xa0, 0xa2, 0x00, 0x0e, - 0x88, - 0x00, 0x00, /* pad bytes */ - 0x00, /* degraded mode off */ - 0x00, 0x00, /* pad bytes */ - 0x00, 0x00, /* altitude */ - 0x00, /* altitude hold auto */ - 0x00, /* use last computed alt */ - 0x00, /* reserved */ - 0x00, /* disable degraded mode */ - 0x00, /* disable dead reckoning */ - 0x01, /* enable track smoothing */ - 0x00, 0x00, 0xb0, 0xb3}; - -static unsigned char enablemid52[] = { - 0xa0, 0xa2, 0x00, 0x08, - 0xa6, 0x00, 0x34, 0x01, 0x00, 0x00, 0x00, 0x00, - 0x00, 0xdb, 0xb0, 0xb3}; -/*@ -charint @*/ -#endif /* ALLOW_RECONFIGURE */ - - -static gps_mask_t sirf_msg_debug(unsigned char *, size_t ); -static gps_mask_t sirf_msg_errors(unsigned char *, size_t ); - -static gps_mask_t sirf_msg_swversion(struct gps_device_t *, unsigned char *, size_t ); -static gps_mask_t sirf_msg_navdata(struct gps_device_t *, unsigned char *, size_t ); -static gps_mask_t sirf_msg_svinfo(struct gps_device_t *, unsigned char *, size_t ); -static gps_mask_t sirf_msg_navsol(struct gps_device_t *, unsigned char *, size_t ); -static gps_mask_t sirf_msg_geodetic(struct gps_device_t *, unsigned char *, size_t ); -static gps_mask_t sirf_msg_sysparam(struct gps_device_t *, unsigned char *, size_t ); -static gps_mask_t sirf_msg_ublox(struct gps_device_t *, unsigned char *, size_t ); -static gps_mask_t sirf_msg_ppstime(struct gps_device_t *, unsigned char *, size_t ); - - -bool sirf_write(int fd, unsigned char *msg) { - unsigned int crc; - size_t i, len; - bool ok; - - len = (size_t)((msg[2] << 8) | msg[3]); - - /* calculate CRC */ - crc = 0; - for (i = 0; i < len; i++) - crc += (int)msg[4 + i]; - crc &= 0x7fff; - - /* enter CRC after payload */ - msg[len + 4] = (unsigned char)((crc & 0xff00) >> 8); - msg[len + 5] = (unsigned char)( crc & 0x00ff); - - gpsd_report(LOG_IO, "Writing SiRF control type %02x:%s\n", msg[4], - gpsd_hexdump_wrapper(msg, len+8, LOG_IO)); - ok = (write(fd, msg, len+8) == (ssize_t)(len+8)); - (void)tcdrain(fd); - return(ok); -} - -static ssize_t sirf_control_send(struct gps_device_t *session, char *msg, size_t len) { - /*@ +charint +matchanyintegral -initallelements -mayaliasunique @*/ - session->msgbuf[0] = 0xa0; - session->msgbuf[1] = 0xa2; - session->msgbuf[2] = (len >> 8) & 0xff; - session->msgbuf[3] = len & 0xff; - memcpy(session->msgbuf+4, msg, len); - session->msgbuf[len + 6] = 0xb0; - session->msgbuf[len + 7] = 0xb3; - session->msgbuflen = len + 8; - - return sirf_write(session->gpsdata.gps_fd, - (unsigned char *)session->msgbuf) ? session->msgbuflen : -1; - /*@ -charint -matchanyintegral +initallelements +mayaliasunique @*/ -} - -static bool sirf_speed(int ttyfd, speed_t speed, char parity, int stopbits) -/* change speed in binary mode */ -{ - /*@ +charint @*/ - static unsigned char msg[] = {0xa0, 0xa2, 0x00, 0x09, - 0x86, /* byte 4: main serial port */ - 0x00, 0x00, 0x12, 0xc0, /* bytes 5-8: 4800 bps */ - 0x08, /* byte 9: 8 data bits */ - 0x01, /* byte 10: 1 stop bit */ - 0x00, /* byte 11: no parity */ - 0x00, /* byte 12: reserved pad */ - 0x00, 0x00, 0xb0, 0xb3}; - /*@ -charint @*/ - - switch (parity) { - case 'E': - case 2: - parity = (char)2; - break; - case 'O': - case 1: - parity = (char)1; - break; - case 'N': - case 0: - default: - parity = (char)0; - break; - } - msg[7] = (unsigned char)HI(speed); - msg[8] = (unsigned char)LO(speed); - msg[10] = (unsigned char)stopbits; - msg[11] = (unsigned char)parity; - return (sirf_write(ttyfd, msg)); -} - -static bool sirf_to_nmea(int ttyfd, speed_t speed) -/* switch from binary to NMEA at specified baud */ -{ - /*@ +charint @*/ - static unsigned char msg[] = {0xa0, 0xa2, 0x00, 0x18, - 0x81, 0x02, - 0x01, 0x01, /* GGA */ - 0x00, 0x00, /* suppress GLL */ - 0x01, 0x01, /* GSA */ - 0x05, 0x01, /* GSV */ - 0x01, 0x01, /* RMC */ - 0x00, 0x00, /* suppress VTG */ - 0x00, 0x01, 0x00, 0x01, - 0x00, 0x01, 0x00, 0x01, - 0x12, 0xc0, /* 4800 bps */ - 0x00, 0x00, 0xb0, 0xb3}; - /*@ -charint @*/ - - msg[26] = (unsigned char)HI(speed); - msg[27] = (unsigned char)LO(speed); - return (sirf_write(ttyfd, msg)); -} - -static void sirfbin_mode(struct gps_device_t *session, int mode) -{ - if (mode == MODE_NMEA) { - (void)sirf_to_nmea(session->gpsdata.gps_fd,session->gpsdata.baudrate); - } else { - session->back_to_nmea = false; - } -} - -static ssize_t sirf_get(struct gps_device_t *session) -{ - ssize_t len = generic_get(session); - - if (session->packet.type == SIRF_PACKET) { - session->gpsdata.driver_mode = MODE_BINARY; - } else if (session->packet.type == NMEA_PACKET) { - session->gpsdata.driver_mode = MODE_NMEA; - (void)gpsd_switch_driver(session, "Generic NMEA"); - } else { - /* should never happen */ - gpsd_report(LOG_PROG, "Unexpected packet type %d\n", - session->packet.type); - (void)gpsd_switch_driver(session, "Generic NMEA"); - } - - return len; -} - -static gps_mask_t sirf_msg_debug(unsigned char *buf, size_t len) -{ - char msgbuf[MAX_PACKET_LENGTH*3 + 2]; - int i; - - bzero(msgbuf, (int)sizeof(msgbuf)); - - /*@ +charint @*/ - if (0xe1 == buf[0]) { /* Development statistics messages */ - for (i = 2; i < (int)len; i++) - (void)snprintf(msgbuf+strlen(msgbuf), - sizeof(msgbuf)-strlen(msgbuf), - "%c", buf[i]^0xff); - gpsd_report(LOG_PROG, "DEV 0xe1: %s\n", msgbuf); - } else if (0xff == (unsigned char)buf[0]) { /* Debug messages */ - for (i = 1; i < (int)len; i++) - if (isprint(buf[i])) - (void)snprintf(msgbuf+strlen(msgbuf), - sizeof(msgbuf)-strlen(msgbuf), - "%c", buf[i]); - else - (void)snprintf(msgbuf+strlen(msgbuf), - sizeof(msgbuf)-strlen(msgbuf), - "\\x%02x", (unsigned int)buf[i]); - gpsd_report(LOG_PROG, "DBG 0xff: %s\n", msgbuf); - } - /*@ -charint @*/ - return 0; -} - -static gps_mask_t sirf_msg_errors(unsigned char *buf, size_t len UNUSED) -{ - switch (getbeuw(buf, 1)) { - case 2: - gpsd_report(LOG_PROG, "EID 0x0a type 2: Subframe %u error on PRN %u\n", getbeul(buf, 9), getbeul(buf, 5)); - break; - - case 4107: - gpsd_report(LOG_PROG, "EID 0x0a type 4107: neither KF nor LSQ fix.\n"); - break; - - default: - gpsd_report(LOG_PROG, "EID 0x0a: Error ID type %d\n", getbeuw(buf, 1)); - break; - } - return 0; -} - -static gps_mask_t sirf_msg_swversion(struct gps_device_t *session, unsigned char *buf, size_t len) -{ - double fv; - - if (len < 20) - return 0; - - gpsd_report(LOG_INF, "FV 0x06: Firmware version: %s\n", buf+1); - (void)strlcpy(session->subtype, (char *)buf+1, sizeof(session->subtype)); - fv = atof((char *)(buf+1)); - if (fv < 231) { - session->driver.sirf.driverstate |= SIRF_LT_231; - if (fv > 200) - sirfbin_mode(session, 0); - } else if (fv < 232) { - session->driver.sirf.driverstate |= SIRF_EQ_231; - } else { -#ifdef ALLOW_RECONFIGURE - gpsd_report(LOG_PROG, "Enabling PPS message...\n"); - (void)sirf_write(session->gpsdata.gps_fd, enablemid52); -#endif /* ALLOW_RECONFIGURE */ - session->driver.sirf.driverstate |= SIRF_GE_232; - session->context->valid |= LEAP_SECOND_VALID; - } - if (strstr((char *)(buf+1), "ES")) - gpsd_report(LOG_INF, "Firmware has XTrac capability\n"); - gpsd_report(LOG_PROG, "Driver state flags are: %0x\n", session->driver.sirf.driverstate); -#ifdef NTPSHM_ENABLE - session->driver.sirf.time_seen = 0; -#endif /* NTPSHM_ENABLE */ -#ifdef ALLOW_RECONFIGURE - if (session->gpsdata.baudrate >= 38400){ - gpsd_report(LOG_PROG, "Enabling subframe transmission...\n"); - (void)sirf_write(session->gpsdata.gps_fd, enablesubframe); - } -#endif /* ALLOW_RECONFIGURE */ - return DEVICEID_SET; -} - -static gps_mask_t sirf_msg_navdata(struct gps_device_t *session, unsigned char *buf, size_t len) -{ - unsigned int words[10]; - - if (len != 43) - return 0; - - //unsigned int chan = (unsigned int)getub(buf, 1); - //unsigned int svid = (unsigned int)getub(buf, 2); - words[0] = (unsigned int)getbeul(buf, 3); - words[1] = (unsigned int)getbeul(buf, 7); - words[2] = (unsigned int)getbeul(buf, 11); - words[3] = (unsigned int)getbeul(buf, 15); - words[4] = (unsigned int)getbeul(buf, 19); - words[5] = (unsigned int)getbeul(buf, 23); - words[6] = (unsigned int)getbeul(buf, 27); - words[7] = (unsigned int)getbeul(buf, 31); - words[8] = (unsigned int)getbeul(buf, 35); - words[9] = (unsigned int)getbeul(buf, 39); - gpsd_report(LOG_PROG, "50B 0x08\n"); - gpsd_interpret_subframe(session, words); - -#ifdef ALLOW_RECONFIGURE - if (session->gpsdata.baudrate < 38400){ - gpsd_report(LOG_PROG, "Disabling subframe transmission...\n"); - (void)sirf_write(session->gpsdata.gps_fd, disablesubframe); - } -#endif /* ALLOW_RECONFIGURE */ - return 0; -} - -static gps_mask_t sirf_msg_svinfo(struct gps_device_t *session, unsigned char *buf, size_t len) -{ - int st, i, j, cn; - - if (len != 188) - return 0; - - gpsd_zero_satellites(&session->gpsdata); - session->gpsdata.sentence_time - = gpstime_to_unix(getbesw(buf, 1), getbeul(buf, 3)*1e-2) - session->context->leap_seconds; - for (i = st = 0; i < SIRF_CHANNELS; i++) { - int off = 8 + 15 * i; - bool good; - session->gpsdata.PRN[st] = (int)getub(buf, off); - session->gpsdata.azimuth[st] = (int)(((unsigned)getub(buf, off+1)*3)/2.0); - session->gpsdata.elevation[st] = (int)((unsigned)getub(buf, off+2)/2.0); - cn = 0; - for (j = 0; j < 10; j++) - cn += (int)getub(buf, off+5+j); - - session->gpsdata.ss[st] = cn/10; - good = session->gpsdata.PRN[st]!=0 && - session->gpsdata.azimuth[st]!=0 && - session->gpsdata.elevation[st]!=0; -#ifdef __UNUSED__ - gpsd_report(LOG_PROG, "PRN=%2d El=%3.2f Az=%3.2f ss=%3d stat=%04x %c\n", - getub(buf, off), - getub(buf, off+2)/2.0, - (getub(buf, off+1)*3)/2.0, - cn/10, - getbeuw(buf, off+3), - good ? '*' : ' '); -#endif /* UNUSED */ - if (good!=0) - st += 1; - } - session->gpsdata.satellites = st; -#ifdef NTPSHM_ENABLE - if (st > 3) { - if ((session->driver.sirf.time_seen & TIME_SEEN_GPS_1)==0) - gpsd_report(LOG_PROG, "valid time in message 0x04, seen=0x%02x\n", - session->driver.sirf.time_seen); - session->driver.sirf.time_seen |= TIME_SEEN_GPS_1; - if (session->context->enable_ntpshm && IS_HIGHEST_BIT(session->driver.sirf.time_seen,TIME_SEEN_GPS_1)) - (void)ntpshm_put(session,session->gpsdata.sentence_time+0.8); - } -#endif /* NTPSHM_ENABLE */ - /* - * The freaking brain-dead SiRF chip doesn't obey its own - * rate-control command for 04, at least at firmware rev. 231, - * so we have to do our own rate-limiting here... - */ - gpsd_report(LOG_PROG, "MTD 0x04: %d satellites\n", st); - if ((session->driver.sirf.satcounter++ % 5) != 0) - return 0; - else - return TIME_SET | SATELLITE_SET; -} - -static gps_mask_t sirf_msg_navsol(struct gps_device_t *session, unsigned char *buf, size_t len) -{ - int i; - unsigned short navtype; - gps_mask_t mask = 0; - - if (len != 41) - return 0; - - session->gpsdata.satellites_used = (int)getub(buf, 28); - memset(session->gpsdata.used,0,sizeof(session->gpsdata.used)); - for (i = 0; i < SIRF_CHANNELS; i++) - session->gpsdata.used[i] = (int)getub(buf, 29+i); - if ((session->driver.sirf.driverstate & (SIRF_GE_232 | UBLOX))==0) { - /* position/velocity is bytes 1-18 */ - ecef_to_wgs84fix(&session->gpsdata, - getbesl(buf, 1)*1.0, getbesl(buf, 5)*1.0, getbesl(buf, 9)*1.0, - getbesw(buf, 13)/8.0, getbesw(buf, 15)/8.0, getbesw(buf, 17)/8.0); - /* fix status is byte 19 */ - navtype = (unsigned short)getub(buf, 19); - session->gpsdata.status = STATUS_NO_FIX; - session->gpsdata.fix.mode = MODE_NO_FIX; - if ((navtype & 0x80) != 0) - session->gpsdata.status = STATUS_DGPS_FIX; - else if ((navtype & 0x07) > 0 && (navtype & 0x07) < 7) - session->gpsdata.status = STATUS_FIX; - if ((navtype & 0x07) == 4 || (navtype & 0x07) == 6) - session->gpsdata.fix.mode = MODE_3D; - else if (session->gpsdata.status != 0) - session->gpsdata.fix.mode = MODE_2D; - if (session->gpsdata.fix.mode == MODE_3D) - mask |= ALTITUDE_SET | CLIMB_SET; - gpsd_report(LOG_PROG, "MND 0x02: Navtype = 0x%0x, Status = %d, mode = %d\n", - navtype,session->gpsdata.status,session->gpsdata.fix.mode); - /* byte 20 is HDOP, see below */ - /* byte 21 is "mode 2", not clear how to interpret that */ - session->gpsdata.fix.time = session->gpsdata.sentence_time = - gpstime_to_unix(getbesw(buf, 22), getbeul(buf, 24)*1e-2) - - session->context->leap_seconds; -#ifdef NTPSHM_ENABLE - if (session->gpsdata.fix.mode > MODE_NO_FIX) { - if ((session->driver.sirf.time_seen & TIME_SEEN_GPS_2) == 0) - gpsd_report(LOG_PROG, "valid time in message 0x02, seen=0x%02x\n", - session->driver.sirf.time_seen); - session->driver.sirf.time_seen |= TIME_SEEN_GPS_2; - if (session->context->enable_ntpshm && IS_HIGHEST_BIT(session->driver.sirf.time_seen,TIME_SEEN_GPS_2)) - (void)ntpshm_put(session, session->gpsdata.fix.time + 0.8); - } -#endif /* NTPSHM_ENABLE */ - /* fix quality data */ - session->gpsdata.hdop = (double)getub(buf, 20)/5.0; - mask |= TIME_SET | LATLON_SET | TRACK_SET | SPEED_SET | STATUS_SET | MODE_SET | HDOP_SET | USED_SET | CYCLE_START_SET; - } - return mask; -} - -static gps_mask_t sirf_msg_geodetic(struct gps_device_t *session, unsigned char *buf, size_t len) -{ - unsigned short navtype; - gps_mask_t mask = 0; - - if (len != 91) - return 0; - - session->gpsdata.sentence_length = 91; - (void)strlcpy(session->gpsdata.tag, "GND",MAXTAGLEN+1); - - navtype = (unsigned short)getbeuw(buf, 3); - session->gpsdata.status = STATUS_NO_FIX; - session->gpsdata.fix.mode = MODE_NO_FIX; - if (navtype & 0x80) - session->gpsdata.status = STATUS_DGPS_FIX; - else if ((navtype & 0x07) > 0 && (navtype & 0x07) < 7) - session->gpsdata.status = STATUS_FIX; - session->gpsdata.fix.mode = MODE_NO_FIX; - if ((navtype & 0x07) == 4 || (navtype & 0x07) == 6) - session->gpsdata.fix.mode = MODE_3D; - else if (session->gpsdata.status) - session->gpsdata.fix.mode = MODE_2D; - gpsd_report(LOG_PROG, "GND 0x29: Navtype = 0x%0x, Status = %d, mode = %d\n", - navtype, session->gpsdata.status, session->gpsdata.fix.mode); - mask |= STATUS_SET | MODE_SET; - - session->gpsdata.fix.latitude = getbesl(buf, 23)*1e-7; - session->gpsdata.fix.longitude = getbesl(buf, 27)*1e-7; - if (session->gpsdata.fix.latitude!=0 && session->gpsdata.fix.latitude!=0) - mask |= LATLON_SET; - - if ((session->gpsdata.fix.eph = getbesl(buf, 50)*1e-2) > 0) - mask |= HERR_SET; - if ((session->gpsdata.fix.epv = getbesl(buf, 54)*1e-2) > 0) - mask |= VERR_SET; - if ((session->gpsdata.fix.eps = getbesw(buf, 62)*1e-2) > 0) - mask |= SPEEDERR_SET; - - /* HDOP should be available at byte 89, but in 231 it's zero. */ - if ((session->gpsdata.hdop = (unsigned int)getub(buf, 89) * 0.2) > 0) - mask |= HDOP_SET; - - if (session->driver.sirf.driverstate & SIRF_GE_232) { - struct tm unpacked_date; - double subseconds; - /* - * Many versions of the SiRF protocol manual don't document - * this sentence at all. Those that do may incorrectly - * describe UTC Day, Hour, and Minute as 2-byte quantities, - * not 1-byte. Chris Kuethe, our SiRF expert, tells us: - * - * "The Geodetic Navigation packet (0x29) was not fully - * implemented in firmware prior to version 2.3.2. So for - * anyone running 231.000.000 or earlier (including ES, - * SiRFDRive, XTrac trains) you won't get UTC time. I don't - * know what's broken in firmwares before 2.3.1..." - * - * To work around the incomplete implementation of this - * packet in 231, we used to assume that only the altitude field - * from this packet is valid. But even this doesn't necessarily - * seem to be the case. Instead, we do our own computation - * of geoid separation now. - * - * UTC is left all zeros in 231 and older firmware versions, - * and misdocumented in the Protocol Reference (version 1.4). - * Documented: Real: - * UTC year 2 2 - * UTC month 1 1 - * UTC day 2 1 - * UTC hour 2 1 - * UTC minute 2 1 - * UTC second 2 2 - * 11 8 - */ - unpacked_date.tm_year = (int)getbeuw(buf, 11)-1900; - unpacked_date.tm_mon = (int)getub(buf, 13)-1; - unpacked_date.tm_mday = (int)getub(buf, 14); - unpacked_date.tm_hour = (int)getub(buf, 15); - unpacked_date.tm_min = (int)getub(buf, 16); - unpacked_date.tm_sec = 0; - subseconds = getbeuw(buf, 17)*1e-3; - /*@ -compdef -unrecog */ - session->gpsdata.fix.time = session->gpsdata.sentence_time = - (double)timegm(&unpacked_date)+subseconds; - /*@ +compdef +unrecog */ - gpsd_report(LOG_PROG, "MID 41 UTC: %lf\n", session->gpsdata.fix.time); -#ifdef NTPSHM_ENABLE - if (session->gpsdata.fix.mode > MODE_NO_FIX && unpacked_date.tm_year != 0) { - if ((session->driver.sirf.time_seen & TIME_SEEN_UTC_1) == 0) - gpsd_report(LOG_PROG, "valid time in message 0x29, seen=0x%02x\n", - session->driver.sirf.time_seen); - session->driver.sirf.time_seen |= TIME_SEEN_UTC_1; - if (session->context->enable_ntpshm && IS_HIGHEST_BIT(session->driver.sirf.time_seen,TIME_SEEN_UTC_1)) - (void)ntpshm_put(session, session->gpsdata.fix.time + 0.8); - } -#endif /* NTPSHM_ENABLE */ - /* skip 4 bytes of satellite map */ - session->gpsdata.fix.altitude = getbesl(buf, 35)*1e-2; - /* skip 1 byte of map datum */ - session->gpsdata.fix.speed = getbesw(buf, 40)*1e-2; - session->gpsdata.fix.track = getbesw(buf, 42)*1e-2; - /* skip 2 bytes of magnetic variation */ - session->gpsdata.fix.climb = getbesw(buf, 46)*1e-2; - mask |= TIME_SET | SPEED_SET | TRACK_SET; - if (session->gpsdata.fix.mode == MODE_3D) - mask |= ALTITUDE_SET | CLIMB_SET; - } - return mask; -} - -static gps_mask_t sirf_msg_sysparam(struct gps_device_t *session, unsigned char *buf, size_t len) -{ - - if (len != 65) - return 0; - -#ifdef ALLOW_RECONFIGURE - /* save these to restore them in the revert method */ - session->driver.sirf.nav_parameters_seen = true; - session->driver.sirf.altitude_hold_mode = getub(buf, 5); - session->driver.sirf.altitude_hold_source = getub(buf, 6); - session->driver.sirf.altitude_source_input = getbesw(buf, 7); - session->driver.sirf.degraded_mode = getub(buf, 9); - session->driver.sirf.degraded_timeout = getub(buf, 10); - session->driver.sirf.dr_timeout = getub(buf, 11); - session->driver.sirf.track_smooth_mode = getub(buf, 12); - gpsd_report(LOG_PROG, "Setting Navigation Parameters\n"); - (void)sirf_write(session->gpsdata.gps_fd, modecontrol); -#endif /* ALLOW_RECONFIGURE */ - return 0; -} - -static gps_mask_t sirf_msg_ublox(struct gps_device_t *session, unsigned char *buf, size_t len UNUSED) -{ - gps_mask_t mask; - unsigned short navtype; - - if (len != 39) - return 0; - - /* this packet is only sent by uBlox firmware from version 1.32 */ - mask = LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET | CLIMB_SET | - STATUS_SET | MODE_SET | HDOP_SET | VDOP_SET | PDOP_SET; - session->gpsdata.fix.latitude = getbesl(buf, 1) * RAD_2_DEG * 1e-8; - session->gpsdata.fix.longitude = getbesl(buf, 5) * RAD_2_DEG * 1e-8; - session->gpsdata.separation = wgs84_separation(session->gpsdata.fix.latitude, session->gpsdata.fix.longitude); - session->gpsdata.fix.altitude = getbesl(buf, 9) * 1e-3 - session->gpsdata.separation; - session->gpsdata.fix.speed = getbesl(buf, 13) * 1e-3; - session->gpsdata.fix.climb = getbesl(buf, 17) * 1e-3; - session->gpsdata.fix.track = getbesl(buf, 21) * RAD_2_DEG * 1e-8; - - navtype = (unsigned short)getub(buf, 25); - session->gpsdata.status = STATUS_NO_FIX; - session->gpsdata.fix.mode = MODE_NO_FIX; - if (navtype & 0x80) - session->gpsdata.status = STATUS_DGPS_FIX; - else if ((navtype & 0x07) > 0 && (navtype & 0x07) < 7) - session->gpsdata.status = STATUS_FIX; - if ((navtype & 0x07) == 4 || (navtype & 0x07) == 6) - session->gpsdata.fix.mode = MODE_3D; - else if (session->gpsdata.status) - session->gpsdata.fix.mode = MODE_2D; - gpsd_report(LOG_PROG, "EMND 0x62: Navtype = 0x%0x, Status = %d, mode = %d\n", - navtype, session->gpsdata.status, session->gpsdata.fix.mode); - - if (navtype & 0x40) { /* UTC corrected timestamp? */ - struct tm unpacked_date; - double subseconds; - mask |= TIME_SET; - unpacked_date.tm_year = (int)getbeuw(buf, 26) - 1900; - unpacked_date.tm_mon = (int)getub(buf, 28) - 1; - unpacked_date.tm_mday = (int)getub(buf, 29); - unpacked_date.tm_hour = (int)getub(buf, 30); - unpacked_date.tm_min = (int)getub(buf, 31); - unpacked_date.tm_sec = 0; - subseconds = ((unsigned short)getbeuw(buf, 32))*1e-3; - /*@ -compdef */ - session->gpsdata.fix.time = session->gpsdata.sentence_time = - (double)mkgmtime(&unpacked_date)+subseconds; - /*@ +compdef */ -#ifdef NTPSHM_ENABLE - if ((session->driver.sirf.time_seen & TIME_SEEN_UTC_2) == 0) - gpsd_report(LOG_PROG, "valid time in message 0x62, seen=0x%02x\n", - session->driver.sirf.time_seen); - session->driver.sirf.time_seen |= TIME_SEEN_UTC_2; - if (session->context->enable_ntpshm && IS_HIGHEST_BIT(session->driver.sirf.time_seen,TIME_SEEN_UTC_2)) - (void)ntpshm_put(session, session->gpsdata.fix.time + 0.8); -#endif /* NTPSHM_ENABLE */ - session->context->valid |= LEAP_SECOND_VALID; - } - - session->gpsdata.gdop = (int)getub(buf, 34) / 5.0; - session->gpsdata.pdop = (int)getub(buf, 35) / 5.0; - session->gpsdata.hdop = (int)getub(buf, 36) / 5.0; - session->gpsdata.vdop = (int)getub(buf, 37) / 5.0; - session->gpsdata.tdop = (int)getub(buf, 38) / 5.0; - session->driver.sirf.driverstate |= UBLOX; - return mask; -} - -static gps_mask_t sirf_msg_ppstime(struct gps_device_t *session, unsigned char *buf, size_t len) -{ - gps_mask_t mask = 0; - - if (len != 19) - return 0; - - gpsd_report(LOG_PROG, "PPS 0x34: Status = 0x%02x\n", getub(buf, 14)); - if (((int)getub(buf, 14) & 0x07) == 0x07) { /* valid UTC time? */ - struct tm unpacked_date; - unpacked_date.tm_hour = (int)getub(buf, 1); - unpacked_date.tm_min = (int)getub(buf, 2); - unpacked_date.tm_sec = (int)getub(buf, 3); - unpacked_date.tm_mday = (int)getub(buf, 4); - unpacked_date.tm_mon = (int)getub(buf, 5) - 1; - unpacked_date.tm_year = (int)getbeuw(buf, 6) - 1900; - /*@ -compdef */ - session->gpsdata.fix.time = session->gpsdata.sentence_time = - (double)mkgmtime(&unpacked_date); - /*@ +compdef */ - session->context->leap_seconds = (int)getbeuw(buf, 8); - session->context->valid |= LEAP_SECOND_VALID; -#ifdef NTPSHM_ENABLE - if ((session->driver.sirf.time_seen & TIME_SEEN_UTC_2) == 0) - gpsd_report(LOG_PROG, "valid time in message 0x34, seen=0x%02x\n", - session->driver.sirf.time_seen); - session->driver.sirf.time_seen |= TIME_SEEN_UTC_2; - if (session->context->enable_ntpshm && IS_HIGHEST_BIT(session->driver.sirf.time_seen,TIME_SEEN_UTC_2)) - (void)ntpshm_put(session, session->gpsdata.fix.time + 0.3); -#endif /* NTPSHM_ENABLE */ - mask |= TIME_SET; - } - return mask; -} - -gps_mask_t sirf_parse(struct gps_device_t *session, unsigned char *buf, size_t len) -{ - - if (len == 0) - return 0; - - buf += 4; - len -= 8; - gpsd_report(LOG_RAW, "Raw SiRF packet type 0x%02x length %zd: %s\n", - buf[0], len, gpsd_hexdump_wrapper(buf, len, LOG_RAW)); - (void)snprintf(session->gpsdata.tag, sizeof(session->gpsdata.tag), - "MID%d",(int)buf[0]); - - switch (buf[0]) - { - case 0x02: /* Measure Navigation Data Out */ - return sirf_msg_navsol(session, buf, len); - - case 0x04: /* Measured tracker data out */ - return sirf_msg_svinfo(session, buf, len); - - case 0x05: /* Raw Tracker Data Out */ - return 0; - - case 0x06: /* Software Version String */ - return sirf_msg_swversion(session, buf, len); - - case 0x07: /* Clock Status Data */ - gpsd_report(LOG_PROG, "CLK 0x07\n"); - return 0; - - case 0x08: /* subframe data -- extract leap-second from this */ - /* - * Chris Kuethe says: - * "Message 8 is generated as the data is received. It is not - * buffered on the chip. So when you enable message 8, you'll - * get one subframe every 6 seconds. Of the data received, the - * almanac and ephemeris are buffered and stored, so you can - * query them at will. Alas, the time parameters are not - * stored, which is really lame, as the UTC-GPS correction - * changes 1 second every few years. Maybe." - */ - return sirf_msg_navdata(session, buf, len); - - case 0x09: /* CPU Throughput */ - gpsd_report(LOG_PROG, - "THR 0x09: SegStatMax=%.3f, SegStatLat=%3.f, AveTrkTime=%.3f, Last MS=%u\n", - (float)getbeuw(buf, 1)/186, (float)getbeuw(buf, 3)/186, - (float)getbeuw(buf, 5)/186, getbeuw(buf, 7)); - return 0; - - case 0x0a: /* Error ID Data */ - return sirf_msg_errors(buf, len); - - case 0x0b: /* Command Acknowledgement */ - gpsd_report(LOG_PROG, "ACK 0x0b: %02x\n",getub(buf, 1)); - return 0; - - case 0x0c: /* Command NAcknowledgement */ - gpsd_report(LOG_PROG, "NAK 0x0c: %02x\n",getub(buf, 1)); - return 0; - - case 0x0d: /* Visible List */ - gpsd_report(LOG_PROG, "VIS 0x0d\n"); - return 0; - - case 0x0e: /* Almanac Data */ - gpsd_report(LOG_PROG, "ALM 0x0e: %s\n", - gpsd_hexdump_wrapper(buf, len, LOG_PROG)); - return 0; - - case 0x0f: /* Ephemeris Data */ - gpsd_report(LOG_PROG, "EPH 0x0f: %s\n", - gpsd_hexdump_wrapper(buf, len, LOG_PROG)); - return 0; - - case 0x11: /* Differential Corrections */ - gpsd_report(LOG_PROG, "DIFF 0x11: %s\n", - gpsd_hexdump_wrapper(buf, len, LOG_PROG)); - return 0; - - case 0x12: /* OK To Send */ - gpsd_report(LOG_PROG, "OTS 0x12: send indicator = %d\n",getub(buf, 1)); - return 0; - -#ifdef ALLOW_RECONFIGURE - case 0x13: /* Navigation Parameters */ - return sirf_msg_sysparam(session, buf, len); -#endif /* ALLOW_RECONFIGURE */ - - case 0x1b: /* DGPS status (undocumented) */ - /****************************************************************** - Not actually documented in any published materials. - Here is what Chris Kuethe got from the SiRF folks, - (plus some corrections from the GpsPaSsion forums): - - Start of message - ---------------- - Message ID 1 byte 27 - Correction Source 1 byte 0=None, 1=SBAS, 2=Serial, 3=Beacon, - 4=Software - - total: 2 bytes - - Middle part of message varies if using beacon or other: - ------------------------------------------------------- - If Beacon: - Receiver Freq Hz 4 bytes - Bit rate BPS 1 byte - Status bit map 1 byte 01=Signal Valid, - 02=Auto frequency detect - 04=Auto bit rate detect - Signal Magnitude 4 bytes Note: in internal units - Signal Strength dB 2 bytes derived from Signal Magnitude - SNR dB 2 bytes - - total: 14 bytes - - If Not Beacon: - Correction Age[12] 1 byte x 12 Age in seconds in same order as follows - Reserved 2 bytes - - total: 14 bytes - - End of Message - -------------- - Repeated 12 times (pad with 0 if less than 12 SV corrections): - SVID 1 byte - Correction (cm) 2 bytes (signed short) - - total 3 x 12 = 36 bytes - ******************************************************************/ - return 0; - - case 0x1c: /* Navigation Library Measurement Data */ - gpsd_report(LOG_PROG, "NLMD 0x1c: %s\n", - gpsd_hexdump_wrapper(buf, len, LOG_PROG)); - return 0; - - case 0x1d: /* Navigation Library DGPS Data */ - gpsd_report(LOG_PROG, "NLDG 0x1d: %s\n", - gpsd_hexdump_wrapper(buf, len, LOG_PROG)); - return 0; - - case 0x1e: /* Navigation Library SV State Data */ - gpsd_report(LOG_PROG, "NLSV 0x1e: %s\n", - gpsd_hexdump_wrapper(buf, len, LOG_PROG)); - return 0; - - case 0x1f: /* Navigation Library Initialization Data */ - gpsd_report(LOG_PROG, "NLID 0x1f: %s\n", - gpsd_hexdump_wrapper(buf, len, LOG_PROG)); - return 0; - - case 0x29: /* Geodetic Navigation Information */ - return sirf_msg_geodetic(session, buf, len); - - case 0x32: /* SBAS corrections */ - return 0; - - case 0x34: /* PPS Time */ - /* - * Carl Carter from SiRF writes: "We do not output on the - * second (unless you are using message ID 52). We make - * measurements in the receiver in time with an internal - * counter that is not slaved to GPS time, so the measurements - * are made at a time that wanders around the second. Then, - * after the measurements are made (all normalized to the same - * point in time) we dispatch the navigation software to make - * a solution, and that solution comes out some 200 to 300 ms - * after the measurement time. So you may get a message at - * 700 ms after the second that uses measurements time tagged - * 450 ms after the second. And if some other task jumps up - * and delays things, that message may not come out until 900 - * ms after the second. Things can get out of sync to the - * point that if you try to resolve the GPS time of our 1 PPS - * pulses using the navigation messages, you will find it - * impossible to be consistent. That is why I added message - * ID 52 to our system -- it is tied to the creation of the 1 - * PPS and always comes out right around the top of the - * second." - */ - return sirf_msg_ppstime(session, buf, len); - - case 0x62: /* uBlox Extended Measured Navigation Data */ - return sirf_msg_ublox(session, buf, len); - - case 0x80: /* Initialize Data Source */ - gpsd_report(LOG_PROG, "INIT 0x80: %s\n", - gpsd_hexdump_wrapper(buf, len, LOG_PROG)); - return 0; - - case 0xe1: /* Development statistics messages */ - /* FALLTHROUGH */ - case 0xff: /* Debug messages */ - (void)sirf_msg_debug(buf, len); - return 0; - - default: - gpsd_report(LOG_WARN, "Unknown SiRF packet id %d length %zd: %s\n", - buf[0], len, gpsd_hexdump_wrapper(buf, len, LOG_WARN)); - return 0; - } -} - -static gps_mask_t sirfbin_parse_input(struct gps_device_t *session) -{ - gps_mask_t st; - - if (session->packet.type == SIRF_PACKET){ - st = sirf_parse(session, session->packet.outbuffer, - session->packet.outbuflen); - session->gpsdata.driver_mode = MODE_BINARY; - return st; -#ifdef NMEA_ENABLE - } else if (session->packet.type == NMEA_PACKET) { - st = nmea_parse((char *)session->packet.outbuffer, session); - session->gpsdata.driver_mode = MODE_NMEA; - return st; -#endif /* NMEA_ENABLE */ - } else - return 0; -} - -#ifdef ALLOW_RECONFIGURE -static void sirfbin_configure(struct gps_device_t *session, unsigned int seq) -{ - if (seq != 0) - return; - if (session->packet.type == NMEA_PACKET) { - gpsd_report(LOG_PROG, "Switching chip mode to SiRF binary.\n"); - (void)nmea_send(session, - "$PSRF100,0,%d,8,1,0", session->gpsdata.baudrate); - } - /* do this every time*/ - { - /*@ +charint @*/ - static unsigned char navparams[] = {0xa0, 0xa2, 0x00, 0x02, - 0x98, 0x00, - 0x00, 0x00, 0xb0, 0xb3}; - static unsigned char dgpscontrol[] = {0xa0, 0xa2, 0x00, 0x07, - 0x85, 0x01, 0x00, 0x00, - 0x00, 0x00, 0x00, - 0x00, 0x00, 0xb0, 0xb3}; - static unsigned char sbasparams[] = {0xa0, 0xa2, 0x00, 0x06, - 0xaa, 0x00, 0x01, 0x00, - 0x00, 0x00, - 0x00, 0x00, 0xb0, 0xb3}; - static unsigned char versionprobe[] = {0xa0, 0xa2, 0x00, 0x02, - 0x84, 0x00, - 0x00, 0x00, 0xb0, 0xb3}; - static unsigned char requestecef[] = {0xa0, 0xa2, 0x00, 0x08, - 0xa6, 0x00, 0x02, 0x01, - 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0xb0, 0xb3}; - static unsigned char requesttracker[] = {0xa0, 0xa2, 0x00, 0x08, - 0xa6, 0x00, 0x04, 0x03, - 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0xb0, 0xb3}; - /*@ -charint @*/ - gpsd_report(LOG_PROG, "Requesting periodic ecef reports...\n"); - (void)sirf_write(session->gpsdata.gps_fd, requestecef); - gpsd_report(LOG_PROG, "Requesting periodic tracker reports...\n"); - (void)sirf_write(session->gpsdata.gps_fd, requesttracker); - gpsd_report(LOG_PROG, "Setting DGPS control to use SBAS...\n"); - (void)sirf_write(session->gpsdata.gps_fd, dgpscontrol); - gpsd_report(LOG_PROG, "Setting SBAS to auto/integrity mode...\n"); - (void)sirf_write(session->gpsdata.gps_fd, sbasparams); - gpsd_report(LOG_PROG, "Probing for firmware version...\n"); - (void)sirf_write(session->gpsdata.gps_fd, versionprobe); - gpsd_report(LOG_PROG, "Requesting navigation parameters...\n"); - (void)sirf_write(session->gpsdata.gps_fd, navparams); - } -} - -static void sirfbin_revert(struct gps_device_t *session) -{ - /*@ +charint @*/ - static unsigned char moderevert[] = {0xa0, 0xa2, 0x00, 0x0e, - 0x88, - 0x00, 0x00, /* pad bytes */ - 0x00, /* degraded mode */ - 0x00, 0x00, /* pad bytes */ - 0x00, 0x00, /* altitude source */ - 0x00, /* altitude hold mode */ - 0x00, /* use last computed alt */ - 0x00, /* reserved */ - 0x00, /* degraded mode timeout */ - 0x00, /* dead reckoning timeout */ - 0x00, /* track smoothing */ - 0x00, 0x00, 0xb0, 0xb3}; - /*@ -charint -shiftimplementation @*/ - putbyte(moderevert, 7, session->driver.sirf.degraded_mode); - putbeword(moderevert, 10, session->driver.sirf.altitude_source_input); - putbyte(moderevert, 12, session->driver.sirf.altitude_hold_mode); - putbyte(moderevert, 13, session->driver.sirf.altitude_hold_source); - putbyte(moderevert, 15, session->driver.sirf.degraded_timeout); - putbyte(moderevert, 16, session->driver.sirf.dr_timeout); - putbyte(moderevert, 17, session->driver.sirf.track_smooth_mode); - /*@ +shiftimplementation @*/ - gpsd_report(LOG_PROG, "Reverting navigation parameters...\n"); - (void)sirf_write(session->gpsdata.gps_fd, moderevert); -} -#endif /* ALLOW_RECONFIGURE */ - -static bool sirfbin_speed(struct gps_device_t *session, - speed_t speed, char parity, int stopbits) -{ - return sirf_speed(session->gpsdata.gps_fd, speed, parity, stopbits); -} - -/* this is everything we export */ -const struct gps_type_t sirf_binary = -{ - .type_name = "SiRF binary", /* full name of type */ - .packet_type = SIRF_PACKET, /* associated lexer packet type */ - .trigger = NULL, /* no trigger */ - .channels = SIRF_CHANNELS, /* consumer-grade GPS */ - .control_send = sirf_control_send,/* how to send a control string */ - .probe_wakeup = NULL, /* no wakeup to be done before hunt */ - .probe_detect = NULL, /* no probe */ - .probe_subtype = NULL, /* can't probe more in NMEA mode */ -#ifdef ALLOW_RECONFIGURE - .configurator = sirfbin_configure,/* initialize the device */ -#endif /* ALLOW_RECONFIGURE */ - .get_packet = sirf_get, /* be prepared for SiRF or NMEA */ - .parse_packet = sirfbin_parse_input,/* parse message packets */ - .rtcm_writer = pass_rtcm, /* send RTCM data straight */ - .speed_switcher = sirfbin_speed, /* we can change baud rate */ - .mode_switcher = sirfbin_mode, /* there's a mode switcher */ - .rate_switcher = NULL, /* no sample-rate switcher */ - .cycle_chars = -1, /* not relevant, no rate switch */ -#ifdef ALLOW_RECONFIGURE - .revert = sirfbin_revert, /* no reversion code */ -#endif /* ALLOW_RECONFIGURE */ - .wrapup = NULL, /* no close hook */ - .cycle = 1, /* updates every second */ -}; -#endif /* defined(SIRF_ENABLE) && defined(BINARY_ENABLE) */ diff --git a/superstar2.c b/superstar2.c deleted file mode 100644 index 18a8f665..00000000 --- a/superstar2.c +++ /dev/null @@ -1,528 +0,0 @@ -/* $Id$ */ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "gpsd_config.h" -#include "gpsd.h" - -#if defined(SUPERSTAR2_ENABLE) && defined(BINARY_ENABLE) -#include "bits.h" -#include "superstar2.h" - -/* - * These routines are specific to this driver - */ - -static gps_mask_t superstar2_parse_input(struct gps_device_t *); -static gps_mask_t superstar2_dispatch(struct gps_device_t *, - unsigned char *, size_t ); -static gps_mask_t superstar2_msg_ack(struct gps_device_t *, - unsigned char *, size_t ); -static gps_mask_t superstar2_msg_navsol_lla(struct gps_device_t *, - unsigned char *, size_t ); -static gps_mask_t superstar2_msg_navsol_ecef(struct gps_device_t *, - unsigned char *, size_t ); -static gps_mask_t superstar2_msg_timing(struct gps_device_t *, - unsigned char *, size_t ); -static gps_mask_t superstar2_msg_svinfo(struct gps_device_t *, - unsigned char *, size_t ); - -/* - * These methods may be called elsewhere in gpsd - */ -static ssize_t superstar2_control_send(struct gps_device_t *, char *, size_t ); -static void superstar2_probe_wakeup(struct gps_device_t *); -static void superstar2_configurator(struct gps_device_t *, unsigned int ); -static bool superstar2_set_speed(struct gps_device_t *, speed_t, char, int); -static void superstar2_set_mode(struct gps_device_t *, int ); -static void superstar2_probe_wakeup(struct gps_device_t *); -static void superstar2_probe_subtype(struct gps_device_t *, unsigned int ); - -/* - * Decode the message ACK message - */ -static gps_mask_t -superstar2_msg_ack(struct gps_device_t *session UNUSED, - unsigned char *buf, size_t data_len) -{ - if (data_len == 11) - gpsd_report(LOG_PROG, - "superstar2 #126 - " - "ACK 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x\n", - buf[5], buf[6], buf[7], buf[8], buf[9]); - return ONLINE_SET; /* always returns ONLINE_SET, but avoid runt packets */ -} - -/* - * Decode the navigation solution message - */ -static gps_mask_t -superstar2_msg_navsol_lla(struct gps_device_t *session, - unsigned char *buf, size_t data_len) -{ - gps_mask_t mask; - unsigned char flags; - union int_float i_f; - union long_double l_d; - double d; - struct tm tm; - - if (data_len != 77) - return 0; - - gpsd_report(LOG_PROG, "superstar2 #20 - user navigation data\n"); - mask = ONLINE_SET; - - flags = getub(buf, 72); - if ((flags & 0x0f) != 3) /* mode 3 is navigation */ - return mask; - - /* extract time data */ - bzero(&tm, sizeof(tm)); - tm.tm_hour = getub(buf, 4) & 0x1f; - tm.tm_min = getub(buf, 5); - d = getled(buf, 6); - tm.tm_sec = (int)d; - tm.tm_mday = getub(buf, 14); - tm.tm_mon = getub(buf, 15) - 1; - tm.tm_year = getleuw(buf, 16) - 1900; - session->gpsdata.fix.time = session->gpsdata.sentence_time = - timegm(&tm) + (d - tm.tm_sec); - mask |= TIME_SET; - - /* extract the local tangential plane (ENU) solution */ - session->gpsdata.fix.latitude = getled(buf,18) * RAD_2_DEG; - session->gpsdata.fix.longitude = getled(buf,26) * RAD_2_DEG; - session->gpsdata.fix.altitude = getlef(buf,34); - session->gpsdata.fix.speed = getlef(buf,38); - session->gpsdata.fix.track = getlef(buf,42) * RAD_2_DEG; - session->gpsdata.fix.climb = getlef(buf,54); - mask |= LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET | CLIMB_SET ; - - session->gpsdata.satellites_used = getub(buf,71) & 0x0f; - session->gpsdata.hdop = getleuw(buf,66) * 0.1; - session->gpsdata.vdop = getleuw(buf,68) * 0.1; - /* other DOP if available */ - mask |= HDOP_SET | VDOP_SET | USED_SET; - - flags = getub(buf,70); - switch (flags & 0x1f) { - case 2: - session->gpsdata.fix.mode = MODE_3D; - session->gpsdata.status = STATUS_FIX; - break; - case 4: - session->gpsdata.fix.mode = MODE_3D; - session->gpsdata.status = STATUS_DGPS_FIX; - break; - case 5: - session->gpsdata.fix.mode = MODE_2D; - session->gpsdata.status = STATUS_DGPS_FIX; - break; - case 3: - case 6: - session->gpsdata.fix.mode = MODE_2D; - session->gpsdata.status = STATUS_FIX; - break; - default: - session->gpsdata.status = STATUS_NO_FIX; - session->gpsdata.fix.mode = MODE_NO_FIX; - } - - /* CYCLE_START_SET if this message starts a reporting period */ - mask |= MODE_SET | STATUS_SET | CYCLE_START_SET ; - - return mask; -} - -static gps_mask_t -superstar2_msg_navsol_ecef(struct gps_device_t *session, - unsigned char *buf, size_t data_len) -{ - gps_mask_t mask; - unsigned char flags; - union int_float i_f; - union long_double l_d; - double tm, tow; - - if (data_len != 85) - return 0; - - gpsd_report(LOG_PROG, "superstar2 #21 - ecef navigation data\n"); - mask = ONLINE_SET; - - flags = getub(buf, 79) & 0x1f; - if ((flags < 2) || (flags > 5)) - return mask; - - /* extract time data */ - tow = getled(buf, 4); - session->driver.superstar2.gps_week = getleuw(buf, 12); - tm = gpstime_to_unix((int)session->driver.superstar2.gps_week, tow) - - session->context->leap_seconds; - session->gpsdata.fix.time = session->gpsdata.sentence_time = tm; - mask |= TIME_SET; - - /* extract the earth-centered, earth-fixed (ECEF) solution */ - ecef_to_wgs84fix(&session->gpsdata, - getled(buf, 14), getled(buf, 22), getled(buf, 30), - getlef(buf, 38), getlef(buf, 42), getlef(buf, 46)); - mask |= LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET | CLIMB_SET ; - - session->gpsdata.satellites_used = getub(buf, 79) & 0x0f; - session->gpsdata.hdop = getleuw(buf, 74) * 0.1; - session->gpsdata.vdop = getleuw(buf, 76) * 0.1; - /* other DOP if available */ - mask |= HDOP_SET | VDOP_SET | USED_SET; - - flags = getub(buf,70); - switch (flags & 0x1f) { - case 2: - session->gpsdata.fix.mode = MODE_3D; - session->gpsdata.status = STATUS_FIX; - break; - case 4: - session->gpsdata.fix.mode = MODE_3D; - session->gpsdata.status = STATUS_DGPS_FIX; - break; - case 5: - session->gpsdata.fix.mode = MODE_2D; - session->gpsdata.status = STATUS_DGPS_FIX; - break; - case 3: - case 6: - session->gpsdata.fix.mode = MODE_2D; - session->gpsdata.status = STATUS_FIX; - break; - default: - session->gpsdata.status = STATUS_NO_FIX; - session->gpsdata.fix.mode = MODE_NO_FIX; - } - - /* CYCLE_START_SET if this message starts a reporting period */ - mask |= MODE_SET | STATUS_SET | CYCLE_START_SET; - - return mask; -} - -/** - * GPS Satellite Info - */ -static gps_mask_t -superstar2_msg_svinfo(struct gps_device_t *session, - unsigned char *buf, size_t data_len) -{ - unsigned char i, st, nchan, nsv; - - if (data_len != 67) - return 0; - - gpsd_report(LOG_PROG, "superstar2 #33 - satellite data"); - - nchan = 12; - gpsd_zero_satellites(&session->gpsdata); - nsv = 0; /* number of actually used satellites */ - for (i = st = 0; i < nchan; i++) { - /* get info for one channel/satellite */ - int off = i*5 + 5; - int porn; - if ((porn = getub(buf, off) & 0x1f) == 0) - porn = (getub(buf, off+3) >> 1) + 87; - - session->gpsdata.PRN[i] = porn; - session->gpsdata.ss[i] = getub(buf, off+4); - session->gpsdata.elevation[i] = getsb(buf, off+1); - session->gpsdata.azimuth[i] = (unsigned short)getub(buf, off+2) + ((unsigned short)(getub(buf, off+3) & 0x1) << 1); - - if ((getub(buf, off) & 0x60) == 0x60) - session->gpsdata.used[nsv++] = session->gpsdata.PRN[i]; - - if(session->gpsdata.PRN[i]) - st++; - } - session->gpsdata.satellites_used = nsv; - session->gpsdata.satellites = st; - return SATELLITE_SET | USED_SET | ONLINE_SET; -} - -static gps_mask_t -superstar2_msg_version(struct gps_device_t *session, - unsigned char *buf, size_t data_len) -{ -#define SZ 16 - char main_sw[SZ], hw_part[SZ], boot_sw[SZ], ser_num[SZ]; - - /* byte 98 is device type, value = 3 means superstar2 */ - if ((data_len != 101) || ((getub(buf,98) & 0x0f) != 3)) - return 0; - - snprintf(main_sw, 15, "%s", buf+4); - snprintf(hw_part, 15, "%s", buf+18); - snprintf(boot_sw, 15, "%s", buf+36); - snprintf(ser_num, 14, "%s", buf+73); - - gpsd_report(LOG_PROG, - "superstar2 #45 - " - "hw part %s boot sw %s main sw %s ser num %s\n", - hw_part, boot_sw, main_sw, ser_num); - strlcpy(session->subtype, main_sw, sizeof(session->subtype)); - return DEVICEID_SET | ONLINE_SET; -} - -/** - * GPS Leap Seconds - */ -static gps_mask_t -superstar2_msg_timing(struct gps_device_t *session, unsigned char *buf, size_t data_len) -{ - union long_double l_d; - double d; - struct tm tm; - - if (data_len != 65) - return 0; - - gpsd_report(LOG_PROG, "superstar2 #113 - timing status\n"); - if ((getub(buf, 55) & 0x30) != 0) - return ONLINE_SET; - - /* extract time data */ - bzero(&tm, sizeof(tm)); - tm.tm_mday = getsb(buf, 37); - tm.tm_mon = getsb(buf, 38) - 1; - tm.tm_year = getlesw(buf, 39) - 1900; - - tm.tm_hour = getsb(buf, 41); - tm.tm_min = getsb(buf, 42); - d = getled(buf, 43); - tm.tm_sec = (int)d; - session->gpsdata.sentence_time = session->gpsdata.fix.time = timegm(&tm); - session->context->leap_seconds = getsb(buf,20); - - return TIME_SET | ONLINE_SET; -} - - -/** - * Write data to the device, doing any required padding or checksumming - */ -static ssize_t -superstar2_control_send(struct gps_device_t *session, char *msg, size_t msglen) -{ - unsigned short c = 0; - size_t i; - - for (i = 0; i < msglen - 2; i++) - c += (unsigned char)msg[i]; -// c = htons(c); // XXX is this needed on big-endian machines? - memcpy(msg + msg[3] + 4, &c, 2); - gpsd_report(LOG_IO, "writing superstar2 control type %02x len %zu:%s\n", - (unsigned char)msg[1], msglen, - gpsd_hexdump_wrapper(msg, msglen, LOG_IO)); - return gpsd_write(session, msg, msglen); -} - -/** - * Parse the data from the device - */ -gps_mask_t -superstar2_dispatch(struct gps_device_t *session, unsigned char *buf, - size_t len) -{ - int type; - - if (len == 0) - return 0; - - type = buf[SUPERSTAR2_TYPE_OFFSET]; - (void)snprintf(session->gpsdata.tag, - sizeof(session->gpsdata.tag), "SS2-%u", (int)type); - - switch (type) - { - case SUPERSTAR2_ACK: /* Message Acknowledgement */ - return superstar2_msg_ack(session, buf, len); - case SUPERSTAR2_SVINFO: /* Satellite Visibility Data */ - return superstar2_msg_svinfo(session, buf, len); - case SUPERSTAR2_NAVSOL_LLA: /* Navigation Data */ - return superstar2_msg_navsol_lla(session, buf, len); - case SUPERSTAR2_NAVSOL_ECEF: /* Navigation Data */ - return superstar2_msg_navsol_ecef(session, buf, len); - case SUPERSTAR2_VERSION: /* Hardware/Software Version */ - return superstar2_msg_version(session, buf, len); - case SUPERSTAR2_TIMING: /* Timing Parameters */ - return superstar2_msg_timing(session, buf, len); - - default: - /* XXX This gets noisy in a hurry. */ - gpsd_report(LOG_WARN, - "unknown superstar2 packet id 0x%02x length %zd: %s\n", - type, len, gpsd_hexdump_wrapper(buf, len, LOG_WARN)); - return 0; - } -} - -/********************************************************** - * - * Externally called routines below here - * - **********************************************************/ -/* canned config messages */ -/* Initiate Link ID# 63 */ -static unsigned char link_msg[] = {0x01, 0x3f, 0xc0, 0x08, - 0x55, 0x47, 0x50, 0x53, 0x2d, 0x30, 0x30, 0x30, - 0x00, 0x00}; - -/* Request Hardware/Software Identification ID# 45 */ -static unsigned char version_msg[] = {0x01, 0x2d, 0xd2, 0x00, 0x00, 0x01}; - -static void -superstar2_probe_wakeup(struct gps_device_t *session) -{ - superstar2_control_send(session, link_msg, sizeof(link_msg)); - usleep(300000); - superstar2_control_send(session, version_msg, sizeof(version_msg)); - return; -} - -static void -superstar2_probe_subtype(struct gps_device_t *session, - unsigned int seq) -{ - if (seq == 0){ - superstar2_control_send(session, link_msg, sizeof(link_msg)); - usleep(300000); - superstar2_control_send(session, version_msg, sizeof(version_msg)); - } - return; -} - -static void superstar2_configurator(struct gps_device_t *session, - unsigned int seq UNUSED) -{ - unsigned char a; - unsigned char message_list[] = { - SUPERSTAR2_NAVSOL_LLA, - SUPERSTAR2_SVINFO, - SUPERSTAR2_TIMING, - SUPERSTAR2_NAVSOL_ECEF, - SUPERSTAR2_DUMMY}; - unsigned char message2_list[] = { - SUPERSTAR2_MEASUREMENT, - SUPERSTAR2_DUMMY}; - unsigned char tmpl_msg[] = { 0x01, 0x00, 0x00, 0x00, 0x00, 0x00}; - unsigned char tmpl2_msg[] = { 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00}; - - for(a = 0; message_list[a] != 0; a++){ - /* set high bit to enable continuous output */ - tmpl_msg[1] = (unsigned char)(message_list[a] | 0x80); - tmpl_msg[2] = (unsigned char)(tmpl_msg[1] ^ 0xff); - superstar2_control_send(session, tmpl_msg, sizeof(tmpl_msg)); - usleep(20000); - } - for(a = 0; message2_list[a] != 0; a++){ - /* set high bit to enable continuous output */ - tmpl2_msg[1] = (unsigned char)(message2_list[a] | 0x80); - tmpl2_msg[2] = (unsigned char)(tmpl2_msg[1] ^ 0xff); - superstar2_control_send(session, tmpl2_msg, sizeof(tmpl2_msg)); - usleep(20000); - } - superstar2_control_send(session, version_msg, sizeof(version_msg)); -} - -/* - * This is the entry point to the driver. When the packet sniffer recognizes - * a packet for this driver, it calls this method which passes the packet to - * the binary processor or the nmea processor, depending on the session type. - */ -static gps_mask_t superstar2_parse_input(struct gps_device_t *session) -{ - gps_mask_t st; - - if (session->packet.type == SUPERSTAR2_PACKET){ - st = superstar2_dispatch(session, session->packet.outbuffer, - session->packet.length); - session->gpsdata.driver_mode = MODE_BINARY; - return st; -#ifdef NMEA_ENABLE - } else if (session->packet.type == NMEA_PACKET) { - st = nmea_parse((char *)session->packet.outbuffer, session); - (void)gpsd_switch_driver(session, "Generic NMEA"); - session->gpsdata.driver_mode = MODE_NMEA; - return st; -#endif /* NMEA_ENABLE */ - } else - return 0; -} - -static bool superstar2_set_speed(struct gps_device_t *session, - speed_t speed, char parity, int stopbits) -{ - /* set port operating mode, speed, bits etc. here */ - return false; -} - -/* - * Switch between NMEA and binary mode, if supported - */ -static void superstar2_set_mode(struct gps_device_t *session, int mode) -{ - if (mode == MODE_NMEA) { - // superstar2_to_nmea(session->gpsdata.gps_fd,session->gpsdata.baudrate); /* send the mode switch control string */ - } else { - session->back_to_nmea = false; - } -} - -const struct gps_type_t superstar2_binary = { - /* Full name of type */ - .type_name = "SuperStarII binary", - /* associated lexer packet type */ - .packet_type = SUPERSTAR2_PACKET, - /* Response string that identifies device (not active) */ - .trigger = NULL, - /* Number of satellite channels supported by the device */ - .channels = 12, - /* Control string sender - should provide checksum and trailer */ - .control_send = superstar2_control_send, - /* Startup-time device detector */ - .probe_detect = NULL, - /* Wakeup to be done before each baud hunt */ - .probe_wakeup = superstar2_probe_wakeup, - /* Initialize the device and get subtype */ - .probe_subtype = superstar2_probe_subtype, -#ifdef ALLOW_RECONFIGURE - /* Enable what reports we need */ - .configurator = superstar2_configurator, -#endif /* ALLOW_RECONFIGURE */ - /* Packet getter (using default routine) */ - .get_packet = generic_get, - /* Parse message packets */ - .parse_packet = superstar2_parse_input, - /* RTCM handler (using default routine) */ - .rtcm_writer = pass_rtcm, - /* Speed (baudrate) switch */ - .speed_switcher = superstar2_set_speed, - /* Switch to NMEA mode */ - .mode_switcher = superstar2_set_mode, - /* Message delivery rate switcher (not active) */ - .rate_switcher = NULL, - /* Number of chars per report cycle (not active) */ - .cycle_chars = -1, -#ifdef ALLOW_RECONFIGURE - /* Undo the actions of .configurator */ - .revert = NULL, -#endif /* ALLOW_RECONFIGURE */ - /* Puts device back to original settings */ - .wrapup = NULL, - /* Number of updates per second */ - .cycle = 1 -}; -#endif /* defined(SUPERSTAR2_ENABLE) && defined(BINARY_ENABLE) */ diff --git a/superstar2.h b/superstar2.h deleted file mode 100644 index 8b1529ca..00000000 --- a/superstar2.h +++ /dev/null @@ -1,59 +0,0 @@ -/* $Id$ */ -#ifndef _GPSD_SUPERSTAR2_H_ -#define _GPSD_SUPERSTAR2_H_ - -#define SUPERSTAR2_BASE_SIZE 4 -#define SUPERSTAR2_TYPE_OFFSET 1 - -/* input-only */ -#define SUPERSTAR2_RESET 2 -#define SUPERSTAR2_LINKUP 63 -#define SUPERSTAR2_CHANNEL_INHIBIT 64 -#define SUPERSTAR2_TIME_PARAMS 69 -#define SUPERSTAR2_ALMANAC_INCREMENT 77 -#define SUPERSTAR2_ALMANAC_UPLOAD 79 -#define SUPERSTAR2_SET_OPMODE 80 -#define SUPERSTAR2_SET_MASK 81 -#define SUPERSTAR2_SET_DGPS 83 -#define SUPERSTAR2_SET_IONOMODEL 84 -#define SUPERSTAR2_SET_MSLMODEL 86 -#define SUPERSTAR2_SET_HEIGHT_MODE 87 -#define SUPERSTAR2_SET_DATUM 88 -#define SUPERSTAR2_SATELLITE_INHIBIT 90 -#define SUPERSTAR2_BASE_CONFIG 91 -#define SUPERSTAR2_SATELLITE_TRACK 95 -#define SUPERSTAR2_NVM_ERASE 99 -#define SUPERSTAR2_SET_TIME 103 -#define SUPERSTAR2_MESSAGE_CONFIG 105 -#define SUPERSTAR2_SERIAL_CONFIG 110 - -/* output-only */ -#define SUPERSTAR2_CHANINF2 7 -#define SUPERSTAR2_LINKERR 125 -#define SUPERSTAR2_ACK 126 - -/* bidirectional */ -#define SUPERSTAR2_DUMMY 0 -#define SUPERSTAR2_CHANINF 6 -#define SUPERSTAR2_NAVSOL_LLA 20 -#define SUPERSTAR2_NAVSOL_ECEF 21 -#define SUPERSTAR2_EPHEMERIS 22 -#define SUPERSTAR2_MEASUREMENT 23 -#define SUPERSTAR2_RECV_CONFIG 30 -#define SUPERSTAR2_SVINFO 33 -#define SUPERSTAR2_DGPSCONFIG 43 -#define SUPERSTAR2_VERSION 45 -#define SUPERSTAR2_BASE_STATUS 47 -#define SUPERSTAR2_DGPS_STATUS 48 -#define SUPERSTAR2_RECV_STATUS 49 -#define SUPERSTAR2_SAT_HEALTH 50 -#define SUPERSTAR2_SELFTEST 51 -#define SUPERSTAR2_RTCM_DATA 65 -#define SUPERSTAR2_SBAS_DATA 67 -#define SUPERSTAR2_SBAS_STATUS 68 -#define SUPERSTAR2_IONO_UTC 75 -#define SUPERSTAR2_ALMANAC_DATA 76 -#define SUPERSTAR2_ALMANAC_STATUS 78 -#define SUPERSTAR2_TIMING 113 - -#endif /* _GPSD_SUPERSTAR2_H_ */ diff --git a/tsip.c b/tsip.c deleted file mode 100644 index 7ee86523..00000000 --- a/tsip.c +++ /dev/null @@ -1,856 +0,0 @@ -/* $Id$ */ -/* - * Handle the Trimble TSIP packet format - * by Rob Janssen, PE1CHL. - */ -#include -#include -#include -#include -#include -#include "gpsd_config.h" -#include "gpsd.h" -#include "bits.h" - -#define USE_SUPERPACKET 1 /* use Super Packet mode? */ - -#define SEMI_2_DEG (180.0 / 2147483647) /* 2^-31 semicircle to deg */ - -#ifdef TSIP_ENABLE -#define TSIP_CHANNELS 12 - -static int tsip_write(struct gps_device_t *session, - unsigned int id, /*@null@*/unsigned char *buf, size_t len) -{ -#ifdef ALLOW_RECONFIGURE - char *ep, *cp; - - gpsd_report(LOG_IO, "Sent TSIP packet id 0x%02x: %s\n", id, - gpsd_hexdump_wrapper(buf, len, LOG_IO)); - - /*@ +charint @*/ - session->msgbuf[0] = '\x10'; - session->msgbuf[1] = (char)id; - ep = session->msgbuf + 2; - /*@ -nullderef @*/ - for (cp = (char *)buf; len-- > 0; cp++) { - if (*cp == '\x10') - *ep++ = '\x10'; - *ep++ = *cp; - } - /*@ +nullderef @*/ - *ep++ = '\x10'; - *ep++ = '\x03'; - session->msgbuflen = (size_t)(ep - (char*)buf); - /*@ -charint @*/ - if (gpsd_write(session,session->msgbuf,session->msgbuflen) != (ssize_t)len) - return -1; - - return 0; -#else - gpsd_report(LOG_IO, "Not sending TSIP packet id 0x%02x: %s\n", id, - gpsd_hexdump_wrapper(buf,len,LOG_IO)); - return -1; -#endif /* ALLOW_RECONFIGURE */ -} - -static ssize_t tsip_control_send(struct gps_device_t *session, - char *buf, size_t buflen) -/* not used by the daemon, it's for gpsctl and friends */ -{ - return (ssize_t)tsip_write(session, - (unsigned int)buf[0], (unsigned char *)buf+1, buflen-1); -} - -static void tsip_probe_subtype(struct gps_device_t *session, unsigned int seq) -{ - unsigned char buf[100]; - - switch (seq) { - case 0: - /* TSIP is ODD parity 1 stopbit, save original values and change it */ - /* XXX Thunderbolts and Copernicus use 8N1... which isn't exactly a */ - /* XXX good idea due to the fragile wire format. We must divine a */ - /* XXX clever heuristic to decide if the parity change is required. */ - session->driver.tsip.parity = session->gpsdata.parity; - session->driver.tsip.stopbits = session->gpsdata.stopbits; - gpsd_set_speed(session, session->gpsdata.baudrate, 'O', 1); - break; - - case 1: - /* Request Software Versions */ - (void)tsip_write(session, 0x1f, NULL, 0); - /* Request Current Time */ - (void)tsip_write(session, 0x21, NULL, 0); - /* Request GPS Systems Message */ - (void)tsip_write(session, 0x28, NULL, 0); - /* Request Current Datum Values */ - putbyte(buf,0,0x15); - (void)tsip_write(session, 0x8e, buf, 1); - /* Request Navigation Configuration */ - putbyte(buf,0,0x03); - (void)tsip_write(session, 0xbb, buf, 1); - break; - } -} - -#ifdef ALLOW_RECONFIGURE -static void tsip_configurator(struct gps_device_t *session, unsigned int seq) -{ - if (seq == 0) { - unsigned char buf[100]; - - /* I/O Options */ - putbyte(buf,0,0x1e); /* Position: DP, MSL, LLA */ - putbyte(buf,1,0x02); /* Velocity: ENU */ - putbyte(buf,2,0x00); /* Time: GPS */ - putbyte(buf,3,0x08); /* Aux: dBHz */ - (void)tsip_write(session, 0x35, buf, 4); - } -} -#endif /* ALLOW_RECONFIGURE */ - -static void tsip_wrapup(struct gps_device_t *session) -{ - /* restore saved parity and stopbits when leaving TSIP mode */ - gpsd_set_speed(session, - session->gpsdata.baudrate, - (unsigned char)session->driver.tsip.parity, - session->driver.tsip.stopbits); -} - -static bool tsip_speed_switch(struct gps_device_t *session, - unsigned int speed, - char parity, int stopbits) -{ - /* parity and stopbit switching are not yet implemented */ - if (parity!=(char)session->gpsdata.parity || (unsigned int)stopbits!=session->gpsdata.parity) { - return false; - } else { - unsigned char buf[100]; - - putbyte(buf,0,0xff); /* current port */ - putbyte(buf,1,(round(log((double)speed/300)/M_LN2))+2); /* input baudrate */ - putbyte(buf,2,getub(buf,1)); /* output baudrate */ - putbyte(buf,3,8); /* character width (8 bits) */ - putbyte(buf,4,1); /* parity (odd) */ - putbyte(buf,5,0); /* stop bits (1 stopbit) */ - putbyte(buf,6,0); /* flow control (none) */ - putbyte(buf,7,0x02); /* input protocol (TSIP) */ - putbyte(buf,8,0x02); /* output protocol (TSIP) */ - putbyte(buf,9,0); /* reserved */ - (void)tsip_write(session, 0xbc, buf, 10); - - return true; /* it would be nice to error-check this */ - } -} - -static gps_mask_t tsip_analyze(struct gps_device_t *session) -{ - int i, j, len, count; - gps_mask_t mask = 0; - unsigned int id; - u_int8_t u1,u2,u3,u4,u5; - int16_t s1,s2,s3,s4; - int32_t sl1,sl2,sl3; - u_int32_t ul1,ul2; - float f1,f2,f3,f4,f5; - double d1,d2,d3,d4,d5; - union int_float i_f; - union long_double l_d; - time_t now; - unsigned char buf[BUFSIZ]; - char buf2[BUFSIZ]; - - if (session->packet.type != TSIP_PACKET) { - gpsd_report(LOG_INF, "tsip_analyze packet type %d\n",session->packet.type); - return 0; - } - - /*@ +charint @*/ - if (session->packet.outbuflen < 4 || session->packet.outbuffer[0] != 0x10) - return 0; - - /* remove DLE stuffing and put data part of message in buf */ - - memset(buf, 0, sizeof(buf)); - buf2[len = 0] = '\0'; - for (i = 2; i < (int)session->packet.outbuflen; i++) { - if (session->packet.outbuffer[i] == 0x10) - if (session->packet.outbuffer[++i] == 0x03) - break; - - (void)snprintf(buf2+strlen(buf2), - sizeof(buf2)-strlen(buf2), - "%02x", buf[len++] = session->packet.outbuffer[i]); - } - /*@ -charint @*/ - - (void)snprintf(session->gpsdata.tag, sizeof(session->gpsdata.tag), - "ID%02x", id = (unsigned)session->packet.outbuffer[1]); - - gpsd_report(LOG_IO, "TSIP packet id 0x%02x length %d: %s\n",id,len,buf2); - (void)time(&now); - - switch (id) { - case 0x13: /* Packet Received */ - u1 = getub(buf,0); - u2 = getub(buf,1); - gpsd_report(LOG_WARN, "Received packet of type %02x cannot be parsed\n",u1); -#if USE_SUPERPACKET - if ((int)u1 == 0x8e && (int)u2 == 0x23) { /* no Compact Super Packet */ - gpsd_report(LOG_WARN, "No Compact Super Packet, use LFwEI\n"); - - /* Request LFwEI Super Packet */ - putbyte(buf,0,0x20); - putbyte(buf,1,0x01); /* enabled */ - (void)tsip_write(session, 0x8e, buf, 2); - } -#endif /* USE_SUPERPACKET */ - break; - case 0x41: /* GPS Time */ - if (len != 10) - break; - session->driver.tsip.last_41 = now; /* keep timestamp for request */ - f1 = getbef(buf,0); /* gpstime */ - s1 = getbesw(buf,4); /* week */ - f2 = getbef(buf,6); /* leap seconds */ - if (f1 >= 0.0 && f2 > 10.0) { - session->driver.tsip.gps_week = s1; - session->context->leap_seconds = (int)round(f2); - session->context->valid |= LEAP_SECOND_VALID; - - session->gpsdata.sentence_time = gpstime_to_unix((int)s1, f1) - f2; - -#ifdef NTPSHM_ENABLE - if (session->context->enable_ntpshm) - (void)ntpshm_put(session,session->gpsdata.sentence_time+0.075); -#endif - mask |= TIME_SET; - } - gpsd_report(LOG_INF, "GPS Time %f %d %f\n",f1,s1,f2); - break; - case 0x42: /* Single-Precision Position Fix, XYZ ECEF */ - if (len != 16) - break; - f1 = getbef(buf,0); /* X */ - f2 = getbef(buf,4); /* Y */ - f3 = getbef(buf,8); /* Z */ - f4 = getbef(buf,12); /* time-of-fix */ - gpsd_report(LOG_INF, "GPS Position XYZ %f %f %f %f\n",f1,f2,f3,f4); - break; - case 0x43: /* Velocity Fix, XYZ ECEF */ - if (len != 20) - break; - f1 = getbef(buf,0); /* X velocity */ - f2 = getbef(buf,4); /* Y velocity */ - f3 = getbef(buf,8); /* Z velocity */ - f4 = getbef(buf,12); /* bias rate */ - f5 = getbef(buf,16); /* time-of-fix */ - gpsd_report(LOG_INF, "GPS Velocity XYZ %f %f %f %f %f\n",f1,f2,f3,f4,f5); - break; - case 0x45: /* Software Version Information */ - if (len != 10) - break; - /*@ -formattype @*/ - (void)snprintf(session->subtype, sizeof(session->subtype), - "%d.%d %02d%02d%02d %d.%d %02d%02d%02d", - getub(buf,0),getub(buf,1),getub(buf,4),getub(buf,2),getub(buf,3), - getub(buf,5),getub(buf,6),getub(buf,9),getub(buf,7),getub(buf,8)); - /*@ +formattype @*/ - gpsd_report(LOG_INF, "Software version: %s\n", session->subtype); - mask |= DEVICEID_SET; - break; - case 0x46: /* Health of Receiver */ - if (len != 2) - break; - session->driver.tsip.last_46 = now; - u1 = getub(buf,0); /* Status code */ - u2 = getub(buf,1); /* Antenna/Battery */ - if (u1 != (u_int8_t)0) { - session->gpsdata.status = STATUS_NO_FIX; - mask |= STATUS_SET; - } - else { - if (session->gpsdata.status < STATUS_FIX) { - session->gpsdata.status = STATUS_FIX; - mask |= STATUS_SET; - } - } - gpsd_report(LOG_PROG, "Receiver health %02x %02x\n",u1,u2); - break; - case 0x47: /* Signal Levels for all Satellites */ - count = (int)getub(buf,0); /* satellite count */ - if (len != (5*count + 1)) - break; - buf2[0] = '\0'; - for (i = 0; i < count; i++) { - u1 = getub(buf,5*i + 1); - if ((f1 = getbef(buf,5*i + 2)) < 0) - f1 = 0.0; - for (j = 0; j < TSIP_CHANNELS; j++) - if (session->gpsdata.PRN[j] == (int)u1) { - session->gpsdata.ss[j] = (int)round(f1); - break; - } - (void)snprintf(buf2+strlen(buf2), sizeof(buf2)-strlen(buf2), - " %d=%.1f",(int)u1,f1); - } - gpsd_report(LOG_PROG, "Signal Levels (%d):%s\n",count,buf2); - mask |= SATELLITE_SET; - break; - case 0x48: /* GPS System Message */ - buf[len] = '\0'; - gpsd_report(LOG_PROG, "GPS System Message: %s\n",buf); - break; - case 0x4a: /* Single-Precision Position LLA */ - if (len != 20) - break; - session->gpsdata.fix.latitude = getbef(buf,0) * RAD_2_DEG; - session->gpsdata.fix.longitude = getbef(buf,4) * RAD_2_DEG; - session->gpsdata.fix.altitude = getbef(buf,8); - f1 = getbef(buf,12); /* clock bias */ - f2 = getbef(buf,16); /* time-of-fix */ - if (session->driver.tsip.gps_week) { - session->gpsdata.fix.time = session->gpsdata.sentence_time = - gpstime_to_unix((int)session->driver.tsip.gps_week, f2) - session->context->leap_seconds; - mask |= TIME_SET; - } - gpsd_report(LOG_PROG, "GPS LLA %f %f %f %f\n", - session->gpsdata.fix.time, - session->gpsdata.fix.latitude, - session->gpsdata.fix.longitude, - session->gpsdata.fix.altitude); - mask |= LATLON_SET | ALTITUDE_SET | CYCLE_START_SET; - break; - case 0x4b: /* Machine/Code ID and Additional Status */ - if (len != 3) - break; - u1 = getub(buf,0); /* Machine ID */ - u2 = getub(buf,1); /* Status 1 */ - u3 = getub(buf,2); /* Status 2 */ - gpsd_report(LOG_INF, "Machine ID %02x %02x %02x\n",u1,u2,u3); -#if USE_SUPERPACKET - if ((u3 & 0x01) != (u_int8_t)0 && !session->driver.tsip.superpkt) { - gpsd_report(LOG_PROG, "Switching to Super Packet mode\n"); - - /* set new I/O Options for Super Packet output */ - putbyte(buf,0,0x2c); /* Position: SP, MSL */ - putbyte(buf,1,0x00); /* Velocity: none (via SP) */ - putbyte(buf,2,0x00); /* Time: GPS */ - putbyte(buf,3,0x08); /* Aux: dBHz */ - (void)tsip_write(session, 0x35, buf, 4); - session->driver.tsip.superpkt = true; - } -#endif /* USE_SUPERPACKET */ - break; - case 0x55: /* IO Options */ - if (len != 4) - break; - u1 = getub(buf,0); /* Position */ - u2 = getub(buf,1); /* Velocity */ - u3 = getub(buf,2); /* Timing */ - u4 = getub(buf,3); /* Aux */ - gpsd_report(LOG_INF, "IO Options %02x %02x %02x %02x\n",u1,u2,u3,u4); -#if USE_SUPERPACKET - if ((u1 & 0x20) != (u_int8_t)0) { /* Output Super Packets? */ - /* No LFwEI Super Packet */ - putbyte(buf,0,0x20); - putbyte(buf,1,0x00); /* disabled */ - (void)tsip_write(session, 0x8e, buf, 2); - - /* Request Compact Super Packet */ - putbyte(buf,0,0x23); - putbyte(buf,1,0x01); /* enabled */ - (void)tsip_write(session, 0x8e, buf, 2); - } -#endif /* USE_SUPERPACKET */ - break; - case 0x56: /* Velocity Fix, East-North-Up (ENU) */ - if (len != 20) - break; - f1 = getbef(buf,0); /* East velocity */ - f2 = getbef(buf,4); /* North velocity */ - f3 = getbef(buf,8); /* Up velocity */ - f4 = getbef(buf,12); /* clock bias rate */ - f5 = getbef(buf,16); /* time-of-fix */ - session->gpsdata.fix.climb = f3; - /*@ -evalorder @*/ - session->gpsdata.fix.speed = sqrt(pow(f2,2) + pow(f1,2)); - /*@ +evalorder @*/ - if ((session->gpsdata.fix.track = atan2(f1,f2) * RAD_2_DEG) < 0) - session->gpsdata.fix.track += 360.0; - gpsd_report(LOG_INF, "GPS Velocity ENU %f %f %f %f %f\n",f1,f2,f3,f4,f5); - mask |= SPEED_SET | TRACK_SET | CLIMB_SET; - break; - case 0x57: /* Information About Last Computed Fix */ - if (len != 8) - break; - u1 = getub(buf,0); /* Source of information */ - u2 = getub(buf,1); /* Mfg. diagnostic */ - f1 = getbef(buf,2); /* gps_time */ - s1 = getbesw(buf,6); /* tsip.gps_week */ - /*@ +charint @*/ - if (getub(buf,0) == 0x01) /* good current fix? */ - session->driver.tsip.gps_week = s1; - /*@ -charint @*/ - gpsd_report(LOG_INF, "Fix info %02x %02x %d %f\n",u1,u2,s1,f1); - break; - case 0x58: /* Satellite System Data/Acknowledge from Receiver */ - break; - case 0x59: /* Status of Satellite Disable or Ignore Health */ - break; - case 0x5a: /* Raw Measurement Data */ - if (len != 29) - break; - f1 = getbef(buf,5); /* Signal Level */ - f2 = getbef(buf,9); /* Code phase */ - f3 = getbef(buf,13); /* Doppler */ - d1 = getbed(buf,17); /* Time of Measurement */ - gpsd_report(LOG_PROG, "Raw Measurement Data %d %f %f %f %f\n",getub(buf,0),f1,f2,f3,d1); - break; - case 0x5c: /* Satellite Tracking Status */ - if (len != 24) - break; - session->driver.tsip.last_5c = now; /* keep timestamp for request */ - u1 = getub(buf,0); /* PRN */ - u2 = getub(buf,1); /* chan */ - u3 = getub(buf,2); /* Acquisition flag */ - u4 = getub(buf,3); /* Ephemeris flag */ - f1 = getbef(buf,4); /* Signal level */ - f2 = getbef(buf,8); /* time of Last measurement */ - d1 = getbef(buf,12) * RAD_2_DEG; /* Elevation */ - d2 = getbef(buf,16) * RAD_2_DEG; /* Azimuth */ - i = (int)(u2 >> 3); /* channel number */ - gpsd_report(LOG_INF, "Satellite Tracking Status: Ch %2d PRN %3d Res %d Acq %d Eph %2d SNR %4.1f LMT %.04f El %4.1f Az %5.1f\n",i,u1,u2&7,u3,u4,f1,f2,d1,d2); - if (i < TSIP_CHANNELS) { - if (d1 >= 0.0) { - session->gpsdata.PRN[i] = (int)u1; - session->gpsdata.ss[i] = (int)round(f1); - session->gpsdata.elevation[i] = (int)round(d1); - session->gpsdata.azimuth[i] = (int)round(d2); - } else { - session->gpsdata.PRN[i] = session->gpsdata.ss[i] = - session->gpsdata.elevation[i] = session->gpsdata.azimuth[i] = 0; - } - if (++i == session->gpsdata.satellites) - mask |= SATELLITE_SET; /* last of the series */ - if (i > session->gpsdata.satellites) - session->gpsdata.satellites = i; - } - break; - case 0x6d: /* All-In-View Satellite Selection */ - u1 = getub(buf,0); /* nsvs/dimension */ - count = (int)((u1 >> 4) & 0x0f); - if (len != (17 + count)) - break; - session->driver.tsip.last_6d = now; /* keep timestamp for request */ - switch (u1 & 7) /* dimension */ - { - case 3: - //session->gpsdata.status = STATUS_FIX; - session->gpsdata.fix.mode = MODE_2D; - break; - case 4: - //session->gpsdata.status = STATUS_FIX; - session->gpsdata.fix.mode = MODE_3D; - break; - default: - //session->gpsdata.status = STATUS_NO_FIX; - session->gpsdata.fix.mode = MODE_NO_FIX; - break; - } - session->gpsdata.satellites_used = count; - session->gpsdata.pdop = getbef(buf,1); - session->gpsdata.hdop = getbef(buf,5); - session->gpsdata.vdop = getbef(buf,9); - session->gpsdata.tdop = getbef(buf,13); - /*@ -evalorder @*/ - session->gpsdata.gdop = sqrt(pow(session->gpsdata.pdop,2)+pow(session->gpsdata.tdop,2)); - /*@ +evalorder @*/ - - memset(session->gpsdata.used,0,sizeof(session->gpsdata.used)); - buf2[0] = '\0'; - /*@ +charint @*/ - for (i = 0; i < count; i++) - (void)snprintf(buf2+strlen(buf2), sizeof(buf2)-strlen(buf2), - " %d",session->gpsdata.used[i] = getub(buf,17 + i)); - /*@ -charint @*/ - - gpsd_report(LOG_INF, "Sat info: mode %d, satellites used %d: %s\n", - session->gpsdata.fix.mode, session->gpsdata.satellites_used,buf2); - gpsd_report(LOG_INF, - "Sat info: DOP P=%.1f H=%.1f V=%.1f T=%.1f G=%.1f\n", - session->gpsdata.pdop, session->gpsdata.hdop, - session->gpsdata.vdop, session->gpsdata.tdop, - session->gpsdata.gdop); - mask |= HDOP_SET | VDOP_SET | PDOP_SET | TDOP_SET | GDOP_SET | STATUS_SET | MODE_SET | USED_SET; - break; - case 0x6e: /* Synchronized Measurements */ - break; - case 0x6f: /* Synchronized Measurements Report */ - /*@ +charint @*/ - if (len < 20 || getub(buf,0) != 1 || getub(buf,1) != 2) - break; - /*@ -charint @*/ - s1 = getbesw(buf,2); /* number of bytes */ - u1 = getub(buf,20); /* number of SVs */ - break; - case 0x70: /* Filter Report */ - break; - case 0x7a: /* NMEA settings */ - break; - case 0x82: /* Differential Position Fix Mode */ - if (len != 1) - break; - u1 = getub(buf,0); /* fix mode */ - /*@ +charint @*/ - if (session->gpsdata.status == STATUS_FIX && (u1 & 0x01)!=0) { - session->gpsdata.status = STATUS_DGPS_FIX; - mask |= STATUS_SET; - } - /*@ -charint @*/ - gpsd_report(LOG_INF, "DGPS mode %d\n",u1); - break; - case 0x83: /* Double-Precision XYZ Position Fix and Bias Information */ - if (len != 36) - break; - d1 = getbed(buf,0); /* X */ - d2 = getbed(buf,8); /* Y */ - d3 = getbed(buf,16); /* Z */ - d4 = getbed(buf,24); /* clock bias */ - f1 = getbef(buf,32); /* time-of-fix */ - gpsd_report(LOG_INF, "GPS Position XYZ %f %f %f %f %f\n",d1,d2,d3,d4,f1); - break; - case 0x84: /* Double-Precision LLA Position Fix and Bias Information */ - if (len != 36) - break; - session->gpsdata.fix.latitude = getbed(buf,0) * RAD_2_DEG; - session->gpsdata.fix.longitude = getbed(buf,8) * RAD_2_DEG; - session->gpsdata.fix.altitude = getbed(buf,16); - d1 = getbed(buf,24); /* clock bias */ - f1 = getbef(buf,32); /* time-of-fix */ - if (session->driver.tsip.gps_week) { - session->gpsdata.fix.time = session->gpsdata.sentence_time = - gpstime_to_unix((int)session->driver.tsip.gps_week, f1) - session->context->leap_seconds; - mask |= TIME_SET; - } - gpsd_report(LOG_INF, "GPS DP LLA %f %f %f %f\n", - session->gpsdata.fix.time, - session->gpsdata.fix.latitude, - session->gpsdata.fix.longitude, - session->gpsdata.fix.altitude); - mask |= LATLON_SET | ALTITUDE_SET | CYCLE_START_SET; - break; - case 0x8f: /* Super Packet. Well... */ - /*@ +charint @*/ - (void)snprintf(session->gpsdata.tag+strlen(session->gpsdata.tag), - sizeof(session->gpsdata.tag)-strlen(session->gpsdata.tag), - "%02x", u1 = getub(buf,0)); - /*@ -charint @*/ - switch (u1) /* sub-packet ID */ - { - case 0x15: /* Current Datum Values */ - if (len != 43) - break; - s1 = getbesw(buf,1); /* Datum Index */ - d1 = getbed(buf,3); /* DX */ - d2 = getbed(buf,11); /* DY */ - d3 = getbed(buf,19); /* DZ */ - d4 = getbed(buf,27); /* A-axis */ - d5 = getbed(buf,35); /* Eccentricity Squared */ - gpsd_report(LOG_INF, "Current Datum %d %f %f %f %f %f\n",s1,d1,d2,d3,d4,d5); - break; - - case 0x20: /* Last Fix with Extra Information (binary fixed point) */ - /* XXX CSK sez "why does my Lassen iQ output oversize packets?" */ - if ((len != 56) && (len != 64)) - break; - s1 = getbesw(buf,2); /* east velocity */ - s2 = getbesw(buf,4); /* north velocity */ - s3 = getbesw(buf,6); /* up velocity */ - ul1 = getbeul(buf,8); /* time */ - sl1 = getbesl(buf,12); /* latitude */ - ul2 = getbeul(buf,16); /* longitude */ - sl2 = getbesl(buf,20); /* altitude */ - u1 = getub(buf,24); /* velocity scaling */ - u2 = getub(buf,27); /* fix flags */ - u3 = getub(buf,28); /* num svs */ - u4 = getub(buf,29); /* utc offset */ - s4 = getbesw(buf,30); /* tsip.gps_week */ - /* PRN/IODE data follows */ - gpsd_report(LOG_RAW, "LFwEI %d %d %d %u %d %u %u %x %x %u %u %d\n",s1,s2,s3,ul1,sl1,ul2,sl2,u1,u2,u3,u4,s4); - - if ((u1 & 0x01) != (u_int8_t)0) /* check velocity scaling */ - d5 = 0.02; - else - d5 = 0.005; - d1 = s1 * d5; /* east velocity m/s */ - d2 = s2 * d5; /* north velocity m/s */ - session->gpsdata.fix.climb = s3 * d5; /* up velocity m/s */ - /*@ -evalorder @*/ - session->gpsdata.fix.speed = sqrt(pow(d2,2) + pow(d1,2)); - /*@ +evalorder @*/ - if ((session->gpsdata.fix.track = atan2(d1,d2) * RAD_2_DEG) < 0) - session->gpsdata.fix.track += 360.0; - session->gpsdata.fix.latitude = sl1 * SEMI_2_DEG; - if ((session->gpsdata.fix.longitude = ul2 * SEMI_2_DEG) > 180.0) - session->gpsdata.fix.longitude -= 360.0; - session->gpsdata.separation = wgs84_separation(session->gpsdata.fix.latitude, session->gpsdata.fix.longitude); - session->gpsdata.fix.altitude = sl2 * 1e-3 - session->gpsdata.separation;; - session->gpsdata.status = STATUS_NO_FIX; - session->gpsdata.fix.mode = MODE_NO_FIX; - if ((u2 & 0x01) == (u_int8_t)0) { /* Fix Available */ - session->gpsdata.status = STATUS_FIX; - if ((u2 & 0x02) != (u_int8_t)0) /* DGPS Corrected */ - session->gpsdata.status = STATUS_DGPS_FIX; - if ((u2 & 0x04) != (u_int8_t)0) /* Fix Dimension */ - session->gpsdata.fix.mode = MODE_2D; - else - session->gpsdata.fix.mode = MODE_3D; - } - session->gpsdata.satellites_used = (int)u3; - if ((int)u4 > 10) { - session->context->leap_seconds = (int)u4; - session->context->valid |= LEAP_SECOND_VALID; - } - session->driver.tsip.gps_week = s4; - session->gpsdata.fix.time = session->gpsdata.sentence_time = - gpstime_to_unix((int)s4, ul1 * 1e-3) - session->context->leap_seconds; - mask |= TIME_SET | LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET | CLIMB_SET | STATUS_SET | MODE_SET | CYCLE_START_SET; - break; - case 0x23: /* Compact Super Packet */ - /* XXX CSK sez "i don't trust this to not be oversized either." */ - if (len < 29) - break; - ul1 = getbeul(buf,1); /* time */ - s1 = getbesw(buf,5); /* tsip.gps_week */ - u1 = getub(buf,7); /* utc offset */ - u2 = getub(buf,8); /* fix flags */ - sl1 = getbesl(buf,9); /* latitude */ - ul2 = getbeul(buf,13); /* longitude */ - sl3 = getbesl(buf,17); /* altitude */ - s2 = getbesw(buf,21); /* east velocity */ - s3 = getbesw(buf,23); /* north velocity */ - s4 = getbesw(buf,25); /* up velocity */ - gpsd_report(LOG_INF, "CSP %u %d %u %u %d %u %d %d %d %d\n",ul1,s1,u1,u2,sl1,ul2,sl3,s2,s3,s4); - session->driver.tsip.gps_week = s1; - if ((int)u1 > 10) { - session->context->leap_seconds = (int)u1; - session->context->valid |= LEAP_SECOND_VALID; - } - session->gpsdata.fix.time = session->gpsdata.sentence_time = - gpstime_to_unix((int)s1, ul1 * 1e-3) - session->context->leap_seconds; - session->gpsdata.status = STATUS_NO_FIX; - session->gpsdata.fix.mode = MODE_NO_FIX; - if ((u2 & 0x01) == (u_int8_t)0) { /* Fix Available */ - session->gpsdata.status = STATUS_FIX; - if ((u2 & 0x02) != (u_int8_t)0) /* DGPS Corrected */ - session->gpsdata.status = STATUS_DGPS_FIX; - if ((u2 & 0x04) != (u_int8_t)0) /* Fix Dimension */ - session->gpsdata.fix.mode = MODE_2D; - else - session->gpsdata.fix.mode = MODE_3D; - } - session->gpsdata.fix.latitude = sl1 * SEMI_2_DEG; - if ((session->gpsdata.fix.longitude = ul2 * SEMI_2_DEG) > 180.0) - session->gpsdata.fix.longitude -= 360.0; - session->gpsdata.separation = wgs84_separation(session->gpsdata.fix.latitude, session->gpsdata.fix.longitude); - session->gpsdata.fix.altitude = sl3 * 1e-3 - session->gpsdata.separation;; - if ((u2 & 0x20) != (u_int8_t)0) /* check velocity scaling */ - d5 = 0.02; - else - d5 = 0.005; - d1 = s2 * d5; /* east velocity m/s */ - d2 = s3 * d5; /* north velocity m/s */ - session->gpsdata.fix.climb = s4 * d5; /* up velocity m/s */ - /*@ -evalorder @*/ - session->gpsdata.fix.speed = sqrt(pow(d2,2) + pow(d1,2)) * MPS_TO_KNOTS; - /*@ +evalorder @*/ - if ((session->gpsdata.fix.track = atan2(d1,d2) * RAD_2_DEG) < 0) - session->gpsdata.fix.track += 360.0; - mask |= TIME_SET | LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET | CLIMB_SET | STATUS_SET | MODE_SET | CYCLE_START_SET; - break; - - - case 0xab: /* Thunderbolt Timing Superpacket */ - if (len != 17) { - gpsd_report(4, "pkt 0xab len=%d\n", len); - break; - } - session->driver.tsip.last_41 = now; /* keep timestamp for request */ - ul1 = getbeul(buf,1); /* gpstime */ - s1 = (short)getbeuw(buf,5); /* week */ - s2 = getbesw(buf,7); /* leap seconds */ - - session->driver.tsip.gps_week = s1; - if ((int)u1 > 10) { - session->context->leap_seconds = (int)s2; - session->context->valid |= LEAP_SECOND_VALID; - - session->gpsdata.fix.time = session->gpsdata.sentence_time = - gpstime_to_unix((int)s1, (double)ul1) - (double)s2; -#ifdef NTPSHM_ENABLE - if (session->context->enable_ntpshm) - (void)ntpshm_put(session,session->gpsdata.sentence_time+0.075); -#endif - mask |= TIME_SET; - } - - gpsd_report(4, "GPS Time %u %d %d\n", ul1, s1, s2); - break; - - - case 0xac: /* Thunderbolt Position Superpacket */ - if (len != 68) { - gpsd_report(4, "pkt 0xac len=%d\n", len); - - break; - } - session->gpsdata.fix.latitude = getbed(buf,36) * RAD_2_DEG; - session->gpsdata.fix.longitude = getbed(buf,44) * RAD_2_DEG; - session->gpsdata.fix.altitude = getbed(buf,52); - f1 = getbef(buf,16); /* clock bias */ - - u1 = getub(buf, 12); /* GPS Decoding Status */ - u2 = getub(buf, 1); /* Reciever Mode */ - if (u1 != (u_int8_t)0) { - session->gpsdata.status = STATUS_NO_FIX; - mask |= STATUS_SET; - } - else { - if (session->gpsdata.status < STATUS_FIX) { - session->gpsdata.status = STATUS_FIX; - mask |= STATUS_SET; - } - } - - /* Decode Fix modes */ - switch (u2 & 7) { - case 6: /* Clock Hold 2D */ - case 3: /* 2D Position Fix */ - //session->gpsdata.status = STATUS_FIX; - session->gpsdata.fix.mode = MODE_2D; - break; - case 7: /* Thunderbolt overdetermined clock */ - case 4: /* 3D position Fix */ - //session->gpsdata.status = STATUS_FIX; - session->gpsdata.fix.mode = MODE_3D; - break; - default: - //session->gpsdata.status = STATUS_NO_FIX; - session->gpsdata.fix.mode = MODE_NO_FIX; - break; - } - - gpsd_report(4, "GPS status=%u mode=%u\n", u1, u2); - gpsd_report(4, "GPS DP LLA %f %f %f\n",session->gpsdata.fix.latitude, - session->gpsdata.fix.longitude, - session->gpsdata.fix.altitude); - - mask |= LATLON_SET | ALTITUDE_SET | CYCLE_START_SET | STATUS_SET | MODE_SET; - break; - - - default: - gpsd_report(LOG_WARN,"Unhandled TSIP superpacket type 0x%02x\n",u1); - } - break; - case 0xbb: /* Navigation Configuration */ - if (len != 40) - break; - u1 = getub(buf,0); /* Subcode */ - u2 = getub(buf,1); /* Operating Dimension */ - u3 = getub(buf,2); /* DGPS Mode */ - u4 = getub(buf,3); /* Dynamics Code */ - f1 = getbef(buf,5); /* Elevation Mask */ - f2 = getbef(buf,9); /* AMU Mask */ - f3 = getbef(buf,13); /* DOP Mask */ - f4 = getbef(buf,17); /* DOP Switch */ - u5 = getub(buf,21); /* DGPS Age Limit */ - gpsd_report(LOG_INF, "Navigation Configuration %u %u %u %u %f %f %f %f %u\n",u1,u2,u3,u4,f1,f2,f3,f4,u5); - break; - default: - gpsd_report(LOG_WARN,"Unhandled TSIP packet type 0x%02x\n",id); - break; - } - - /* see if it is time to send some request packets for reports that */ - /* the receiver won't send at fixed intervals */ - - if ((now - session->driver.tsip.last_41) > 5) { - /* Request Current Time */ - (void)tsip_write(session, 0x21, buf, 0); - session->driver.tsip.last_41 = now; - } - - if ((now - session->driver.tsip.last_6d) > 5) { - /* Request GPS Receiver Position Fix Mode */ - (void)tsip_write(session, 0x24, buf, 0); - session->driver.tsip.last_6d = now; - } - - if ((now - session->driver.tsip.last_5c) >= 5) { - /* Request Current Satellite Tracking Status */ - putbyte(buf,0,0x00); /* All satellites */ - (void)tsip_write(session, 0x3c, buf, 1); - session->driver.tsip.last_5c = now; - } - - if ((now - session->driver.tsip.last_46) > 5) { - /* Request Health of Receiver */ - (void)tsip_write(session, 0x26, buf, 0); - session->driver.tsip.last_46 = now; - } - - return mask; -} - -static gps_mask_t tsip_parse_input(struct gps_device_t *session) -{ - gps_mask_t st; - - if (session->packet.type == TSIP_PACKET){ - st = tsip_analyze(session); - session->gpsdata.driver_mode = MODE_BINARY; - return st; -#ifdef EVERMORE_ENABLE - } else if (session->packet.type == EVERMORE_PACKET) { - (void)gpsd_switch_driver(session, "EverMore binary"); - st = evermore_parse(session, session->packet.outbuffer, session->packet.outbuflen); - session->gpsdata.driver_mode = MODE_BINARY; - return st; -#endif /* EVERMORE_ENABLE */ - } else - return 0; -} - -/* this is everything we export */ -const struct gps_type_t tsip_binary = -{ - .type_name = "Trimble TSIP", /* full name of type */ - .packet_type = TSIP_PACKET, /* associated lexer packet type */ - .trigger = NULL, /* no trigger */ - .channels = TSIP_CHANNELS, /* consumer-grade GPS */ - .control_send = tsip_control_send,/* how to send commands */ - .probe_wakeup = NULL, /* no wakeup to be done before hunt */ - .probe_detect = NULL, /* no probe */ - .probe_subtype = tsip_probe_subtype, /* no more subtype discovery */ -#ifdef ALLOW_RECONFIGURE - .configurator = tsip_configurator,/* initial mode sets */ -#endif /* ALLOW_RECONFIGURE */ - .get_packet = generic_get, /* use the generic packet getter */ - .parse_packet = tsip_parse_input, /* parse message packets */ - .rtcm_writer = NULL, /* doesn't accept DGPS corrections */ - .speed_switcher = tsip_speed_switch,/* change baud rate */ - .mode_switcher = NULL, /* no mode switcher */ - .rate_switcher = NULL, /* no rate switcher */ - .cycle_chars = -1, /* not relevant, no rate switcher */ -#ifdef ALLOW_RECONFIGURE - .revert = NULL, /* FIXME: revert sentence mix */ -#endif /* ALLOW_RECONFIGURE */ - .wrapup = tsip_wrapup, /* restore comms parameters */ - .cycle = 1, /* updates every second */ -}; - -#endif /* TSIP_ENABLE */ diff --git a/ubx.c b/ubx.c deleted file mode 100644 index 4d481746..00000000 --- a/ubx.c +++ /dev/null @@ -1,688 +0,0 @@ -/* $Id$ - * - * UBX driver - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "gpsd_config.h" -#include "gpsd.h" -#if defined(UBX_ENABLE) && defined(BINARY_ENABLE) -#include "ubx.h" - -#include "bits.h" - -/* - * A ubx packet looks like this: - * leader: 0xb5 0x62 - * message class: 1 byte - * message type: 1 byte - * length of payload: 2 bytes - * payload: variable length - * checksum: 2 bytes - * - * see also the FV25 and UBX documents on reference.html - */ - -static bool have_port_configuration = false; -static unsigned char original_port_settings[20]; -static unsigned char sbas_in_use; - - bool ubx_write(struct gps_device_t *session, unsigned int msg_class, unsigned int msg_id, unsigned char *msg, unsigned short data_len); - gps_mask_t ubx_parse(struct gps_device_t *session, unsigned char *buf, size_t len); - void ubx_catch_model(struct gps_device_t *session, unsigned char *buf, size_t len); -static gps_mask_t ubx_msg_nav_sol(struct gps_device_t *session, unsigned char *buf, size_t data_len); -static gps_mask_t ubx_msg_nav_dop(struct gps_device_t *session, unsigned char *buf, size_t data_len); -static gps_mask_t ubx_msg_nav_timegps(struct gps_device_t *session, unsigned char *buf, size_t data_len); -static gps_mask_t ubx_msg_nav_svinfo(struct gps_device_t *session, unsigned char *buf, size_t data_len); -static void ubx_msg_sbas(unsigned char *buf); -static void ubx_msg_inf(unsigned char *buf, size_t data_len); - -/** - * Navigation solution message - */ -static gps_mask_t -ubx_msg_nav_sol(struct gps_device_t *session, unsigned char *buf, size_t data_len) -{ - unsigned short gw; - unsigned int tow, flags; - double epx, epy, epz, evx, evy, evz; - unsigned char navmode; - gps_mask_t mask; - double t; - - if (data_len != 52) - return 0; - - flags = (unsigned int)getub(buf, 11); - mask = ONLINE_SET; - if ((flags & (UBX_SOL_VALID_WEEK |UBX_SOL_VALID_TIME)) != 0){ - tow = getleul(buf, 0); - gw = (unsigned short)getlesw(buf, 8); - session->driver.ubx.gps_week = gw; - - t = gpstime_to_unix((int)session->driver.ubx.gps_week, tow/1000.0) - session->context->leap_seconds; - session->gpsdata.sentence_time = t; - session->gpsdata.fix.time = t; - mask |= TIME_SET; -#ifdef NTPSHM_ENABLE - /* TODO overhead */ - if (session->context->enable_ntpshm) - (void)ntpshm_put(session, session->gpsdata.sentence_time); -#endif - } - - epx = (double)(getlesl(buf, 12)/100.0); - epy = (double)(getlesl(buf, 16)/100.0); - epz = (double)(getlesl(buf, 20)/100.0); - evx = (double)(getlesl(buf, 28)/100.0); - evy = (double)(getlesl(buf, 32)/100.0); - evz = (double)(getlesl(buf, 36)/100.0); - ecef_to_wgs84fix(&session->gpsdata, epx, epy, epz, evx, evy, evz); - mask |= LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET | CLIMB_SET; - session->gpsdata.fix.eph = (double)(getlesl(buf, 24)/100.0); - session->gpsdata.fix.eps = (double)(getlesl(buf, 40)/100.0); - session->gpsdata.pdop = (double)(getleuw(buf, 44)/100.0); - session->gpsdata.satellites_used = (int)getub(buf, 47); - mask |= PDOP_SET ; - - navmode = getub(buf, 10); - switch (navmode){ - case UBX_MODE_TMONLY: - case UBX_MODE_3D: - session->gpsdata.fix.mode = MODE_3D; - break; - case UBX_MODE_2D: - case UBX_MODE_DR: /* consider this too as 2D */ - case UBX_MODE_GPSDR: /* XXX DR-aided GPS may be valid 3D */ - session->gpsdata.fix.mode = MODE_2D; - break; - default: - session->gpsdata.fix.mode = MODE_NO_FIX; - } - - if ((flags & UBX_SOL_FLAG_DGPS) != 0) - session->gpsdata.status = STATUS_DGPS_FIX; - else if (session->gpsdata.fix.mode != MODE_NO_FIX) - session->gpsdata.status = STATUS_FIX; - - mask |= MODE_SET | STATUS_SET | CYCLE_START_SET | USED_SET ; - - return mask; -} - -/** - * Dilution of precision message - */ -static gps_mask_t -ubx_msg_nav_dop(struct gps_device_t *session, unsigned char *buf, size_t data_len) -{ - if (data_len != 18) - return 0; - - session->gpsdata.gdop = (double)(getleuw(buf, 4)/100.0); - session->gpsdata.pdop = (double)(getleuw(buf, 6)/100.0); - session->gpsdata.tdop = (double)(getleuw(buf, 8)/100.0); - session->gpsdata.vdop = (double)(getleuw(buf, 10)/100.0); - session->gpsdata.hdop = (double)(getleuw(buf, 12)/100.0); - - return DOP_SET; -} - -/** - * GPS Leap Seconds - */ -static gps_mask_t -ubx_msg_nav_timegps(struct gps_device_t *session, unsigned char *buf, size_t data_len) -{ - unsigned int gw, tow, flags; - double t; - - if (data_len != 16) - return 0; - - tow = getleul(buf, 0); - gw = (uint)getlesw(buf, 8); - if (gw > session->driver.ubx.gps_week) - session->driver.ubx.gps_week = gw; - - flags = (unsigned int)getub(buf, 11); - if ((flags & 0x7) != 0) - session->context->leap_seconds = (int)getub(buf, 10); - - t = gpstime_to_unix((int)session->driver.ubx.gps_week, tow/1000.0) - session->context->leap_seconds; - session->gpsdata.sentence_time = session->gpsdata.fix.time = t; - - return TIME_SET | ONLINE_SET; -} - -/** - * GPS Satellite Info - */ -static gps_mask_t -ubx_msg_nav_svinfo(struct gps_device_t *session, unsigned char *buf, size_t data_len) -{ - unsigned int i, j, tow, nchan, nsv, st; - - if (data_len < 152 ) { - gpsd_report(LOG_PROG, "runt svinfo (datalen=%zd)\n", data_len); - return 0; - } - tow = getleul(buf, 0); -// session->gpsdata.sentence_time = gpstime_to_unix(gps_week, tow) -// - session->context->leap_seconds; - /*@ +charint @*/ - nchan = getub(buf, 4); - if (nchan > MAXCHANNELS){ - gpsd_report(LOG_WARN, "Invalid NAV SVINFO message, >%d reported",MAXCHANNELS); - return 0; - } - /*@ -charint @*/ - gpsd_zero_satellites(&session->gpsdata); - nsv = 0; - for (i = j = st = 0; i < nchan; i++) { - unsigned int off = 8 + 12 * i; - if((int)getub(buf, off+4) == 0) continue; /* LEA-5H seems to have a bug reporting sats it does not see or hear*/ - session->gpsdata.PRN[j] = (int)getub(buf, off+1); - session->gpsdata.ss[j] = (int)getub(buf, off+4); - session->gpsdata.elevation[j] = (int)getsb(buf, off+5); - session->gpsdata.azimuth[j] = (int)getlesw(buf, off+6); - if(session->gpsdata.PRN[j]) - st++; - /*@ -predboolothers */ - if (getub(buf, off+2) & 0x01) - session->gpsdata.used[nsv++] = session->gpsdata.PRN[j]; - if (session->gpsdata.PRN[j] == (int)sbas_in_use) - session->gpsdata.used[nsv++] = session->gpsdata.PRN[j]; - /*@ +predboolothers */ - j++; - } - session->gpsdata.satellites = (int)st; - session->gpsdata.satellites_used = (int)nsv; - return SATELLITE_SET | USED_SET; -} - -/* - * SBAS Info - */ -static void -ubx_msg_sbas(unsigned char *buf) -{ -#ifdef UBX_SBAS_DEBUG - unsigned int i, nsv; - - gpsd_report(LOG_WARN, "SBAS: %d %d %d %d %d\n", - (int)getub(buf, 4), (int)getub(buf, 5), (int)getub(buf, 6), (int)getub(buf, 7), (int)getub(buf, 8)); - - nsv = (int)getub(buf, 8); - for (i = 0; i < nsv; i++) { - int off = 12 + 12 * i; - gpsd_report(LOG_WARN, "SBAS info on SV: %d\n", (int)getub(buf, off)); - } -#endif -/* really 'in_use' depends on the sats info, EGNOS is still in test */ -/* In WAAS areas one might also check for the type of corrections indicated */ - sbas_in_use = getub(buf, 4); -} - -static void -ubx_msg_inf(unsigned char *buf, size_t data_len) -{ - unsigned short msgid; - static char txtbuf[MAX_PACKET_LENGTH]; - - msgid = (unsigned short)((buf[2] << 8) | buf[3]); - if (data_len > MAX_PACKET_LENGTH-1) - data_len = MAX_PACKET_LENGTH-1; - - (void)strlcpy(txtbuf, (char *)buf+6, MAX_PACKET_LENGTH); txtbuf[data_len] = '\0'; - switch (msgid) { - case UBX_INF_DEBUG: - gpsd_report(LOG_PROG, "UBX_INF_DEBUG: %s\n", txtbuf); - break; - case UBX_INF_TEST: - gpsd_report(LOG_PROG, "UBX_INF_TEST: %s\n", txtbuf); - break; - case UBX_INF_NOTICE: - gpsd_report(LOG_INF, "UBX_INF_NOTICE: %s\n", txtbuf); - break; - case UBX_INF_WARNING: - gpsd_report(LOG_WARN, "UBX_INF_WARNING: %s\n", txtbuf); - break; - case UBX_INF_ERROR: - gpsd_report(LOG_WARN, "UBX_INF_ERROR: %s\n", txtbuf); - break; - default: - break; - } - return ; -} - -/*@ +charint @*/ -gps_mask_t ubx_parse(struct gps_device_t *session, unsigned char *buf, size_t len) -{ - size_t data_len; - unsigned short msgid; - gps_mask_t mask = 0; - int i; - - if (len < 6) /* the packet at least contains a head of six bytes */ - return 0; - - /* extract message id and length */ - msgid = (buf[2] << 8) | buf[3]; - data_len = (size_t)getlesw(buf, 4); - switch (msgid) - { - case UBX_NAV_POSECEF: - gpsd_report(LOG_IO, "UBX_NAV_POSECEF\n"); - break; - case UBX_NAV_POSLLH: - gpsd_report(LOG_IO, "UBX_NAV_POSLLH\n"); - break; - case UBX_NAV_STATUS: - gpsd_report(LOG_IO, "UBX_NAV_STATUS\n"); - break; - case UBX_NAV_DOP: - gpsd_report(LOG_PROG, "UBX_NAV_DOP\n"); - mask = ubx_msg_nav_dop(session, &buf[6], data_len); - break; - case UBX_NAV_SOL: - gpsd_report(LOG_PROG, "UBX_NAV_SOL\n"); - mask = ubx_msg_nav_sol(session, &buf[6], data_len); - break; - case UBX_NAV_POSUTM: - gpsd_report(LOG_IO, "UBX_NAV_POSUTM\n"); - break; - case UBX_NAV_VELECEF: - gpsd_report(LOG_IO, "UBX_NAV_VELECEF\n"); - break; - case UBX_NAV_VELNED: - gpsd_report(LOG_IO, "UBX_NAV_VELNED\n"); - break; - case UBX_NAV_TIMEGPS: - gpsd_report(LOG_PROG, "UBX_NAV_TIMEGPS\n"); - mask = ubx_msg_nav_timegps(session, &buf[6], data_len); - break; - case UBX_NAV_TIMEUTC: - gpsd_report(LOG_IO, "UBX_NAV_TIMEUTC\n"); - break; - case UBX_NAV_CLOCK: - gpsd_report(LOG_IO, "UBX_NAV_CLOCK\n"); - break; - case UBX_NAV_SVINFO: - gpsd_report(LOG_PROG, "UBX_NAV_SVINFO\n"); - mask = ubx_msg_nav_svinfo(session, &buf[6], data_len); - break; - case UBX_NAV_DGPS: - gpsd_report(LOG_IO, "UBX_NAV_DGPS\n"); - break; - case UBX_NAV_SBAS: - gpsd_report(LOG_IO, "UBX_NAV_SBAS\n"); - ubx_msg_sbas(&buf[6]); - break; - case UBX_NAV_EKFSTATUS: - gpsd_report(LOG_IO, "UBX_NAV_EKFSTATUS\n"); - break; - - case UBX_RXM_RAW: - gpsd_report(LOG_IO, "UBX_RXM_RAW\n"); - break; - case UBX_RXM_SFRB: - gpsd_report(LOG_IO, "UBX_RXM_SFRB\n"); - break; - case UBX_RXM_SVSI: - gpsd_report(LOG_PROG, "UBX_RXM_SVSI\n"); - break; - case UBX_RXM_ALM: - gpsd_report(LOG_IO, "UBX_RXM_ALM\n"); - break; - case UBX_RXM_EPH: - gpsd_report(LOG_IO, "UBX_RXM_EPH\n"); - break; - case UBX_RXM_POSREQ: - gpsd_report(LOG_IO, "UBX_RXM_POSREQ\n"); - break; - - case UBX_MON_SCHED: - gpsd_report(LOG_IO, "UBX_MON_SCHED\n"); - break; - case UBX_MON_IO: - gpsd_report(LOG_IO, "UBX_MON_IO\n"); - break; - case UBX_MON_IPC: - gpsd_report(LOG_IO, "UBX_MON_IPC\n"); - break; - case UBX_MON_VER: - gpsd_report(LOG_IO, "UBX_MON_VER\n"); - break; - case UBX_MON_EXCEPT: - gpsd_report(LOG_IO, "UBX_MON_EXCEPT\n"); - break; - case UBX_MON_MSGPP: - gpsd_report(LOG_IO, "UBX_MON_MSGPP\n"); - break; - case UBX_MON_RXBUF: - gpsd_report(LOG_IO, "UBX_MON_RXBUF\n"); - break; - case UBX_MON_TXBUF: - gpsd_report(LOG_IO, "UBX_MON_TXBUF\n"); - break; - case UBX_MON_HW: - gpsd_report(LOG_IO, "UBX_MON_HW\n"); - break; - case UBX_MON_USB: - gpsd_report(LOG_IO, "UBX_MON_USB\n"); - break; - - case UBX_INF_DEBUG: - /* FALLTHROUGH */ - case UBX_INF_TEST: - /* FALLTHROUGH */ - case UBX_INF_NOTICE: - /* FALLTHROUGH */ - case UBX_INF_WARNING: - /* FALLTHROUGH */ - case UBX_INF_ERROR: - ubx_msg_inf(buf, data_len); - break; - - case UBX_TIM_TP: - gpsd_report(LOG_IO, "UBX_TIM_TP\n"); - break; - case UBX_TIM_TM: - gpsd_report(LOG_IO, "UBX_TIM_TM\n"); - break; - case UBX_TIM_TM2: - gpsd_report(LOG_IO, "UBX_TIM_TM2\n"); - break; - case UBX_TIM_SVIN: - gpsd_report(LOG_IO, "UBX_TIM_SVIN\n"); - break; - - case UBX_CFG_PRT: - gpsd_report(LOG_IO, "UBX_CFG_PRT\n"); - for(i=6;i<26;i++) - original_port_settings[i-6] = buf[i]; /* copy the original port settings */ - buf[14+6] &= ~0x02; /* turn off NMEA output on this port */ - (void)ubx_write(session, 0x06, 0x00, &buf[6], 20); /* send back with all other settings intact */ - have_port_configuration = true; - break; - - case UBX_ACK_NAK: - gpsd_report(LOG_IO, "UBX_ACK_NAK, class: %02x, id: %02x\n", buf[6], buf[7]); - break; - case UBX_ACK_ACK: - gpsd_report(LOG_IO, "UBX_ACK_ACK, class: %02x, id: %02x\n", buf[6], buf[7]); - break; - - default: - gpsd_report(LOG_WARN, - "UBX: unknown packet id 0x%04hx (length %zd) %s\n", - msgid, len, gpsd_hexdump_wrapper(buf, len, LOG_WARN)); - } - - if (mask) - (void)snprintf(session->gpsdata.tag, sizeof(session->gpsdata.tag), - "0x%04hx", msgid); - - return mask | ONLINE_SET; -} -/*@ -charint @*/ - -static gps_mask_t parse_input(struct gps_device_t *session) -{ - gps_mask_t st; - - if (session->packet.type == UBX_PACKET){ - st = ubx_parse(session, session->packet.outbuffer, session->packet.outbuflen); - session->gpsdata.driver_mode = MODE_BINARY; - return st; -#ifdef NMEA_ENABLE - } else if (session->packet.type == NMEA_PACKET) { - st = nmea_parse((char *)session->packet.outbuffer, session); - session->gpsdata.driver_mode = MODE_NMEA; - return st; -#endif /* NMEA_ENABLE */ - } else - return 0; -} - -void ubx_catch_model(struct gps_device_t *session, unsigned char *buf, size_t len) -{ - /*@ +charint */ - unsigned char *ip = &buf[19]; - unsigned char *op = (unsigned char *)session->subtype; - size_t end = ((len - 19) < 63)?(len - 19):63; - size_t i; - - for(i=0;imsgbuf[0] = 0xb5; - session->msgbuf[1] = 0x62; - - CK_A = CK_B = 0; - session->msgbuf[2] = msg_class; - session->msgbuf[3] = msg_id; - session->msgbuf[4] = data_len & 0xff; - session->msgbuf[5] = (data_len >> 8) & 0xff; - - assert(msg != NULL || data_len == 0); - if (msg != NULL) - (void)memcpy(&session->msgbuf[6], msg, data_len); - - /* calculate CRC */ - for (i = 2; i < 6; i++) { - CK_A += session->msgbuf[i]; - CK_B += CK_A; - } - /*@ -nullderef @*/ - for (i = 0; i < data_len; i++) { - CK_A += msg[i]; - CK_B += CK_A; - } - - session->msgbuf[6 + data_len] = CK_A; - session->msgbuf[7 + data_len] = CK_B; - session->msgbuflen = data_len + 7; - /*@ +type @*/ - - gpsd_report(LOG_IO, - "=> GPS: UBX class: %02x, id: %02x, len: %d, data:%s, crc: %02x%02x\n", - msg_class, msg_id, data_len, - gpsd_hexdump_wrapper(msg, (size_t)data_len, LOG_IO), - CK_A, CK_B); - - count = write(session->gpsdata.gps_fd, - session->msgbuf, session->msgbuflen); - (void)tcdrain(session->gpsdata.gps_fd); - ok = (count == (ssize_t)session->msgbuflen); - /*@ +nullderef @*/ - return(ok); -} - -static ssize_t ubx_control_send(struct gps_device_t *session, char *msg, size_t data_len) -/* not used by gpsd, it's for gpsctl and friends */ -{ - return ubx_write(session, (unsigned int)msg[0], (unsigned int)msg[1], - (unsigned char *)msg+2, (unsigned short)(data_len-2)) ? ((ssize_t)(data_len+7)) : -1; -} - -#ifdef ALLOW_RECONFIGURE -static void ubx_configure(struct gps_device_t *session, unsigned int seq) -{ - unsigned char msg[32]; - - gpsd_report(LOG_IO, "UBX configure: %d\n",seq); - - (void)ubx_write(session, 0x06u, 0x00, NULL, 0); /* get this port's settings */ - - /*@ -type @*/ - msg[0] = 0x03; /* SBAS mode enabled, accept testbed mode */ - msg[1] = 0x07; /* SBAS usage: range, differential corrections and integrity */ - msg[2] = 0x03; /* use the maximun search range: 3 channels */ - msg[3] = 0x00; /* PRN numbers to search for all set to 0 => auto scan */ - msg[4] = 0x00; - msg[5] = 0x00; - msg[6] = 0x00; - msg[7] = 0x00; - (void)ubx_write(session, 0x06u, 0x16, msg, 8); - - msg[0] = 0x01; /* class */ - msg[1] = 0x04; /* msg id = UBX_NAV_DOP */ - msg[2] = 0x01; /* rate */ - (void)ubx_write(session, 0x06u, 0x01, msg, 3); - msg[0] = 0x01; /* class */ - msg[1] = 0x06; /* msg id = NAV-SOL */ - msg[2] = 0x01; /* rate */ - (void)ubx_write(session, 0x06u, 0x01, msg, 3); - msg[0] = 0x01; /* class */ - msg[1] = 0x20; /* msg id = UBX_NAV_TIMEGPS */ - msg[2] = 0x01; /* rate */ - (void)ubx_write(session, 0x06u, 0x01, msg, 3); - msg[0] = 0x01; /* class */ - msg[1] = 0x30; /* msg id = NAV-SVINFO */ - msg[2] = 0x0a; /* rate */ - (void)ubx_write(session, 0x06u, 0x01, msg, 3); - msg[0] = 0x01; /* class */ - msg[1] = 0x32; /* msg id = NAV-SBAS */ - msg[2] = 0x0a; /* rate */ - (void)ubx_write(session, 0x06u, 0x01, msg, 3); - /*@ +type @*/ - -} - -static void ubx_revert(struct gps_device_t *session) -{ - /*@ -type @*/ - unsigned char msg[4] = { - 0x00, 0x00, /* hotstart */ - 0x01, /* controlled software reset */ - 0x00}; /* reserved */ - /*@ +type @*/ - - gpsd_report(LOG_IO, "UBX revert\n"); - - /* Reverting all in one fast and reliable reset */ - (void)ubx_write(session, 0x06, 0x04, msg, 4); /* CFG-RST */ -} -#endif /* ALLOW_RECONFIGURE */ - -static void ubx_nmea_mode(struct gps_device_t *session, int mode) -{ - int i; - unsigned char buf[20]; - - if(!have_port_configuration) - return; - - /*@ +charint -usedef @*/ - for(i=0;i<22;i++) - buf[i] = original_port_settings[i]; /* copy the original port settings */ - if(buf[0] == 0x01) /* set baudrate on serial port only */ - putlelong(buf, 8, session->gpsdata.baudrate); - - if (mode == 0) { - buf[14] &= ~0x01; /* turn off UBX output on this port */ - buf[14] |= 0x02; /* turn on NMEA output on this port */ - } else { - buf[14] &= ~0x02; /* turn off NMEA output on this port */ - buf[14] |= 0x01; /* turn on UBX output on this port */ - } - /*@ -charint +usedef @*/ - (void)ubx_write(session, 0x06u, 0x00, &buf[6], 20); /* send back with all other settings intact */ -} - -static bool ubx_speed(struct gps_device_t *session, - speed_t speed, char parity, int stopbits) -{ - int i; - unsigned char buf[20]; - unsigned long usart_mode; - - /*@ +charint -usedef -compdef */ - if((!have_port_configuration) || (buf[0] != 0x01)) /* set baudrate on serial port only */ - return false; - - for(i=0;i<22;i++) - buf[i] = original_port_settings[i]; /* copy the original port settings */ - usart_mode = getleul(buf, 4); - usart_mode &=~ 0xE00; /* zero bits 11:9 */ - switch (parity) { - case (int)'E': - case 2: - usart_mode |= 0x00; - break; - case (int)'O': - case 1: - usart_mode |= 0x01; - break; - case (int)'N': - case 0: - default: - usart_mode |= 0x4; /* 0x5 would work too */ - break; - } - usart_mode &=~ 0x03000; /* zero bits 13:12 */ - if (stopbits == 2) - usart_mode |= 0x2000; /* zero value means 1 stop bit */ - putlelong(buf, 4, usart_mode); - putlelong(buf, 8, speed); - (void)ubx_write(session, 0x06, 0x00, &buf[6], 20); /* send back with all other settings intact */ - /*@ -charint +usedef +compdef */ - return true; -} - -/* This is everything we export */ -const struct gps_type_t ubx_binary = { - .type_name = "uBlox UBX binary", /* Full name of type */ - .packet_type = UBX_PACKET, /* associated lexer packet type */ - .trigger = NULL, /* Response string that identifies device (not active) */ - .channels = 50, /* Number of satellite channels supported by the device */ - .control_send = ubx_control_send, /* no control sender yet */ - .probe_detect = NULL, /* Startup-time device detector */ - .probe_wakeup = NULL, /* Wakeup to be done before each baud hunt */ - .probe_subtype = NULL, /* Initialize the device and get subtype */ -#ifdef ALLOW_RECONFIGURE - .configurator = ubx_configure, /* Enable what reports we need */ -#endif /* ALLOW_RECONFIGURE */ - .get_packet = generic_get, /* Packet getter (using default routine) */ - .parse_packet = parse_input, /* Parse message packets */ - .rtcm_writer = NULL, /* RTCM handler (using default routine) */ - .speed_switcher = ubx_speed, /* Speed (baudrate) switch */ - .mode_switcher = ubx_nmea_mode, /* Switch to NMEA mode */ - .rate_switcher = NULL, /* Message delivery rate switcher */ - .cycle_chars = -1, /* Number of chars per report cycle */ -#ifdef ALLOW_RECONFIGURE - .revert = ubx_revert, /* Undo the actions of .configurator */ -#endif /* ALLOW_RECONFIGURE */ - .wrapup = NULL, /* Puts device back to original settings */ - .cycle = 1 /* Number of updates per second */ -}; -#endif /* defined(UBX_ENABLE) && defined(BINARY_ENABLE) */ diff --git a/ubx.h b/ubx.h deleted file mode 100644 index e5102967..00000000 --- a/ubx.h +++ /dev/null @@ -1,101 +0,0 @@ -/* $Id$ */ -#ifndef _GPSD_UBX_H_ -#define _GPSD_UBX_H_ - -#define UBX_MESSAGE_BASE_SIZE 6 -#define UBX_MESSAGE_DATA_OFFSET UBX_MESSAGE_BASE_SIZE - -typedef enum { - UBX_CLASS_NAV = 0x01, /**< Navigation */ - UBX_CLASS_RXM = 0x02, /**< Receiver Manager */ - UBX_CLASS_INF = 0x04, /**< Informative text messages */ - UBX_CLASS_ACK = 0x05, /**< (Not) Acknowledges for cfg messages */ - UBX_CLASS_CFG = 0x06, /**< Configuration requests */ - UBX_CLASS_UPD = 0x09, /**< Firmware updates */ - UBX_CLASS_MON = 0x0a, /**< System monitoring */ - UBX_CLASS_AID = 0x0b, /**< AGPS */ - UBX_CLASS_TIM = 0x0d, /**< Time */ -} ubx_classes_t; - -#define UBX_MSGID(cls_, id_) (((cls_)<<8)|(id_)) - -typedef enum { - UBX_NAV_POSECEF = UBX_MSGID(UBX_CLASS_NAV, 0x01), - UBX_NAV_POSLLH = UBX_MSGID(UBX_CLASS_NAV, 0x02), - UBX_NAV_STATUS = UBX_MSGID(UBX_CLASS_NAV, 0x03), - UBX_NAV_DOP = UBX_MSGID(UBX_CLASS_NAV, 0x04), - UBX_NAV_SOL = UBX_MSGID(UBX_CLASS_NAV, 0x06), - UBX_NAV_POSUTM = UBX_MSGID(UBX_CLASS_NAV, 0x08), - UBX_NAV_VELECEF = UBX_MSGID(UBX_CLASS_NAV, 0x11), - UBX_NAV_VELNED = UBX_MSGID(UBX_CLASS_NAV, 0x12), - UBX_NAV_TIMEGPS = UBX_MSGID(UBX_CLASS_NAV, 0x20), - UBX_NAV_TIMEUTC = UBX_MSGID(UBX_CLASS_NAV, 0x21), - UBX_NAV_CLOCK = UBX_MSGID(UBX_CLASS_NAV, 0x22), - UBX_NAV_SVINFO = UBX_MSGID(UBX_CLASS_NAV, 0x30), - UBX_NAV_DGPS = UBX_MSGID(UBX_CLASS_NAV, 0x31), - UBX_NAV_SBAS = UBX_MSGID(UBX_CLASS_NAV, 0x32), - UBX_NAV_EKFSTATUS = UBX_MSGID(UBX_CLASS_NAV, 0x40), - - UBX_RXM_RAW = UBX_MSGID(UBX_CLASS_RXM, 0x10), - UBX_RXM_SFRB = UBX_MSGID(UBX_CLASS_RXM, 0x11), - UBX_RXM_SVSI = UBX_MSGID(UBX_CLASS_RXM, 0x20), - UBX_RXM_ALM = UBX_MSGID(UBX_CLASS_RXM, 0x30), - UBX_RXM_EPH = UBX_MSGID(UBX_CLASS_RXM, 0x31), - UBX_RXM_POSREQ = UBX_MSGID(UBX_CLASS_RXM, 0x40), - - UBX_INF_ERROR = UBX_MSGID(UBX_CLASS_INF, 0X00), - UBX_INF_WARNING = UBX_MSGID(UBX_CLASS_INF, 0X01), - UBX_INF_NOTICE = UBX_MSGID(UBX_CLASS_INF, 0x02), - UBX_INF_TEST = UBX_MSGID(UBX_CLASS_INF, 0x03), - UBX_INF_DEBUG = UBX_MSGID(UBX_CLASS_INF, 0x04), - UBX_INF_USER = UBX_MSGID(UBX_CLASS_INF, 0x07), - - UBX_ACK_NAK = UBX_MSGID(UBX_CLASS_ACK, 0x00), - UBX_ACK_ACK = UBX_MSGID(UBX_CLASS_ACK, 0x01), - - UBX_CFG_PRT = UBX_MSGID(UBX_CLASS_CFG, 0x00), - - UBX_UPD_DOWNL = UBX_MSGID(UBX_CLASS_UPD, 0x01), - UBX_UPD_UPLOAD = UBX_MSGID(UBX_CLASS_UPD, 0x02), - UBX_UPD_EXEC = UBX_MSGID(UBX_CLASS_UPD, 0x03), - UBX_UPD_MEMCPY = UBX_MSGID(UBX_CLASS_UPD, 0x04), - - UBX_MON_SCHED = UBX_MSGID(UBX_CLASS_MON, 0x01), - UBX_MON_IO = UBX_MSGID(UBX_CLASS_MON, 0x02), - UBX_MON_IPC = UBX_MSGID(UBX_CLASS_MON, 0x03), - UBX_MON_VER = UBX_MSGID(UBX_CLASS_MON, 0x04), - UBX_MON_EXCEPT = UBX_MSGID(UBX_CLASS_MON, 0x05), - UBX_MON_MSGPP = UBX_MSGID(UBX_CLASS_MON, 0x06), - UBX_MON_RXBUF = UBX_MSGID(UBX_CLASS_MON, 0x07), - UBX_MON_TXBUF = UBX_MSGID(UBX_CLASS_MON, 0x08), - UBX_MON_HW = UBX_MSGID(UBX_CLASS_MON, 0x09), - UBX_MON_USB = UBX_MSGID(UBX_CLASS_MON, 0x0a), - - UBX_AID_REQ = UBX_MSGID(UBX_CLASS_AID, 0x00), - UBX_AID_INI = UBX_MSGID(UBX_CLASS_AID, 0x01), - UBX_AID_HUI = UBX_MSGID(UBX_CLASS_AID, 0x02), - UBX_AID_DATA = UBX_MSGID(UBX_CLASS_AID, 0x10), - UBX_AID_ALM = UBX_MSGID(UBX_CLASS_AID, 0x30), - UBX_AID_EPH = UBX_MSGID(UBX_CLASS_AID, 0x31), - - UBX_TIM_TP = UBX_MSGID(UBX_CLASS_TIM, 0x01), - UBX_TIM_TM = UBX_MSGID(UBX_CLASS_TIM, 0x02), - UBX_TIM_TM2 = UBX_MSGID(UBX_CLASS_TIM, 0x03), - UBX_TIM_SVIN = UBX_MSGID(UBX_CLASS_TIM, 0x04), -} ubx_message_t; - -typedef enum { - UBX_MODE_NOFIX = 0x00, /* no fix available */ - UBX_MODE_DR = 0x01, /* Dead reckoning */ - UBX_MODE_2D = 0x02, /* 2D fix */ - UBX_MODE_3D = 0x03, /* 3D fix */ - UBX_MODE_GPSDR = 0x04, /* GPS + dead reckoning */ - UBX_MODE_TMONLY = 0x05, /* Time-only fix */ -} ubx_mode_t; - -#define UBX_SOL_FLAG_GPS_FIX_OK 0x01 -#define UBX_SOL_FLAG_DGPS 0x02 -#define UBX_SOL_VALID_WEEK 0x04 -#define UBX_SOL_VALID_TIME 0x08 - -#endif /* _GPSD_UBX_H_ */ diff --git a/zodiac.c b/zodiac.c deleted file mode 100644 index 4afcc211..00000000 --- a/zodiac.c +++ /dev/null @@ -1,496 +0,0 @@ -/* $Id$ */ -/* - * Handle the Rockwell binary packet format supported by the old Zodiac chipset - */ -#include -#include -#include -#include -#include -#include -#include "gpsd_config.h" -#include "gpsd.h" - -#include "bits.h" - -#ifdef ZODIAC_ENABLE -struct header { - unsigned short sync; - unsigned short id; - unsigned short ndata; - unsigned short flags; - unsigned short csum; -}; - -static unsigned short zodiac_checksum(unsigned short *w, int n) -{ - unsigned short csum = 0; - - while (n-- > 0) - csum += *(w++); - return -csum; -} - -/* zodiac_spew - Takes a message type, an array of data words, and a length - for the array, and prepends a 5 word header (including checksum). - The data words are expected to be checksummed */ -#if defined (WORDS_BIGENDIAN) -/* data is assumed to contain len/2 unsigned short words - * we change the endianness to little, when needed. - */ -static int end_write(int fd, void *d, int len) -{ - char buf[BUFSIZ]; - char *p = buf; - char *data = (char *)d; - size_t n = (size_t)len; - - while (n>0) { - *p++ = *(data+1); *p++ = *data; - data += 2; n -= 2; - } - return write(fd, buf, len); -} -#else -#define end_write write -#endif - -static ssize_t zodiac_spew(struct gps_device_t *session, unsigned short type, unsigned short *dat, int dlen) -{ - struct header h; - int i; - char buf[BUFSIZ]; - - h.sync = 0x81ff; - h.id = (unsigned short)type; - h.ndata = (unsigned short)(dlen - 1); - h.flags = 0; - h.csum = zodiac_checksum((unsigned short *) &h, 4); - -#ifdef ALLOW_RECONFIGURE - if (session->gpsdata.gps_fd != -1) { - size_t hlen, datlen; - hlen = sizeof(h); - datlen = sizeof(unsigned short) * dlen; - if (end_write(session->gpsdata.gps_fd, &h, hlen) != (ssize_t)hlen || - end_write(session->gpsdata.gps_fd, dat, datlen) != (ssize_t)datlen) { - gpsd_report(LOG_RAW, "Reconfigure write failed\n"); - return -1; - } - } -#endif /* ALLOW_RECONFIGURE */ - - (void)snprintf(buf, sizeof(buf), - "%04x %04x %04x %04x %04x", - h.sync,h.id,h.ndata,h.flags,h.csum); - for (i = 0; i < dlen; i++) - (void)snprintf(buf+strlen(buf), sizeof(buf)-strlen(buf), - " %04x", dat[i]); - - gpsd_report(LOG_RAW, "Sent Zodiac packet: %s\n",buf); - - return 0; -} - -static bool zodiac_speed_switch(struct gps_device_t *session, - speed_t speed, char parity, int stopbits) -{ - unsigned short data[15]; - - if (session->driver.zodiac.sn++ > 32767) - session->driver.zodiac.sn = 0; - - switch (parity) { - case 'E': - case 2: - parity = (char)2; - break; - case 'O': - case 1: - parity = (char)1; - break; - case 'N': - case 0: - default: - parity = (char)0; - break; - } - - memset(data, 0, sizeof(data)); - /* data is the part of the message starting at word 6 */ - data[0] = session->driver.zodiac.sn; /* sequence number */ - data[1] = 1; /* port 1 data valid */ - data[2] = (unsigned short)parity; /* port 1 character width (8 bits) */ - data[3] = (unsigned short)(stopbits-1); /* port 1 stop bits (1 stopbit) */ - data[4] = 0; /* port 1 parity (none) */ - data[5] = (unsigned short)(round(log((double)speed/300)/M_LN2)+1); /* port 1 speed */ - data[14] = zodiac_checksum(data, 14); - - (void)zodiac_spew(session, 1330, data, 15); -#ifdef ALLOW_RECONFIGURE - return true; /* it would be nice to error-check this */ -#else - return false; -#endif /* ALLOW_RECONFIGURE */ -} - -static ssize_t zodiac_control_send(struct gps_device_t *session, - char *msg, size_t len) -{ - unsigned short *shortwords = (unsigned short *)msg; - - /* and if len isn't even, it's your own fault */ - return zodiac_spew(session, shortwords[0], shortwords+1, (int)(len/2-1)); -} - -static void send_rtcm(struct gps_device_t *session, - char *rtcmbuf, size_t rtcmbytes) -{ - unsigned short data[34]; - int n = 1 + (int)(rtcmbytes/2 + rtcmbytes%2); - - if (session->driver.zodiac.sn++ > 32767) - session->driver.zodiac.sn = 0; - - memset(data, 0, sizeof(data)); - data[0] = session->driver.zodiac.sn; /* sequence number */ - memcpy(&data[1], rtcmbuf, rtcmbytes); - data[n] = zodiac_checksum(data, n); - - (void)zodiac_spew(session, 1351, data, n+1); -} - -static ssize_t zodiac_send_rtcm(struct gps_device_t *session, - char *rtcmbuf, size_t rtcmbytes) -{ - size_t len; - - while (rtcmbytes > 0) { - len = (size_t)(rtcmbytes>64?64:rtcmbytes); - send_rtcm(session, rtcmbuf, len); - rtcmbytes -= len; - rtcmbuf += len; - } - return 1; -} - -#define getzword(n) getwordz(session->packet.outbuffer, n) -#define getzlong(n) getlongz(session->packet.outbuffer, n) - -static gps_mask_t handle1000(struct gps_device_t *session) -{ - double subseconds; - struct tm unpacked_date; - /* ticks = getzlong(6); */ - /* sequence = getzword(8); */ - /* measurement_sequence = getzword(9); */ - /*@ -boolops -predboolothers @*/ - session->gpsdata.status = (getzword(10) & 0x1c) ? 0 : 1; - if (session->gpsdata.status != 0) - session->gpsdata.fix.mode = (getzword(10) & 1) ? MODE_2D : MODE_3D; - else - session->gpsdata.fix.mode = MODE_NO_FIX; - /*@ +boolops -predboolothers @*/ - - /* solution_type = getzword(11); */ - session->gpsdata.satellites_used = (int)getzword(12); - /* polar_navigation = getzword(13); */ - /* gps_week = getzword(14); */ - /* gps_seconds = getzlong(15); */ - /* gps_nanoseconds = getzlong(17); */ - unpacked_date.tm_mday = (int)getzword(19); - unpacked_date.tm_mon = (int)getzword(20) - 1; - unpacked_date.tm_year = (int)getzword(21) - 1900; - unpacked_date.tm_hour = (int)getzword(22); - unpacked_date.tm_min = (int)getzword(23); - unpacked_date.tm_sec = (int)getzword(24); - subseconds = (int)getzlong(25) / 1e9; - /*@ -compdef */ - session->gpsdata.fix.time = session->gpsdata.sentence_time = - (double)mkgmtime(&unpacked_date) + subseconds; - /*@ +compdef */ -#ifdef NTPSHM_ENABLE - /* Removing/changing the magic number below is likely to disturb - * the handling of the 1pps signal from the gps device. The regression - * tests and simple gps applications do not detect this. A live test - * with the 1pps signal active is required. */ - if (session->context->enable_ntpshm && session->gpsdata.fix.mode > MODE_NO_FIX) - (void)ntpshm_put(session, session->gpsdata.fix.time + 1.1); -#endif - /*@ -type @*/ - session->gpsdata.fix.latitude = ((long)getzlong(27)) * RAD_2_DEG * 1e-8; - session->gpsdata.fix.longitude = ((long)getzlong(29)) * RAD_2_DEG * 1e-8; - /* - * The Rockwell Jupiter TU30-D140 reports altitude as uncorrected height - * above WGS84 geoid. The Zodiac binary protocol manual does not - * specify whether word 31 is geodetic or WGS 84. - */ - session->gpsdata.fix.altitude = ((long)getzlong(31)) * 1e-2; - /*@ +type @*/ - session->gpsdata.separation = ((short)getzword(33)) * 1e-2; - session->gpsdata.fix.altitude -= session->gpsdata.separation; - session->gpsdata.fix.speed = (int)getzlong(34) * 1e-2; - session->gpsdata.fix.track = (int)getzword(36) * RAD_2_DEG * 1e-3; - session->mag_var = ((short)getzword(37)) * RAD_2_DEG * 1e-4; - session->gpsdata.fix.climb = ((short)getzword(38)) * 1e-2; - /* map_datum = getzword(39); */ - /* manual says these are 1-sigma */ - session->gpsdata.fix.eph = (int)getzlong(40) * 1e-2 * GPSD_CONFIDENCE; - session->gpsdata.fix.epv = (int)getzlong(42) * 1e-2 * GPSD_CONFIDENCE; - session->gpsdata.fix.ept = (int)getzlong(44) * 1e-2 * GPSD_CONFIDENCE; - session->gpsdata.fix.eps = (int)getzword(46) * 1e-2 * GPSD_CONFIDENCE; - /* clock_bias = (int)getzlong(47) * 1e-2; */ - /* clock_bias_sd = (int)getzlong(49) * 1e-2; */ - /* clock_drift = (int)getzlong(51) * 1e-2; */ - /* clock_drift_sd = (int)getzlong(53) * 1e-2; */ - -#if 0 - gpsd_report(LOG_INF, "date: %lf\n", session->gpsdata.fix.time); - gpsd_report(LOG_INF, " solution invalid:\n"); - gpsd_report(LOG_INF, " altitude: %d\n", (getzword(10) & 1) ? 1 : 0); - gpsd_report(LOG_INF, " no diff gps: %d\n", (getzword(10) & 2) ? 1 : 0); - gpsd_report(LOG_INF, " not enough satellites: %d\n", (getzword(10) & 4) ? 1 : 0); - gpsd_report(LOG_INF, " exceed max EHPE: %d\n", (getzword(10) & 8) ? 1 : 0); - gpsd_report(LOG_INF, " exceed max EVPE: %d\n", (getzword(10) & 16) ? 1 : 0); - gpsd_report(LOG_INF, " solution type:\n"); - gpsd_report(LOG_INF, " propagated: %d\n", (getzword(11) & 1) ? 1 : 0); - gpsd_report(LOG_INF, " altitude: %d\n", (getzword(11) & 2) ? 1 : 0); - gpsd_report(LOG_INF, " differential: %d\n", (getzword(11) & 4) ? 1 : 0); - gpsd_report(LOG_INF, "Number of measurements in solution: %d\n", getzword(12)); - gpsd_report(LOG_INF, "Lat: %f\n", getzlong(27) * RAD_2_DEG * 1e-8); - gpsd_report(LOG_INF, "Lon: %f\n", getzlong(29) * RAD_2_DEG * 1e-8); - gpsd_report(LOG_INF, "Alt: %f\n", (double) getzlong(31) * 1e-2); - gpsd_report(LOG_INF, "Speed: %f\n", (double) getzlong(34) * 1e-2 * MPS_TO_KNOTS); - gpsd_report(LOG_INF, "Map datum: %d\n", getzword(39)); - gpsd_report(LOG_INF, "Magnetic variation: %f\n", getzword(37) * RAD_2_DEG * 1e-4); - gpsd_report(LOG_INF, "Course: %f\n", getzword(36) * RAD_2_DEG * 1e-4); - gpsd_report(LOG_INF, "Separation: %f\n", getzword(33) * 1e-2); -#endif - - session->gpsdata.sentence_length = 55; - return TIME_SET|LATLON_SET|ALTITUDE_SET|CLIMB_SET|SPEED_SET|TRACK_SET|STATUS_SET|MODE_SET|CYCLE_START_SET; /* |HERR_SET|VERR_SET|SPEEDERR_SET */ -} - -static gps_mask_t handle1002(struct gps_device_t *session) -{ - int i, j, status, prn; - - session->gpsdata.satellites_used = 0; - memset(session->gpsdata.used,0,sizeof(session->gpsdata.used)); - /* ticks = getzlong(6); */ - /* sequence = getzword(8); */ - /* measurement_sequence = getzword(9); */ - /* gps_week = getzword(10); */ - /* gps_seconds = getzlong(11); */ - /* gps_nanoseconds = getzlong(13); */ - for (i = 0; i < ZODIAC_CHANNELS; i++) { - /*@ -type @*/ - session->driver.zodiac.Zv[i] = status = (int)getzword(15 + (3 * i)); - session->driver.zodiac.Zs[i] = prn = (int)getzword(16 + (3 * i)); - /*@ +type @*/ -#if 0 - gpsd_report(LOG_INF, "Sat%02d:\n", i); - gpsd_report(LOG_INF, " used:%d\n", (status & 1) ? 1 : 0); - gpsd_report(LOG_INF, " eph:%d\n", (status & 2) ? 1 : 0); - gpsd_report(LOG_INF, " val:%d\n", (status & 4) ? 1 : 0); - gpsd_report(LOG_INF, " dgps:%d\n", (status & 8) ? 1 : 0); - gpsd_report(LOG_INF, " PRN:%d\n", prn); - gpsd_report(LOG_INF, " C/No:%d\n", getzword(17 + (3 * i))); -#endif - if (status & 1) - session->gpsdata.used[session->gpsdata.satellites_used++] = prn; - for (j = 0; j < ZODIAC_CHANNELS; j++) { - if (session->gpsdata.PRN[j] != prn) - continue; - session->gpsdata.ss[j] = (int)getzword(17 + (3 * i)); - break; - } - } - return SATELLITE_SET | USED_SET; -} - -static gps_mask_t handle1003(struct gps_device_t *session) -{ - int i; - - /* ticks = getzlong(6); */ - /* sequence = getzword(8); */ - session->gpsdata.gdop = (unsigned int)getzword(9) * 1e-2; - session->gpsdata.pdop = (unsigned int)getzword(10) * 1e-2; - session->gpsdata.hdop = (unsigned int)getzword(11) * 1e-2; - session->gpsdata.vdop = (unsigned int)getzword(12) * 1e-2; - session->gpsdata.tdop = (unsigned int)getzword(13) * 1e-2; - session->gpsdata.satellites = (int)getzword(14); - - for (i = 0; i < ZODIAC_CHANNELS; i++) { - if (i < session->gpsdata.satellites) { - session->gpsdata.PRN[i] = (int)getzword(15 + (3 * i)); - session->gpsdata.azimuth[i] = (int)(((short)getzword(16 + (3 * i))) * RAD_2_DEG * 1e-4); - if (session->gpsdata.azimuth[i] < 0) - session->gpsdata.azimuth[i] += 360; - session->gpsdata.elevation[i] = (int)(((short)getzword(17 + (3 * i))) * RAD_2_DEG * 1e-4); -#if 0 - gpsd_report(LOG_INF, "Sat%02d: PRN:%d az:%d el:%d\n", - i, getzword(15+(3 * i)),getzword(16+(3 * i)),getzword(17+(3 * i))); -#endif - } else { - session->gpsdata.PRN[i] = 0; - session->gpsdata.azimuth[i] = 0; - session->gpsdata.elevation[i] = 0; - } - } - return SATELLITE_SET | HDOP_SET | VDOP_SET | PDOP_SET; -} - -static void handle1005(struct gps_device_t *session UNUSED) -{ - /* ticks = getzlong(6); */ - /* sequence = getzword(8); */ - int numcorrections = (int)getzword(12); -#if 0 - int i; - - gpsd_report(LOG_INF, "Packet: %d\n", session->driver.zodiac.sn); - gpsd_report(LOG_INF, "Station bad: %d\n", (getzword(9) & 1) ? 1 : 0); - gpsd_report(LOG_INF, "User disabled: %d\n", (getzword(9) & 2) ? 1 : 0); - gpsd_report(LOG_INF, "Station ID: %d\n", getzword(10)); - gpsd_report(LOG_INF, "Age of last correction in seconds: %d\n", getzword(11)); - gpsd_report(LOG_INF, "Number of corrections: %d\n", getzword(12)); - for (i = 0; i < numcorrections; i++) { - gpsd_report(LOG_INF, "Sat%02d:\n", getzword(13+i) & 0x3f); - gpsd_report(LOG_INF, "ephemeris:%d\n", (getzword(13+i) & 64) ? 1 : 0); - gpsd_report(LOG_INF, "rtcm corrections:%d\n", (getzword(13+i) & 128) ? 1 : 0); - gpsd_report(LOG_INF, "rtcm udre:%d\n", (getzword(13+i) & 256) ? 1 : 0); - gpsd_report(LOG_INF, "sat health:%d\n", (getzword(13+i) & 512) ? 1 : 0); - gpsd_report(LOG_INF, "rtcm sat health:%d\n", (getzword(13+i) & 1024) ? 1 : 0); - gpsd_report(LOG_INF, "corrections state:%d\n", (getzword(13+i) & 2048) ? 1 : 0); - gpsd_report(LOG_INF, "iode mismatch:%d\n", (getzword(13+i) & 4096) ? 1 : 0); - } -#endif - if (session->gpsdata.fix.mode == MODE_NO_FIX) - session->gpsdata.status = STATUS_NO_FIX; - else if (numcorrections == 0) - session->gpsdata.status = STATUS_FIX; - else - session->gpsdata.status = STATUS_DGPS_FIX; -} - -static gps_mask_t handle1011(struct gps_device_t *session) -{ - /* - * This is UNTESTED -- but harmless if buggy. Added to support - * client querying of the ID with firmware version in 2006. - * The Zodiac is supposed to send one of these messages on startup. - */ - getstringz(session->subtype, - session->packet.outbuffer, - 19, 28); /* software version field */ - gpsd_report(LOG_INF, "Software version: %s\n", session->subtype); - return DEVICEID_SET; -} - - -static void handle1108(struct gps_device_t *session) -{ - /* ticks = getzlong(6); */ - /* sequence = getzword(8); */ - /* utc_week_seconds = getzlong(14); */ - /* leap_nanoseconds = getzlong(17); */ - if ((int)(getzword(19) & 3) == 3) - session->context->leap_seconds = (int)getzword(16); -#if 0 - gpsd_report(LOG_INF, "Leap seconds: %d.%09d\n", getzword(16), getzlong(17)); - gpsd_report(LOG_INF, "UTC validity: %d\n", getzword(19) & 3); -#endif -} - -static gps_mask_t zodiac_analyze(struct gps_device_t *session) -{ - char buf[BUFSIZ]; - int i; - unsigned int id = (unsigned int)((session->packet.outbuffer[3]<<8) | session->packet.outbuffer[2]); - - if (session->packet.type != ZODIAC_PACKET) { - const struct gps_type_t **dp; - gpsd_report(LOG_PROG, "zodiac_analyze packet type %d\n",session->packet.type); - // Wrong packet type ? - // Maybe find a trigger just in case it's an Earthmate - gpsd_report(LOG_RAW+4, "Is this a trigger: %s ?\n", (char*)session->packet.outbuffer); - - for (dp = gpsd_drivers; *dp; dp++) { - char *trigger = (*dp)->trigger; - - if (trigger!=NULL && strncmp((char *)session->packet.outbuffer, trigger, strlen(trigger))==0 && isatty(session->gpsdata.gps_fd)!=0) { - gpsd_report(LOG_PROG, "found %s.\n", trigger); - - (void)gpsd_switch_driver(session, (*dp)->type_name); - return 0; - } - } - return 0; - } - - buf[0] = '\0'; - for (i = 0; i < (int)session->packet.outbuflen; i++) - (void)snprintf(buf+strlen(buf), sizeof(buf)-strlen(buf), - "%02x", (unsigned int)session->packet.outbuffer[i]); - (void)strlcat(buf, "\n", BUFSIZ); - gpsd_report(LOG_RAW, "Raw Zodiac packet type %d length %zd: %s\n", - id, session->packet.outbuflen, buf); - - if (session->packet.outbuflen < 10) - return 0; - - (void)snprintf(session->gpsdata.tag,sizeof(session->gpsdata.tag),"%u",id); - - switch (id) { - case 1000: - return handle1000(session); - case 1002: - return handle1002(session); - case 1003: - return handle1003(session); - case 1005: - handle1005(session); - return 0; - case 1011: - return handle1011(session); - case 1108: - handle1108(session); - return 0; - default: - return 0; - } -} - -/* caller needs to specify a wrapup function */ - -/* this is everything we export */ -const struct gps_type_t zodiac_binary = -{ - .type_name = "Zodiac binary", /* full name of type */ - .packet_type = ZODIAC_PACKET, /* associated lexer packet type */ - .trigger = NULL, /* no trigger */ - .channels = 12, /* consumer-grade GPS */ - .control_send = zodiac_control_send, /* for gpsctl and friends */ - .probe_wakeup = NULL, /* no probe on baud rate change */ - .probe_detect = NULL, /* no probe */ - .probe_subtype = NULL, /* no initialization */ -#ifdef ALLOW_RECONFIGURE - .configurator = NULL, /* no configuration */ -#endif /* ALLOW_RECONFIGURE */ - .get_packet = generic_get, /* use the generic packet getter */ - .parse_packet = zodiac_analyze, /* parse message packets */ - .rtcm_writer = zodiac_send_rtcm, /* send DGPS correction */ - .speed_switcher = zodiac_speed_switch,/* we can change baud rate */ - .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 reversion hook */ -#endif /* ALLOW_RECONFIGURE */ - .wrapup = NULL, /* caller might supply a close hook */ - .cycle = 1, /* updates every second */ -}; - -#endif /* ZODIAC_ENABLE */ -- cgit v1.2.1