/* * UBX driver * * Week counters are not limited to 10 bits. It's unknown what * the firmware is doing to disambiguate them, if anything; it might just * be adding a fixed offset based on a hidden epoch value, in which case * unhappy things will occur on the next rollover. * * This file is Copyright (c) 2010 by the GPSD project * BSD terms apply: see the file COPYING in the distribution root for details. * */ #include #include #include #include #include #include #ifndef S_SPLINT_S #include #endif /* S_SPLINT_S */ #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 gps_mask_t ubx_parse(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(struct gps_device_t *session, 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; if (data_len != 52) return 0; flags = (unsigned int)getub(buf, 11); mask = 0; if ((flags & (UBX_SOL_VALID_WEEK | UBX_SOL_VALID_TIME)) != 0) { tow = (unsigned int)getleu32(buf, 0); gw = (unsigned short)getles16(buf, 8); session->newdata.time = gpsd_gpstime_resolve(session, gw, tow / 1000.0); mask |= TIME_SET | PPSTIME_IS; } epx = (double)(getles32(buf, 12) / 100.0); epy = (double)(getles32(buf, 16) / 100.0); epz = (double)(getles32(buf, 20) / 100.0); evx = (double)(getles32(buf, 28) / 100.0); evy = (double)(getles32(buf, 32) / 100.0); evz = (double)(getles32(buf, 36) / 100.0); ecef_to_wgs84fix(&session->newdata, &session->gpsdata.separation, epx, epy, epz, evx, evy, evz); mask |= LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET | CLIMB_SET; session->newdata.epx = session->newdata.epy = (double)(getles32(buf, 24) / 100.0) / sqrt(2); session->newdata.eps = (double)(getles32(buf, 40) / 100.0); /* Better to have a single point of truth about DOPs */ //session->gpsdata.dop.pdop = (double)(getleu16(buf, 44)/100.0); session->gpsdata.satellites_used = (int)getub(buf, 47); navmode = (unsigned char)getub(buf, 10); switch (navmode) { case UBX_MODE_TMONLY: case UBX_MODE_3D: session->newdata.mode = MODE_3D; break; case UBX_MODE_2D: case UBX_MODE_DR: /* consider this too as 2D */ case UBX_MODE_GPSDR: /* FIX-ME: DR-aided GPS may be valid 3D */ session->newdata.mode = MODE_2D; break; default: session->newdata.mode = MODE_NO_FIX; } if ((flags & UBX_SOL_FLAG_DGPS) != 0) session->gpsdata.status = STATUS_DGPS_FIX; else if (session->newdata.mode != MODE_NO_FIX) session->gpsdata.status = STATUS_FIX; mask |= MODE_SET | STATUS_SET; gpsd_report(LOG_DATA, "NAVSOL: time=%.2f lat=%.2f lon=%.2f alt=%.2f track=%.2f speed=%.2f climb=%.2f mode=%d status=%d used=%d\n", session->newdata.time, session->newdata.latitude, session->newdata.longitude, session->newdata.altitude, session->newdata.track, session->newdata.speed, session->newdata.climb, session->newdata.mode, session->gpsdata.status, session->gpsdata.satellites_used); 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; /* * We make a deliberate choice not to clear DOPs from the * last skyview here, but rather to treat this as a supplement * to our calculations from the visiniolity matrix, trusting * the firmware algorithms over ours. */ session->gpsdata.dop.gdop = (double)(getleu16(buf, 4) / 100.0); session->gpsdata.dop.pdop = (double)(getleu16(buf, 6) / 100.0); session->gpsdata.dop.tdop = (double)(getleu16(buf, 8) / 100.0); session->gpsdata.dop.vdop = (double)(getleu16(buf, 10) / 100.0); session->gpsdata.dop.hdop = (double)(getleu16(buf, 12) / 100.0); gpsd_report(LOG_DATA, "NAVDOP: gdop=%.2f pdop=%.2f " "hdop=%.2f vdop=%.2f tdop=%.2f mask={DOP}\n", session->gpsdata.dop.gdop, session->gpsdata.dop.hdop, session->gpsdata.dop.vdop, session->gpsdata.dop.pdop, session->gpsdata.dop.tdop); 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; if (data_len != 16) return 0; tow = (unsigned int)getleu32(buf, 0); gw = (unsigned int)getles16(buf, 8); flags = (unsigned int)getub(buf, 11); if ((flags & 0x7) != 0) session->context->leap_seconds = (int)getub(buf, 10); session->newdata.time = gpsd_gpstime_resolve(session, (unsigned short int)gw, (double)tow / 1000.0); gpsd_report(LOG_DATA, "TIMEGPS: time=%.2f mask={TIME}\n", session->newdata.time); return TIME_SET | PPSTIME_IS; } /** * 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, nchan, nsv, st; if (data_len < 152) { gpsd_report(LOG_PROG, "runt svinfo (datalen=%zd)\n", data_len); return 0; } /*@ +charint @*/ nchan = (unsigned int)getub(buf, 4); if (nchan > MAXCHANNELS) { gpsd_report(LOG_WARN, "Invalid NAV SVINFO message, >%d reported visible", 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] = (float)getub(buf, off + 4); session->gpsdata.elevation[j] = (int)getsb(buf, off + 5); session->gpsdata.azimuth[j] = (int)getles16(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)session->driver.ubx.sbas_in_use) session->gpsdata.used[nsv++] = session->gpsdata.PRN[j]; /*@ +predboolothers */ j++; } session->gpsdata.skyview_time = NAN; session->gpsdata.satellites_visible = (int)st; session->gpsdata.satellites_used = (int)nsv; gpsd_report(LOG_DATA, "SVINFO: visible=%d used=%d mask={SATELLITE|USED}\n", session->gpsdata.satellites_visible, session->gpsdata.satellites_used); return SATELLITE_SET | USED_IS; } /* * SBAS Info */ static void ubx_msg_sbas(struct gps_device_t *session, unsigned char *buf) { #ifdef __UNUSED_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 /* __UNUSED_DEBUG__ */ /* 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 */ session->driver.ubx.sbas_in_use = (unsigned char)getub(buf, 4); } /* * Raw Subframes */ static gps_mask_t ubx_msg_sfrb(struct gps_device_t *session, unsigned char *buf) { unsigned int i, chan, svid; uint32_t words[10]; chan = (unsigned int)getub(buf, 0); svid = (unsigned int)getub(buf, 1); gpsd_report(LOG_PROG, "UBX_RXM_SFRB: %u %u\n", chan, svid); /* UBX does all the parity checking, but still bad data gets through */ for (i = 0; i < 10; i++) { words[i] = (uint32_t)getleu32(buf, 4 * i + 2) & 0xffffff; } return gpsd_interpret_subframe(session, svid, words); } 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; session->cycle_end_reliable = true; /* extract message id and length */ msgid = (buf[2] << 8) | buf[3]; data_len = (size_t) getles16(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) | (CLEAR_IS | REPORT_IS); 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(session, &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: mask = ubx_msg_sfrb(session, &buf[6]); 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++) session->driver.ubx.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 */ session->driver.ubx.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)\n", msgid, len); } 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 = 0; if (session->packet.type == UBX_PACKET) { st = ubx_parse(session, session->packet.outbuffer, session->packet.outbuflen); session->gpsdata.dev.driver_mode = MODE_BINARY; #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; #endif /* NMEA_ENABLE */ } return st; } bool ubx_write(struct gps_device_t * session, unsigned int msg_class, unsigned int msg_id, unsigned char *msg, unsigned short data_len) { unsigned char CK_A, CK_B; ssize_t i, count; bool ok; /*@ -type @*/ session->msgbuf[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 @*/ if (msg != NULL) 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 + 8; /*@ +type @*/ gpsd_report(LOG_IO, "=> GPS: UBX class: %02x, id: %02x, len: %d, crc: %02x%02x\n", msg_class, msg_id, data_len, CK_A, CK_B); count = gpsd_write(session, session->msgbuf, session->msgbuflen); ok = (count == (ssize_t) session->msgbuflen); /*@ +nullderef @*/ return (ok); } #ifdef CONTROLSEND_ENABLE 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; } #endif /* CONTROLSEND_ENABLE */ static 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; i < end; i++) { if ((*ip == 0x00) || (*ip == '*')) { *op = 0x00; break; } *(op++) = *(ip++); } /*@ -charint */ } static void ubx_event_hook(struct gps_device_t *session, event_t event) { if (session->context->readonly) return; if (event == event_triggermatch) ubx_catch_model(session, session->packet.outbuffer, session->packet.outbuflen); else if (event == event_identified || event == event_reactivate) { unsigned char msg[32]; gpsd_report(LOG_IO, "UBX configure: %d\n", session->packet.counter); (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 @*/ } else if (event == event_deactivate) { /*@ -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 */ } } #ifdef RECONFIGURE_ENABLE static void ubx_nmea_mode(struct gps_device_t *session, int mode) { int i; unsigned char buf[sizeof(session->driver.ubx.original_port_settings)]; if (!session->driver.ubx.have_port_configuration) return; /*@ +charint -usedef @*/ for (i = 0; i < (int)sizeof(session->driver.ubx.original_port_settings); i++) buf[i] = session->driver.ubx.original_port_settings[i]; /* copy the original port settings */ if (buf[0] == 0x01) /* set baudrate on serial port only */ putle32(buf, 8, session->gpsdata.dev.baudrate); if (mode == MODE_NMEA) { buf[14] &= ~0x01; /* turn off UBX output on this port */ buf[14] |= 0x02; /* turn on NMEA output on this port */ } else { /* MODE_BINARY */ 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[sizeof(session->driver.ubx.original_port_settings)]; unsigned long usart_mode; /*@ +charint -usedef -compdef */ for (i = 0; i < (int)sizeof(session->driver.ubx.original_port_settings); i++) buf[i] = session->driver.ubx.original_port_settings[i]; /* copy the original port settings */ if ((!session->driver.ubx.have_port_configuration) || (buf[0] != 0x01)) /* set baudrate on serial port only */ return false; usart_mode = (unsigned long)getleu32(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 */ putle32(buf, 4, usart_mode); putle32(buf, 8, speed); (void)ubx_write(session, 0x06, 0x00, &buf[6], 20); /* send back with all other settings intact */ /*@ -charint +usedef +compdef */ return true; } static bool ubx_rate(struct gps_device_t *session, double cycletime) /* change the sample rate of the GPS */ { unsigned short s; /*@ -type @*/ unsigned char msg[6] = { 0x00, 0x00, /* U2: Measurement rate (ms) */ 0x00, 0x01, /* U2: Navigation rate (cycles) */ 0x00, 0x00, /* U2: Alignment to reference time: 0 = UTC, !0 = GPS */ }; /*@ +type @*/ /* clamp to cycle times that i know work on my receiver */ if (cycletime > 1000.0) cycletime = 1000.0; if (cycletime < 200.0) cycletime = 200.0; gpsd_report(LOG_IO, "UBX rate change, report every %f secs\n", cycletime); s = (unsigned short)cycletime; msg[0] = (unsigned char)(s >> 8); msg[1] = (unsigned char)(s & 0xff); return ubx_write(session, 0x06, 0x08, msg, 6); /* CFG-RATE */ } #endif /* RECONFIGURE_ENABLE */ /* This is everything we export */ /* *INDENT-OFF* */ const struct gps_type_t ubx_binary = { .type_name = "uBlox UBX binary", /* Full name of type */ .packet_type = UBX_PACKET, /* associated lexer packet type */ .flags = DRIVER_NOFLAGS, /* no flags set */ .trigger = "$GPTXT,01,01,02,MOD", .channels = 50, /* Number of satellite channels supported by the device */ .probe_detect = NULL, /* Startup-time device detector */ .get_packet = generic_get, /* Packet getter (using default routine) */ .parse_packet = parse_input, /* Parse message packets */ .rtcm_writer = gpsd_write, /* RTCM handler (using default routine) */ .event_hook = ubx_event_hook, /* Fiew in variious lifetime events */ #ifdef RECONFIGURE_ENABLE .speed_switcher = ubx_speed, /* Speed (baudrate) switch */ .mode_switcher = ubx_nmea_mode, /* Switch to NMEA mode */ .rate_switcher = ubx_rate, /* Message delivery rate switcher */ .min_cycle = 0.25, /* Maximum 4Hz sample rate */ #endif /* RECONFIGURE_ENABLE */ #ifdef CONTROLSEND_ENABLE .control_send = ubx_control_send, /* no control sender yet */ #endif /* CONTROLSEND_ENABLE */ #ifdef NTPSHM_ENABLE .ntp_offset = NULL, /* no method for NTP fudge factor */ #endif /* NTPSHM_ ENABLE */ }; /* *INDENT-ON* */ #endif /* defined(UBX_ENABLE) && defined(BINARY_ENABLE) */