static void bedumpall(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("getbes16: %016" PRIx64 " %016" PRIx64 " %016" PRIx64 " %016" PRIx64 "\n",
(uint64_t) sw1, (uint64_t) sw2,
(uint64_t) getbes16(buf, 0), (uint64_t) getbes16(buf, 8));
(void)printf("getbeu16: %016" PRIx64 " %016" PRIx64 " %016" PRIx64 " %016" PRIx64 "\n",
(uint64_t) uw1, (uint64_t) uw2,
(uint64_t) getbeu16(buf, 0), (uint64_t) getbeu16(buf, 8));
(void)printf("getbes32: %016" PRIx64 " %016" PRIx64 " %016" PRIx64 " %016" PRIx64 "\n",
(uint64_t) sl1, (uint64_t) sl2,
(uint64_t) getbes32(buf, 0), (uint64_t) getbes32(buf, 8));
(void)printf("getbeu32: %016" PRIx64 " %016" PRIx64 " %016" PRIx64 " %016" PRIx64 "\n",
(uint64_t) ul1, (uint64_t) ul2,
(uint64_t) getbeu32(buf, 0), (uint64_t) getbeu32(buf, 8));
(void)printf("getbes64: %016" PRIx64 " %016" PRIx64 " %016" PRIx64 " %016" PRIx64 "\n",
(uint64_t) sL1, (uint64_t) sL2,
(uint64_t) getbes64(buf, 0), (uint64_t) getbes64(buf, 8));
(void)printf("getbeu64: %016" PRIx64 " %016" PRIx64 " %016" PRIx64 " %016" PRIx64 "\n",
(uint64_t) uL1, (uint64_t) uL2,
(uint64_t) getbeu64(buf, 0), (uint64_t) getbeu64(buf, 8));
(void)printf("getbef32: %f %f\n", f1, getbef32((const char *)buf, 24));
(void)printf("getbed64: %.16f %.16f\n", d1, getbed64((const char *)buf, 16));
}
/*@ -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);
}
/*
* 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;
}
