/* $Id$ */ /* * Driver for the iTalk binary protocol used by FasTrax */ #include #include #include #include #include #include #ifndef S_SPLINT_S #include #endif /* S_SPLINT_S */ #include #include #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_subframe(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, eph; double t; if (len != 296) { gpsd_report(LOG_PROG, "ITALK: bad NAV_FIX (len %zu, should be 296)\n", len); return -1; } flags = (ushort)getleuw(buf, 7 + 4); cflags = (ushort)getleuw(buf, 7 + 6); pflags = (ushort)getleuw(buf, 7 + 8); session->gpsdata.status = STATUS_NO_FIX; session->gpsdata.fix.mode = MODE_NO_FIX; mask = ONLINE_SET | MODE_SET | STATUS_SET | CLEAR_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 = (uint)getleul(buf, 7 + 84); t = gpstime_to_unix((int)gps_week, tow/1000.0) - session->context->leap_seconds; 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 ; eph = (double)(getlesl(buf, 7 + 252)/100.0); /* eph is a circular error, sqrt(epx**2 + epy**2) */ session->gpsdata.fix.epx = session->gpsdata.fix.epy = eph/sqrt(2); session->gpsdata.fix.eps = (double)(getlesl(buf, 7 + 254)/100.0); #define MAX(a,b) (((a) > (b)) ? (a) : (b)) session->gpsdata.satellites_used = (int)MAX(getleuw(buf, 7 + 12), getleuw(buf, 7 + 14)); mask |= USED_SET ; if (flags & FIX_CONV_DOP_VALID) { clear_dop(&session->gpsdata.dop); session->gpsdata.dop.hdop = (double)(getleuw(buf, 7 + 56)/100.0); session->gpsdata.dop.gdop = (double)(getleuw(buf, 7 + 58)/100.0); session->gpsdata.dop.pdop = (double)(getleuw(buf, 7 + 60)/100.0); session->gpsdata.dop.vdop = (double)(getleuw(buf, 7 + 62)/100.0); session->gpsdata.dop.tdop = (double)(getleuw(buf, 7 + 64)/100.0); mask |= DOP_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; } gpsd_report(LOG_DATA, "NAV_FIX: time=%.2f, lat=%.2f lon=%.2f alt=%.f speed=%.2f track=%.2f climb=%.2f mode=%d status=%d gdop=%.2f pdop=%.2f hdop=%.2f vdop=%.2f tdop=%.2f mask=%s\n", session->gpsdata.fix.time, session->gpsdata.fix.latitude, session->gpsdata.fix.longitude, session->gpsdata.fix.altitude, session->gpsdata.fix.speed, session->gpsdata.fix.track, session->gpsdata.fix.climb, session->gpsdata.fix.mode, session->gpsdata.status, session->gpsdata.dop.gdop, session->gpsdata.dop.pdop, session->gpsdata.dop.hdop, session->gpsdata.dop.vdop, session->gpsdata.dop.tdop, gpsd_maskdump(mask)); 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; gps_mask_t mask; if (len < 62) { gpsd_report(LOG_PROG, "ITALK: runt PRN_STATUS (len=%zu)\n", len); mask = ERROR_SET; } else { gps_week = (ushort)getleuw(buf, 7 + 4); tow = (uint)getleul(buf, 7 + 6); t = gpstime_to_unix((int)gps_week, tow/1000.0) - session->context->leap_seconds; session->gpsdata.skyview_time = t; gpsd_zero_satellites(&session->gpsdata); nsv = 0; nchan = (unsigned int)getleuw(buf, 7 +50); if (nchan > MAX_NR_VISIBLE_PRNS) nchan = MAX_NR_VISIBLE_PRNS; for (i = st = 0; i < nchan; i++) { unsigned int off = 7+ 52 + 10 * i; unsigned short flags; flags = (ushort)getleuw(buf, off); session->gpsdata.ss[i] = (float)(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_visible = (int)st; session->gpsdata.satellites_used = (int)nsv; mask = USED_SET | SATELLITE_SET;; gpsd_report(LOG_DATA, "PRN_STATUS: time=%.2f visible=%d used=%d mask={USED|SATELLITE}\n", session->gpsdata.fix.time, session->gpsdata.satellites_visible, session->gpsdata.satellites_used); } return mask; } 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 = (ushort)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 = (ushort)getleuw(buf, 7 + 36); tow = (uint)getleul(buf, 7 + 38); t = gpstime_to_unix((int)gps_week, tow/1000.0) - session->context->leap_seconds; session->gpsdata.fix.time = t; gpsd_report(LOG_DATA, "UTC_IONO_MODEL: time=%.2f mask={TIME}\n", session->gpsdata.fix.time); return TIME_SET; } static gps_mask_t decode_itk_subframe(struct gps_device_t *session, unsigned char *buf, size_t len) { unsigned short flags, prn, sf; unsigned int words[10]; if (len != 64) { gpsd_report(LOG_PROG, "ITALK: bad SUBFRAME (len %zu, should be 64)\n", len); return ERROR_SET; } flags = (ushort)getleuw(buf, 7 + 4); prn = (ushort)getleuw(buf, 7 + 6); sf = (ushort)getleuw(buf, 7 + 8); gpsd_report(LOG_PROG, "iTalk SUBFRAME prn %u sf %u - decode %s %s\n", prn, sf, flags & SUBFRAME_WORD_FLAG_MASK ? "error" : "ok", flags & SUBFRAME_GPS_PREAMBLE_INVERTED ? "(inverted)" : ""); if (flags & SUBFRAME_WORD_FLAG_MASK) return ONLINE_SET | ERROR_SET; // don't try decode an erroneous packet /* * Timo says "SUBRAME message contains decoded navigation message subframe * words with parity checking done but parity bits still present." */ /*@-type@*/ words[0] = (getbeul(buf, 7 + 14) & 0x3fffffff) >> 6; words[1] = (getleul(buf, 7 + 18) & 0x3fffffff) >> 6; words[2] = (getleul(buf, 7 + 22) & 0x3fffffff) >> 6; words[3] = (getleul(buf, 7 + 26) & 0x3fffffff) >> 6; words[4] = (getleul(buf, 7 + 30) & 0x3fffffff) >> 6; words[5] = (getleul(buf, 7 + 34) & 0x3fffffff) >> 6; words[6] = (getleul(buf, 7 + 38) & 0x3fffffff) >> 6; words[7] = (getleul(buf, 7 + 42) & 0x3fffffff) >> 6; words[8] = (getleul(buf, 7 + 46) & 0x3fffffff) >> 6; words[9] = (getleul(buf, 7 + 50) & 0x3fffffff) >> 6; /*@+type@*/ gpsd_interpret_subframe(session, words); return ONLINE_SET; } /*@ +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)); session->cycle_end_reliable = true; switch (type) { case ITALK_NAV_FIX: gpsd_report(LOG_IO, "iTalk NAV_FIX len %zu\n", len); mask = decode_itk_navfix(session, buf, len) | (CLEAR_SET|REPORT_SET); 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: mask = decode_itk_subframe(session, buf, 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.dev.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.dev.driver_mode = MODE_NMEA; /* NMEA */ return st; #endif /* NMEA_ENABLE */ } else return 0; } #ifdef ALLOW_CONTROLSEND /*@ +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)); status = write(session->gpsdata.gps_fd, msg, msglen); (void)tcdrain(session->gpsdata.gps_fd); return(status); } /*@ -charint +usedef +compdef @*/ #endif /* ALLOW_CONTROLSEND */ #ifdef ALLOW_RECONFIGURE 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.dev.baudrate, (char)session->gpsdata.dev.parity, (int)session->gpsdata.dev.stopbits, false); } } static void italk_event_hook(struct gps_device_t *session, event_t event) { /* * FIXME: It might not be necessary to call this on reactivate. * Experiment to see if the holds its settings through a close. */ if ((event == event_identified || event == event_reactivate) && session->packet.type == NMEA_PACKET) (void)italk_set_mode(session, session->gpsdata.dev.baudrate, (char)session->gpsdata.dev.parity, (int)session->gpsdata.dev.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 */ .probe_detect = NULL, /* how to detect at startup time */ .get_packet = generic_get, /* use generic packet grabber */ .parse_packet = italk_parse_input,/* parse message packets */ .rtcm_writer = pass_rtcm, /* send RTCM data straight */ .event_hook = italk_event_hook, /* lifetime event handler */ #ifdef ALLOW_RECONFIGURE .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 */ .min_cycle = 1, /* not relevant, no rate switch */ #endif /* ALLOW_RECONFIGURE */ #ifdef ALLOW_CONTROLSEND .control_send = italk_control_send, /* how to send a control string */ #endif /* ALLOW_CONTROLSEND */ }; #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 event_t event, *session) /* set synchronous mode */ { if (event == event_configure && session.packet_counter == 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); } } static bool itrax_speed(struct gps_device_t *session, speed_t speed, char parity UNUSED, int stopbits UNUSED) /* change the baud rate */ { return literal_send(session->gpsdata.gps_fd, ITRAX_MODESTRING, speed) >= 0; return false; } static bool itrax_rate(struct gps_device_t *session, double rate) /* change the sample rate of the GPS */ { return literal_send(session->gpsdata.gps_fd, "$PSFT,FIXRATE,%d\r\n", rate) >= 0; } #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 */ .probe_detect = NULL, /* no probe */ .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 */ .event_hook = event_hook, /* lietime event handler */ #ifdef ALLOW_RECONFIGURE .speed_switcher = itrax_speed, /* how to change speeds */ .mode_switcher = NULL, /* no mode switcher */ .rate_switcher = itrax_rate, /* there's a sample-rate switcher */ .min_cycle = 0, /* no hard limit */ #endif /* ALLOW_RECONFIGURE */ #ifdef ALLOW_CONTROLSEND .control_send = garmin_control_send, /* send raw bytes */ #endif /* ALLOW_CONTROLSEND */ }; /*@ -redef @*/ #endif /* ITRAX_ENABLE */