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; }
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 i; uint32_t words[10]; if (len != 64) { gpsd_report(LOG_PROG, "ITALK: bad SUBFRAME (len %zu, should be 64)\n", len); return 0; } flags = (ushort) getleu16(buf, 7 + 4); prn = (ushort) getleu16(buf, 7 + 6); sf = (ushort) getleu16(buf, 7 + 8); gpsd_report(LOG_PROG, "iTalk 50B 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 0; // 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." */ for (i = 0; i < 10; i++) words[i] = (uint32_t)(getleu32(buf, 7 + 14 + 4 * i) >> 6) & 0xffffff; return gpsd_interpret_subframe(session, prn, words); }
static void display_itk_prnstatus(unsigned char *buf, size_t len) { int i, nchan; if (len < 62) return; nchan = (int)getleu16(buf, 7 + 50); if (nchan > MAX_NR_VISIBLE_PRNS) nchan = MAX_NR_VISIBLE_PRNS; for (i = 0; i < nchan; i++) { int off = 7 + 52 + 10 * i; unsigned short fl; unsigned char ss, prn, el, az; fl = (unsigned short)getleu16(buf, off); ss = (unsigned char)getleu16(buf, off + 2) & 0xff; prn = (unsigned char)getleu16(buf, off + 4) & 0xff; el = (unsigned char)getles16(buf, off + 6) & 0xff; az = (unsigned char)getles16(buf, off + 8) & 0xff; (void)wmove(satwin, i + 2, 4); (void)wprintw(satwin, "%3d %3d %2d %02d %04x %c", prn, az, el, ss, fl, (fl & PRN_FLAG_USE_IN_NAV) ? 'Y' : ' '); } for (; i < MAX_NR_VISIBLE_PRNS; i++) { (void)wmove(satwin, (int)i + 2, 4); (void)wprintw(satwin, " "); } (void)wnoutrefresh(satwin); return; }
static void ledumpall(void) { (void)printf("getsb: %016" PRIx64 " %016" PRIx64 " %016" PRIx64 " %016" PRIx64 "\n", (uint64_t) sb1, (uint64_t) sb2, (uint64_t) getsb(buf, 0), (uint64_t) getsb(buf, 8)); (void)printf("getub: %016" PRIx64 " %016" PRIx64 " %016" PRIx64 " %016" PRIx64 "\n", (uint64_t) ub1, (uint64_t) ub2, (uint64_t) getub(buf, 0), (uint64_t) getub(buf, 8)); (void)printf("getles16: %016" PRIx64 " %016" PRIx64 " %016" PRIx64 " %016" PRIx64 "\n", (uint64_t) sw1, (uint64_t) sw2, (uint64_t) getles16(buf, 0), (uint64_t) getles16(buf, 8)); (void)printf("getleu16: %016" PRIx64 " %016" PRIx64 " %016" PRIx64 " %016" PRIx64 "\n", (uint64_t) uw1, (uint64_t) uw2, (uint64_t) getleu16(buf, 0), (uint64_t) getleu16(buf, 8)); (void)printf("getles32: %016" PRIx64 " %016" PRIx64 " %016" PRIx64 " %016" PRIx64 "\n", (uint64_t) sl1, (uint64_t) sl2, (uint64_t) getles32(buf, 0), (uint64_t) getles32(buf, 8)); (void)printf("getleu32: %016" PRIx64 " %016" PRIx64 " %016" PRIx64 " %016" PRIx64 "\n", (uint64_t) ul1, (uint64_t) ul2, (uint64_t) getleu32(buf, 0), (uint64_t) getleu32(buf, 8)); (void)printf("getles64: %016" PRIx64 " %016" PRIx64 " %016" PRIx64 " %016" PRIx64 "\n", (uint64_t) sL1, (uint64_t) sL2, (uint64_t) getles64(buf, 0), (uint64_t) getles64(buf, 8)); (void)printf("getleu64: %016" PRIx64 " %016" PRIx64 " %016" PRIx64 " %016" PRIx64 "\n", (uint64_t) uL1, (uint64_t) uL2, (uint64_t) getleu64(buf, 0), (uint64_t) getleu64(buf, 8)); (void)printf("getlef32: %f %f\n", f1, getlef32((const char *)buf, 24)); (void)printf("getled64: %.16f %.16f\n", d1, getled64((const char *)buf, 16)); }
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 void display_nav_svinfo(unsigned char *buf, size_t data_len) { int i, nchan; if (data_len < 152) return; nchan = (int)getub(buf, 4); if (nchan > 16) nchan = 16; for (i = 0; i < nchan; i++) { int off = 8 + 12 * i; unsigned char ss, prn; char el; short az; unsigned short fl; prn = (unsigned char)getub(buf, off + 1); fl = (unsigned short)getleu16(buf, off + 2); ss = (unsigned char)getub(buf, off + 4); el = (char)getsb(buf, off + 5); az = (short)getles16(buf, off + 6); (void)wmove(satwin, (int)(i + 2), 4); (void)wprintw(satwin, "%3d %3d %3d %2d %04x %c", prn, az, el, ss, fl, (fl & UBX_SAT_USED) ? 'Y' : ' '); } (void)wnoutrefresh(satwin); return; }
static void display_nav_dop(unsigned char *buf, size_t data_len) { if (data_len != 18) return; (void)wmove(dopwin, 1, 9); (void)wprintw(dopwin, "%4.1f", getleu16(buf, 12) / 100.0); (void)wmove(dopwin, 1, 18); (void)wprintw(dopwin, "%4.1f", getleu16(buf, 10) / 100.0); (void)wmove(dopwin, 1, 27); (void)wprintw(dopwin, "%4.1f", getleu16(buf, 6) / 100.0); (void)wmove(dopwin, 1, 36); (void)wprintw(dopwin, "%4.1f", getleu16(buf, 8) / 100.0); (void)wmove(dopwin, 1, 45); (void)wprintw(dopwin, "%4.1f", getleu16(buf, 4) / 100.0); (void)wnoutrefresh(dopwin); }
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 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; }
/*@ -duplicatequals +ignorequals @*/ int main(int argc, char *argv[]) { bool failures = false; bool quiet = (argc > 1) && (strcmp(argv[1], "--quiet") == 0); /*@ -observertrans -usereleased @*/ struct unsigned_test *up, unsigned_tests[] = { /* tests using the big buffer */ {buf, 0, 1, 0, false, "first bit of first byte"}, {buf, 0, 8, 0x01, false, "first 8 bits"}, {buf, 32, 7, 0x02, false, "first seven bits of fifth byte (0x05)"}, {buf, 56, 12, 0x8f, false, "12 bits crossing 7th to 8th bytes (0x08ff)"}, {buf, 78, 4, 0xb, false, "4 bits crossing 8th to 9th byte (0xfefd)"}, {buf, 0, 1, 0, true, "first bit of first byte"}, {buf, 0, 8, 0x80, true, "first 8 bits"}, {buf, 32, 7, 0x20, true, "first seven bits of fifth byte (0x05)"}, {buf, 56, 12, 0xf10,true, "12 bits crossing 7th to 8th bytes (0x08ff)"}, {buf, 78, 4, 0xd, true, "4 bits crossing 8th to 9th byte (0xfefd)"}, /* sporadic tests based on found bugs */ {(unsigned char *)"\x19\x23\f6", 7, 2, 2, false, "2 bits crossing 1st to 2nd byte (0x1923)"}, }; unsigned char *sp; memcpy(buf, "\x01\x02\x03\x04\x05\x06\x07\x08", 8); memcpy(buf + 8, "\xff\xfe\xfd\xfc\xfb\xfa\xf9\xf8", 8); memcpy(buf + 16, "\x40\x09\x21\xfb\x54\x44\x2d\x18", 8); memcpy(buf + 24, "\x40\x49\x0f\xdb", 4); /*@ +observertrans +usereleased @*/ /*@-type@*/ sb1 = getsb(buf, 0); sb2 = getsb(buf, 8); ub1 = getub(buf, 0); ub2 = getub(buf, 8); sw1 = getbes16(buf, 0); sw2 = getbes16(buf, 8); uw1 = getbeu16(buf, 0); uw2 = getbeu16(buf, 8); sl1 = getbes32(buf, 0); sl2 = getbes32(buf, 8); ul1 = getbeu32(buf, 0); ul2 = getbeu32(buf, 8); sL1 = getbes64(buf, 0); sL2 = getbes64(buf, 8); uL1 = getbeu64(buf, 0); uL2 = getbeu64(buf, 8); f1 = getbef32((const char *)buf, 24); d1 = getbed64((const char *)buf, 16); sb1 = getsb(buf, 0); sb2 = getsb(buf, 8); ub1 = getub(buf, 0); ub2 = getub(buf, 8); sw1 = getles16(buf, 0); sw2 = getles16(buf, 8); uw1 = getleu16(buf, 0); uw2 = getleu16(buf, 8); sl1 = getles32(buf, 0); sl2 = getles32(buf, 8); ul1 = getleu32(buf, 0); ul2 = getleu32(buf, 8); sL1 = getles64(buf, 0); sL2 = getles64(buf, 8); uL1 = getleu64(buf, 0); uL2 = getleu64(buf, 8); f1 = getlef32((const char *)buf, 24); d1 = getled64((const char *)buf, 16); /*@+type@*/ if (!quiet) { (void)fputs("Test data:", stdout); for (sp = buf; sp < buf + 28; sp++) (void)printf(" %02x", *sp); (void)putc('\n', stdout); /* big-endian test */ printf("Big-endian:\n"); bedumpall(); /* little-endian test */ printf("Little-endian:\n"); ledumpall(); } if (sb1 != 1) printf("getsb(buf, 0) FAILED\n"); if (sb2 != -1) printf("getsb(buf, 8) FAILED\n"); if (ub1 != 1) printf("getub(buf, 0) FAILED\n"); if (ub2 != 0xff) printf("getub(buf, 8) FAILED\n"); (void)printf("Testing bitfield extraction\n"); for (up = unsigned_tests; up < unsigned_tests + sizeof(unsigned_tests) / sizeof(unsigned_tests[0]); up++) { uint64_t res = ubits((unsigned char *)buf, up->start, up->width, up->le); bool success = (res == up->expected); if (!success) failures = true; if (!success || !quiet) (void)printf("ubits(%s, %d, %d, %s) %s should be %" PRIx64 ", is %" PRIx64 ": %s\n", hexdump(buf, strlen((char *)buf)), up->start, up->width, up->le ? "true" : "false", up->description, up->expected, res, success ? "succeeded" : "FAILED"); } exit(failures ? EXIT_FAILURE : EXIT_SUCCESS); }
static void display_itk_navfix(unsigned char *buf, size_t len) { unsigned int tow, tod, d, svlist; unsigned short gps_week, nsv; unsigned short year, mon, day, hour, min, sec; double epx, epy, epz, evx, evy, evz; double latitude, longitude; float altitude, speed, track, climb; float hdop, gdop, pdop, vdop, tdop; if (len != 296) return; #ifdef __UNUSED__ flags = (unsigned short) getleu16(buf, 7 + 4); */ cflags = (unsigned short) getleu16(buf, 7 + 6); pflags = (unsigned short) getleu16(buf, 7 + 8); #endif /* __UNUSED__ */ #define MAX(a,b) (((a) > (b)) ? (a) : (b)) nsv = (unsigned short) MAX(getleu16(buf, 7 + 12), getleu16(buf, 7 + 14)); svlist = (unsigned int) ((unsigned short) getleu32(buf, 7 + 16) | getleu32(buf, 7 + 24)); hour = (unsigned short) getleu16(buf, 7 + 66); min = (unsigned short) getleu16(buf, 7 + 68); sec = (unsigned short) getleu16(buf, 7 + 70); //nsec = (unsigned short) getleu32(buf, 7 + 72); year = (unsigned short) getleu16(buf, 7 + 76); mon = (unsigned short) getleu16(buf, 7 + 78); day = (unsigned short) getleu16(buf, 7 + 80); gps_week = (unsigned short) getles16(buf, 7 + 82); tow = (unsigned short) getleu32(buf, 7 + 84); 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); latitude = (double)(getles32(buf, 7 + 144) / 1e7); longitude = (double)(getles32(buf, 7 + 148) / 1e7); altitude = (float)(getles32(buf, 7 + 152) / 1e3); climb = (float)(getles32(buf, 7 + 206) / 1e3); speed = (float)(getleu32(buf, 7 + 210) / 1e3); track = (float)(getleu16(buf, 7 + 214) / 1e2); hdop = (float)(getleu16(buf, 7 + 56) / 100.0); gdop = (float)(getleu16(buf, 7 + 58) / 100.0); pdop = (float)(getleu16(buf, 7 + 60) / 100.0); vdop = (float)(getleu16(buf, 7 + 62) / 100.0); tdop = (float)(getleu16(buf, 7 + 64) / 100.0); (void)wmove(navfixwin, 1, 11); (void)wprintw(navfixwin, "%12.2lf %12.2lf %12.2lfm", epx, epy, epz); (void)wmove(navfixwin, 2, 11); (void)wprintw(navfixwin, "%11.2lf %11.2lf %11.2lfm/s", evx, evy, evz); (void)wmove(navfixwin, 4, 11); (void)wprintw(navfixwin, "%11.8lf %13.8lf %8.1lfm", latitude, longitude, altitude); (void)mvwaddch(navfixwin, 4, 22, ACS_DEGREE); (void)mvwaddch(navfixwin, 4, 38, ACS_DEGREE); (void)wmove(navfixwin, 5, 11); (void)wprintw(navfixwin, "%6.2lfm/s %5.1lf %6.2lfm/s climb", speed, track, climb); (void)mvwaddch(navfixwin, 5, 27, ACS_DEGREE); (void)wmove(navfixwin, 7, 11); (void)wprintw(navfixwin, "%04u-%02u-%02u %02u:%02u:%02u", year, mon, day, hour, min, sec); (void)wmove(navfixwin, 8, 11); (void)wprintw(navfixwin, "%04u+%010.3lf", gps_week, tow / 1000.0); (void)wmove(navfixwin, 8, 33); d = (tow / 1000) / 86400; tod = (tow / 1000) - (d * 86400); sec = (unsigned short)tod % 60; min = (unsigned short)(tod / 60) % 60; hour = (unsigned short)tod / 3600; (void)wprintw(navfixwin, "%1d %02d:%02d:%02d", d, hour, min, sec); (void)wmove(navfixwin, 10, 9); (void)wprintw(navfixwin, "%-5.1f", hdop); (void)wmove(navfixwin, 10, 18); (void)wprintw(navfixwin, "%-5.1f", vdop); (void)wmove(navfixwin, 10, 27); (void)wprintw(navfixwin, "%-5.1f", pdop); (void)wmove(navfixwin, 10, 36); (void)wprintw(navfixwin, "%-5.1f", tdop); (void)wmove(navfixwin, 10, 45); (void)wprintw(navfixwin, "%-5.1f", gdop); (void)wmove(navfixwin, 11, 6); { char prn[4], satlist[38]; unsigned int i; satlist[0] = '\0'; for (i = 0; i < 32; i++) { if (svlist & (1 << i)) { (void)snprintf(prn, 4, "%u ", i + 1); (void)strlcat(satlist, prn, 38); } } (void)wprintw(navfixwin, "%02d = %-38s", nsv, satlist); } (void)wnoutrefresh(navfixwin); }
/*@ +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; } }
static void display_nav_sol(unsigned char *buf, size_t data_len) { unsigned short gw = 0; unsigned int tow = 0, flags; double epx, epy, epz, evx, evy, evz; unsigned char navmode; struct gps_data_t g; double separation; if (data_len != 52) return; navmode = (unsigned char)getub(buf, 10); flags = (unsigned int)getub(buf, 11); if ((flags & (UBX_SOL_VALID_WEEK | UBX_SOL_VALID_TIME)) != 0) { tow = (unsigned int)getleu32(buf, 0); gw = (unsigned short)getles16(buf, 8); } 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(&g.fix, &separation, epx, epy, epz, evx, evy, evz); g.fix.epx = g.fix.epy = (double)(getles32(buf, 24) / 100.0); g.fix.eps = (double)(getles32(buf, 40) / 100.0); g.dop.pdop = (double)(getleu16(buf, 44) / 100.0); g.satellites_used = (int)getub(buf, 47); (void)wmove(navsolwin, 1, 11); (void)wprintw(navsolwin, "%+10.2fm %+10.2fm %+10.2fm", epx, epy, epz); (void)wmove(navsolwin, 2, 11); (void)wprintw(navsolwin, "%+9.2fm/s %+9.2fm/s %+9.2fm/s", evx, evy, evz); (void)wmove(navsolwin, 4, 11); (void)wattrset(navsolwin, A_UNDERLINE); (void)wprintw(navsolwin, "%12.9f %13.9f %8.2fm", g.fix.latitude, g.fix.longitude, g.fix.altitude); (void)mvwaddch(navsolwin, 4, 23, ACS_DEGREE); (void)mvwaddch(navsolwin, 4, 38, ACS_DEGREE); (void)wmove(navsolwin, 5, 11); (void)wprintw(navsolwin, "%6.2fm/s %5.1fo %6.2fm/s", g.fix.speed, g.fix.track, g.fix.climb); (void)mvwaddch(navsolwin, 5, 26, ACS_DEGREE); (void)wattrset(navsolwin, A_NORMAL); (void)wmove(navsolwin, 7, 7); { unsigned int day = tow / 8640000; unsigned int tod = tow % 8640000; unsigned int h = tod / 360000; unsigned int m = tod % 360000; unsigned int s = m % 6000; m = (m - s) / 6000; (void)wattrset(navsolwin, A_UNDERLINE); (void)wprintw(navsolwin, "%u %02u:%02u:%05.2f", day, h, m, (double)s / 100); (void)wattrset(navsolwin, A_NORMAL); } (void)wmove(navsolwin, 8, 11); if ((flags & (UBX_SOL_VALID_WEEK | UBX_SOL_VALID_TIME)) != 0) { (void)wprintw(navsolwin, "%d+%10.3lf", gw, (double)(tow / 1000.0)); (void)wmove(navsolwin, 8, 36); (void)wprintw(navsolwin, "%d", (tow / 86400000)); } /* relies on the fact that epx and epy are set to same value */ (void)wmove(navsolwin, 10, 12); (void)wprintw(navsolwin, "%7.2f", g.fix.epx); (void)wmove(navsolwin, 10, 33); (void)wprintw(navsolwin, "%6.2f", g.fix.epv); (void)wmove(navsolwin, 11, 7); (void)wprintw(navsolwin, "%2d", g.satellites_used); (void)wmove(navsolwin, 11, 15); (void)wprintw(navsolwin, "%5.1f", g.dop.pdop); (void)wmove(navsolwin, 11, 25); (void)wprintw(navsolwin, "0x%02x", navmode); (void)wmove(navsolwin, 11, 36); (void)wprintw(navsolwin, "0x%02x", flags); (void)wnoutrefresh(navsolwin); }