static gps_mask_t decode_itk_utcionomodel(struct gps_device_t *session, unsigned char *buf, size_t len) { int leap; unsigned short flags; if (len != 64) { gpsd_report(LOG_PROG, "ITALK: bad UTC_IONO_MODEL (len %zu, should be 64)\n", len); return 0; } flags = (ushort) getleu16(buf, 7); if (0 == (flags & UTC_IONO_MODEL_UTCVALID)) return 0; leap = (int)getleu16(buf, 7 + 24); if (session->context->leap_seconds < leap) session->context->leap_seconds = leap; session->newdata.time = gpsd_gpstime_resolve(session, (unsigned short) getleu16(buf, 7 + 36), (unsigned int)getleu32(buf, 7 + 38) / 1000.0); gpsd_report(LOG_DATA, "UTC_IONO_MODEL: time=%.2f mask={TIME}\n", session->newdata.time); return TIME_SET | PPSTIME_IS; }
/* * decode MID 0xDC, Measurement Time * * 10 bytes */ static gps_mask_t sky_msg_DC(struct gps_device_t *session, unsigned char *buf, size_t len) { unsigned int iod; /* Issue of data 0 - 255 */ unsigned int wn; /* week number 0 - 65535 */ unsigned int tow; /* receiver tow 0 - 604799999 in mS */ unsigned int mp; /* measurement period 1 - 1000 ms */ /* calculated */ double f_tow; /* tow in seconds */ unsigned int msec; /* mSec part of tow */ if ( 10 != len) return 0; iod = (unsigned int)getub(buf, 1); wn = getbeu16(buf, 2); tow = getbeu32(buf, 4); f_tow = (double)(tow / 1000); msec = tow % 1000; mp = getbeu16(buf, 8); /* should this be newdata.skyview_time? */ session->gpsdata.skyview_time = gpsd_gpstime_resolve(session, wn, f_tow ); gpsd_log(&session->context->errout, LOG_DATA, "Skytraq: MID 0xDC: iod=%u, wn=%u, tow=%u, mp=%u, t=%lld.%03u\n", iod, wn, tow, mp, (long long)session->gpsdata.skyview_time, msec); return 0; }
static gps_mask_t decode_itk_prnstatus(struct gps_device_t *session, unsigned char *buf, size_t len) { gps_mask_t mask; if (len < 62) { gpsd_log(&session->context->errout, LOG_PROG, "ITALK: runt PRN_STATUS (len=%zu)\n", len); mask = 0; } else { unsigned int i, nsv, nchan, st; session->gpsdata.skyview_time = gpsd_gpstime_resolve(session, (unsigned short)getleu16(buf, 7 + 4), (unsigned int)getleu32(buf, 7 + 6) / 1000.0), gpsd_zero_satellites(&session->gpsdata); nchan = (unsigned int)getleu16(buf, 7 + 50); if (nchan > MAX_NR_VISIBLE_PRNS) nchan = MAX_NR_VISIBLE_PRNS; for (i = st = nsv = 0; i < nchan; i++) { unsigned int off = 7 + 52 + 10 * i; unsigned short flags; bool used; flags = (unsigned short) getleu16(buf, off); used = (bool)(flags & PRN_FLAG_USE_IN_NAV); session->gpsdata.skyview[st].ss = (float)(getleu16(buf, off + 2) & 0xff); session->gpsdata.skyview[st].PRN = (short)(getleu16(buf, off + 4) & 0xff); session->gpsdata.skyview[st].elevation = (short)(getles16(buf, off + 6) & 0xff); session->gpsdata.skyview[st].azimuth = (short)(getles16(buf, off + 8) & 0xff); session->gpsdata.skyview[st].used = used; if (session->gpsdata.skyview[st].PRN > 0) { st++; if (used) nsv++; } } session->gpsdata.satellites_visible = (int)st; session->gpsdata.satellites_used = (int)nsv; mask = USED_IS | SATELLITE_SET;; gpsd_log(&session->context->errout, LOG_DATA, "PRN_STATUS: time=%.2f visible=%d used=%d mask={USED|SATELLITE}\n", session->newdata.time, session->gpsdata.satellites_visible, session->gpsdata.satellites_used); } return mask; }
static gps_mask_t decode_itk_pseudo(struct gps_device_t *session, unsigned char *buf, size_t len) { unsigned short flags, n, i; union long_double l_d; n = (ushort) getleu16(buf, 7 + 4); if ((n < 1) || (n > MAXCHANNELS)){ gpsd_report(LOG_INF, "ITALK: bad PSEUDO channel count\n"); return 0; } if (len != (size_t)((n+1)*36)) { gpsd_report(LOG_PROG, "ITALK: bad PSEUDO len %zu\n", len); } gpsd_report(LOG_PROG, "iTalk PSEUDO [%u]\n", n); flags = (unsigned short)getleu16(buf, 7 + 6); if ((flags & 0x3) != 0x3) return 0; // bail if measurement time not valid. session->newdata.time = gpsd_gpstime_resolve(session, (unsigned short int) getleu16(buf, 7 + 8), (unsigned int)getleu32(buf, 7 + 38) / 1000.0); /*@-type@*/ for (i = 0; i < n; i++){ session->gpsdata.PRN[i] = getleu16(buf, 7 + 26 + (i*36)) & 0xff; session->gpsdata.ss[i] = getleu16(buf, 7 + 26 + (i*36 + 2)) & 0x3f; session->gpsdata.raw.satstat[i] = getleu32(buf, 7 + 26 + (i*36 + 4)); session->gpsdata.raw.pseudorange[i] = getled(buf, 7 + 26 + (i*36 + 8)); session->gpsdata.raw.doppler[i] = getled(buf, 7 + 26 + (i*36 + 16)); session->gpsdata.raw.carrierphase[i] = getleu16(buf, 7 + 26 + (i*36 + 28)); session->gpsdata.raw.mtime[i] = session->newdata.time; session->gpsdata.raw.codephase[i] = NAN; session->gpsdata.raw.deltarange[i] = NAN; } /*@+type@*/ return RAW_IS; }
static gps_mask_t handle1002(struct gps_device_t *session) /* satellite signal quality report */ { int i; /* ticks = getzlong(6); */ /* sequence = getzword(8); */ /* measurement_sequence = getzword(9); */ /*@+charint@*/ int gps_week = getzword(10); int gps_seconds = getzlong(11); /* gps_nanoseconds = getzlong(13); */ /*@-charint@*/ /* Note: this week counter is not limited to 10 bits. */ session->context->gps_week = (unsigned short)gps_week; session->gpsdata.satellites_used = 0; for (i = 0; i < ZODIAC_CHANNELS; i++) { int status, prn; /*@ -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 (status & 1) session->gpsdata.satellites_used++; session->gpsdata.skyview[i].PRN = (short)prn; session->gpsdata.skyview[i].ss = (float)getzword(17 + (3 * i)); session->gpsdata.skyview[i].used = (bool)(status & 1); } session->gpsdata.skyview_time = gpsd_gpstime_resolve(session, (unsigned short)gps_week, (double)gps_seconds); gpsd_log(&session->context->errout, LOG_DATA, "1002: visible=%d used=%d mask={SATELLITE|USED}\n", session->gpsdata.satellites_visible, session->gpsdata.satellites_used); return SATELLITE_SET | USED_IS; }
static gps_mask_t decode_itk_navfix(struct gps_device_t *session, unsigned char *buf, size_t len) { unsigned short flags, pflags; gps_mask_t mask = 0; double epx, epy, epz, evx, evy, evz, eph; if (len != 296) { gpsd_report(LOG_PROG, "ITALK: bad NAV_FIX (len %zu, should be 296)\n", len); return -1; } flags = (ushort) getleu16(buf, 7 + 4); //cflags = (ushort) getleu16(buf, 7 + 6); pflags = (ushort) getleu16(buf, 7 + 8); session->gpsdata.status = STATUS_NO_FIX; session->newdata.mode = MODE_NO_FIX; mask = ONLINE_SET | MODE_SET | STATUS_SET | CLEAR_IS; /* just bail out if this fix is not marked valid */ if (0 != (pflags & FIX_FLAG_MASK_INVALID) || 0 == (flags & FIXINFO_FLAG_VALID)) return mask; session->newdata.time = gpsd_gpstime_resolve(session, (unsigned short) getles16(buf, 7 + 82), (unsigned int)getleu32(buf, 7 + 84) / 1000.0); mask |= TIME_SET | PPSTIME_IS; epx = (double)(getles32(buf, 7 + 96) / 100.0); epy = (double)(getles32(buf, 7 + 100) / 100.0); epz = (double)(getles32(buf, 7 + 104) / 100.0); evx = (double)(getles32(buf, 7 + 186) / 1000.0); evy = (double)(getles32(buf, 7 + 190) / 1000.0); evz = (double)(getles32(buf, 7 + 194) / 1000.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; eph = (double)(getles32(buf, 7 + 252) / 100.0); /* eph is a circular error, sqrt(epx**2 + epy**2) */ session->newdata.epx = session->newdata.epy = eph / sqrt(2); session->newdata.eps = (double)(getles32(buf, 7 + 254) / 100.0); #define MAX(a,b) (((a) > (b)) ? (a) : (b)) session->gpsdata.satellites_used = (int)MAX(getleu16(buf, 7 + 12), getleu16(buf, 7 + 14)); mask |= USED_IS; if (flags & FIX_CONV_DOP_VALID) { session->gpsdata.dop.hdop = (double)(getleu16(buf, 7 + 56) / 100.0); session->gpsdata.dop.gdop = (double)(getleu16(buf, 7 + 58) / 100.0); session->gpsdata.dop.pdop = (double)(getleu16(buf, 7 + 60) / 100.0); session->gpsdata.dop.vdop = (double)(getleu16(buf, 7 + 62) / 100.0); session->gpsdata.dop.tdop = (double)(getleu16(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->newdata.mode = MODE_3D; else session->newdata.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\n", session->newdata.time, session->newdata.latitude, session->newdata.longitude, session->newdata.altitude, session->newdata.speed, session->newdata.track, session->newdata.climb, session->newdata.mode, session->gpsdata.status, session->gpsdata.dop.gdop, session->gpsdata.dop.pdop, session->gpsdata.dop.hdop, session->gpsdata.dop.vdop, session->gpsdata.dop.tdop); return mask; }
/* * decode MID 0xDF, Nav status (PVT) * * 81 bytes */ static gps_mask_t sky_msg_DF(struct gps_device_t *session, unsigned char *buf, size_t len) { unsigned int iod; /* Issue of data 0 - 255 */ unsigned short navstat; unsigned int wn; /* week number 0 - 65535 */ double f_tow; /* receiver tow Sec */ double clock_bias; double clock_drift; gps_mask_t mask = 0; if ( 81 != len) return 0; iod = (unsigned int)getub(buf, 1); /* fix status is byte 2 */ navstat = (unsigned short)getub(buf, 2); session->gpsdata.status = STATUS_NO_FIX; session->newdata.mode = MODE_NO_FIX; switch ( navstat ) { case 1: /* fix prediction, ignore */ break; case 2: session->gpsdata.status = STATUS_FIX; session->newdata.mode = MODE_2D; break; case 3: session->gpsdata.status = STATUS_FIX; session->newdata.mode = MODE_3D; mask |= ALTITUDE_SET | CLIMB_SET; break; case 4: session->gpsdata.status = STATUS_DGPS_FIX; session->newdata.mode = MODE_3D; mask |= ALTITUDE_SET | CLIMB_SET; break; default: break; } wn = getbeu16(buf, 3); f_tow = getbed64((const char *)buf, 5); /* position/velocity is bytes 13-48, meters and m/s */ ecef_to_wgs84fix(&session->newdata, &session->gpsdata.separation, (double)getbed64((const char *)buf, 13), (double)getbed64((const char *)buf, 21), (double)getbed64((const char *)buf, 29), (double)getbef32((const char *)buf, 37), (double)getbef32((const char *)buf, 41), (double)getbef32((const char *)buf, 46)); clock_bias = getbed64((const char *)buf, 49); clock_drift = getbes32(buf, 57); session->gpsdata.dop.gdop = getbef32((const char *)buf, 61); session->gpsdata.dop.pdop = getbef32((const char *)buf, 65); session->gpsdata.dop.hdop = getbef32((const char *)buf, 69); session->gpsdata.dop.vdop = getbef32((const char *)buf, 73); session->gpsdata.dop.tdop = getbef32((const char *)buf, 77); session->newdata.time = gpsd_gpstime_resolve(session, wn, f_tow ); gpsd_log(&session->context->errout, LOG_DATA, "Skytraq: MID 0xDF: iod=%u, stat=%u, wn=%u, tow=%f, t=%.6f " "cb: %f, cd: %f " "gdop: %.2f, pdop: %.2f, hdop: %.2f, vdop: %.2f, tdop: %.2f\n", iod, navstat, wn, f_tow, session->newdata.time, clock_bias, clock_drift, session->gpsdata.dop.gdop, session->gpsdata.dop.pdop, session->gpsdata.dop.hdop, session->gpsdata.dop.vdop, session->gpsdata.dop.tdop); mask |= TIME_SET | LATLON_SET | TRACK_SET | SPEED_SET | STATUS_SET | MODE_SET | DOP_SET | CLEAR_IS | REPORT_IS; return mask; }
/*@ +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 type, used, visible, satcnt, j, k; double version; gps_mask_t mask = 0; /* must have two leader bytes, length, and two trailer bytes minimum */ if (len < 5) return 0; /* time to unstuff it and discard the header and footer */ cp = buf + 2; if (*cp == 0x10) cp++; datalen = (size_t) * cp++; datalen -= 2; /*@ -usedef @*/ /* prevent 'Assigned value is garbage or undefined' from scan-build */ memset(buf2, '\0', sizeof(buf2)); tp = buf2; for (i = 0; i < (size_t) datalen; i++) { *tp = *cp++; if (*tp == 0x10) cp++; tp++; } type = (unsigned char)getub(buf2, 0); /*@ +usedef @*/ /*@ -usedef -compdef @*/ gpsd_log(&session->context->errout, LOG_RAW, "EverMore packet type 0x%02x (%zd bytes)\n", type, tp-buf2); /*@ +usedef +compdef @*/ session->cycle_end_reliable = true; switch (type) { case 0x02: /* Navigation Data Output */ session->newdata.time = gpsd_gpstime_resolve(session, (unsigned short)getleu16(buf2, 3), (double)getleu32(buf2, 5) * 0.01); ecef_to_wgs84fix(&session->newdata, &session->gpsdata.separation, (double)getles32(buf2, 9) * 1.0, (double)getles32(buf2, 13) * 1.0, (double)getles32(buf2, 17) * 1.0, (double)getles16(buf2, 21) / 10.0, (double)getles16(buf2, 23) / 10.0, (double)getles16(buf2, 25) / 10.0); used = (unsigned char)getub(buf2, 27) & 0x0f; //visible = (getub(buf2, 27) & 0xf0) >> 4; version = (uint) getleu16(buf2, 28) / 100.0; /* that's all the information in this packet */ if (used < 3) session->newdata.mode = MODE_NO_FIX; else if (used == 3) session->newdata.mode = MODE_2D; else { session->newdata.mode = MODE_3D; mask |= ALTITUDE_SET | CLIMB_SET; } mask |= TIME_SET | PPSTIME_IS | LATLON_SET | TRACK_SET | SPEED_SET | MODE_SET; if (session->subtype[0] == '\0') { (void)snprintf(session->subtype, sizeof(session->subtype), "%3.2f", version); mask |= DEVICEID_SET; } gpsd_log(&session->context->errout, LOG_DATA, "NDO 0x02: time=%.2f, lat=%.2f lon=%.2f alt=%.2f speed=%.2f track=%.2f climb=%.2f mode=%d subtype='%s\n", session->newdata.time, session->newdata.latitude, session->newdata.longitude, session->newdata.altitude, session->newdata.speed, session->newdata.track, session->newdata.climb, session->newdata.mode, session->gpsdata.dev.subtype); return mask | CLEAR_IS | REPORT_IS; case 0x04: /* DOP Data Output */ session->newdata.time = gpsd_gpstime_resolve(session, (unsigned short)getleu16(buf2, 3), (double)getleu32(buf2, 5) * 0.01); /* * 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 visibility matrix, trusting * the firmware algorithms over ours. */ session->gpsdata.dop.gdop = (double)getub(buf2, 9) * 0.1; session->gpsdata.dop.pdop = (double)getub(buf2, 10) * 0.1; session->gpsdata.dop.hdop = (double)getub(buf2, 11) * 0.1; session->gpsdata.dop.vdop = (double)getub(buf2, 12) * 0.1; session->gpsdata.dop.tdop = (double)getub(buf2, 13) * 0.1; switch (getub(buf2, 14)) { case 0: /* no position fix */ case 1: /* manual calls this "1D navigation" */ session->gpsdata.status = STATUS_NO_FIX; session->newdata.mode = MODE_NO_FIX; break; case 2: /* 2D navigation */ session->gpsdata.status = STATUS_FIX; session->newdata.mode = MODE_2D; break; case 3: /* 3D navigation */ session->gpsdata.status = STATUS_FIX; session->newdata.mode = MODE_3D; break; case 4: /* 3D navigation with DGPS */ session->gpsdata.status = STATUS_DGPS_FIX; session->newdata.mode = MODE_3D; break; } /* that's all the information in this packet */ mask = TIME_SET | PPSTIME_IS | DOP_SET | MODE_SET | STATUS_SET; gpsd_log(&session->context->errout, LOG_DATA, "DDO 0x04: gdop=%.2f pdop=%.2f hdop=%.2f vdop=%.2f tdop=%.2f mode=%d, status=%d mask={TIME| DOP|MODE|STATUS}\n", session->gpsdata.dop.gdop, session->gpsdata.dop.pdop, session->gpsdata.dop.hdop, session->gpsdata.dop.vdop, session->gpsdata.dop.tdop, session->newdata.mode, session->gpsdata.status); return mask; case 0x06: /* Channel Status Output */ session->gpsdata.skyview_time = gpsd_gpstime_resolve(session, (unsigned short)getleu16(buf2, 3), (double)getleu32(buf2, 5) * 0.01); session->gpsdata.satellites_visible = (int)getub(buf2, 9); gpsd_zero_satellites(&session->gpsdata); memset(session->gpsdata.used, 0, sizeof(session->gpsdata.used)); if (session->gpsdata.satellites_visible > 12) { gpsd_log(&session->context->errout, LOG_WARN, "Warning: EverMore packet has information about %d satellites!\n", session->gpsdata.satellites_visible); } if (session->gpsdata.satellites_visible > EVERMORE_CHANNELS) session->gpsdata.satellites_visible = EVERMORE_CHANNELS; satcnt = 0; for (i = 0; i < (size_t) session->gpsdata.satellites_visible; i++) { int prn; // channel = getub(buf2, 7*i+7+3) prn = (int)getub(buf2, 7 * i + 7 + 4); if (prn == 0) continue; /* satellite record is not valid */ session->gpsdata.PRN[satcnt] = prn; session->gpsdata.azimuth[satcnt] = (int)getleu16(buf2, 7 * i + 7 + 5); session->gpsdata.elevation[satcnt] = (int)getub(buf2, 7 * i + 7 + 7); session->gpsdata.ss[satcnt] = (float)getub(buf2, 7 * i + 7 + 8); /* * 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 + 9) & 0x40) { session->gpsdata.used[session->gpsdata.satellites_used++] = prn; } satcnt++; } session->gpsdata.satellites_visible = (int)satcnt; /* that's all the information in this packet */ mask = SATELLITE_SET | USED_IS; gpsd_log(&session->context->errout, LOG_DATA, "CSO 0x06: time=%.2f used=%d visible=%d mask={TIME|SATELLITE|USED}\n", session->newdata.time, session->gpsdata.satellites_used, session->gpsdata.satellites_visible); return mask; case 0x08: /* Measurement Data Output */ /* clock offset is a manufacturer diagnostic */ /* (int)getleu16(buf2, 9); clock offset, 29000..29850 ?? */ session->newdata.time = gpsd_gpstime_resolve(session, (unsigned short)getleu16(buf2, 3), (double)getleu32(buf2, 5) * 0.01); visible = (unsigned char)getub(buf2, 11); /* * Note: This code is untested. It was written from the manual. * The results need to be sanity-checked against a GPS with * known-good raw decoding and the same skyview. * * We can get pseudo range (m), delta-range (m/s), doppler (Hz) * and status for each channel from the chip. We cannot get * codephase or carrierphase. */ #define SBITS(sat, s, l) sbits((signed char *)buf, 10 + (sat*14) + s, l, false) #define UBITS(sat, s, l) ubits((unsigned char *)buf, 10 + (sat*14) + s, l, false) for (k = 0; k < visible; k++) { int prn = (int)UBITS(k, 4, 5); /* this is so we can tell which never got set */ for (j = 0; j < MAXCHANNELS; j++) session->gpsdata.raw.mtime[j] = 0; for (j = 0; j < MAXCHANNELS; j++) { if (session->gpsdata.PRN[j] == prn) { session->gpsdata.raw.codephase[j] = NAN; session->gpsdata.raw.carrierphase[j] = NAN; session->gpsdata.raw.mtime[j] = session->newdata.time; session->gpsdata.raw.satstat[j] = (unsigned)UBITS(k, 24, 8); session->gpsdata.raw.pseudorange[j] = (double)SBITS(k,40,32); session->gpsdata.raw.deltarange[j] = (double)SBITS(k,72,32); session->gpsdata.raw.doppler[j] = (double)SBITS(k, 104, 16); } } } #undef SBITS #undef UBITS gpsd_log(&session->context->errout, LOG_DATA, "MDO 0x04: time=%.2f mask={TIME|RAW}\n", session->newdata.time); return TIME_SET | PPSTIME_IS | RAW_IS; case 0x20: /* LogConfig Info, could be used as a probe for EverMore GPS */ gpsd_log(&session->context->errout, LOG_DATA, "LogConfig EverMore packet, length %zd\n", datalen); return ONLINE_SET; case 0x22: /* LogData */ gpsd_log(&session->context->errout, LOG_DATA, "LogData EverMore packet, length %zd\n", datalen); return ONLINE_SET; case 0x38: /* ACK */ gpsd_log(&session->context->errout, LOG_PROG, "EverMore command %02X ACK\n", getub(buf2, 3)); return ONLINE_SET; default: gpsd_log(&session->context->errout, LOG_WARN, "unknown EverMore packet EID 0x%02x, length %zd\n", buf2[0], datalen); return 0; } }