Exemplo n.º 1
0
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));
}
Exemplo n.º 2
0
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;
}
Exemplo n.º 3
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;
}
Exemplo n.º 4
0
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;
}
Exemplo n.º 5
0
static void ubx_update(void)
{
    unsigned char *buf;
    size_t data_len;
    unsigned short msgid;

    buf = session.lexer.outbuffer;
    msgid = (unsigned short)((buf[2] << 8) | buf[3]);
    data_len = (size_t) getles16(buf, 4);
    switch (msgid) {
    case UBX_NAV_SVINFO:
	display_nav_svinfo(&buf[6], data_len);
	break;
    case UBX_NAV_DOP:
	display_nav_dop(&buf[6], data_len);
	break;
    case UBX_NAV_SOL:
	display_nav_sol(&buf[6], data_len);
	break;
    default:
	break;
    }

#ifdef NTP_ENABLE
    toff_update(ppswin, TOFF_LINE, TOFF_COLUMN + 6);
#endif /* NTP_ENABLE */

#ifdef PPS_ENABLE
    pps_update(ppswin, PPS_LINE, PPS_COLUMN + 5);
#endif /* PPS_ENABLE */
}
Exemplo n.º 6
0
static void ubx_update(void)
{
    unsigned char *buf;
    size_t data_len;
    unsigned short msgid;
#ifdef PPS_ENABLE
    struct timedrift_t drift;
#endif /* PPS_ENABLE */

    buf = session.packet.outbuffer;
    msgid = (unsigned short)((buf[2] << 8) | buf[3]);
    data_len = (size_t) getles16(buf, 4);
    switch (msgid) {
    case UBX_NAV_SVINFO:
	display_nav_svinfo(&buf[6], data_len);
	break;
    case UBX_NAV_DOP:
	display_nav_dop(&buf[6], data_len);
	break;
    case UBX_NAV_SOL:
	display_nav_sol(&buf[6], data_len);
	break;
    default:
	break;
    }

#ifdef PPS_ENABLE
    /*@-compdef@*/
    /*@-type@*/ /* splint is confused about struct timespec */
    if (pps_thread_lastpps(&session, &drift) > 0) {
	double timedelta = timespec_diff_ns(drift.real, drift.clock) * 1e-9;
	(void)mvwprintw(ppswin, 1, 13, "%.9f", timedelta);
	(void)wnoutrefresh(ppswin);
    }
    /*@+type@*/
    /*@+compdef@*/
#endif /* PPS_ENABLE */
}
Exemplo n.º 7
0
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;
}
Exemplo n.º 8
0
/*@ -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);
}
Exemplo n.º 9
0
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);

}
Exemplo n.º 10
0
/*@ +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;
    }
}
Exemplo n.º 11
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);
}