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 handle1003(struct gps_device_t *session) /* skyview report */ { int i, n; /* The Polaris (and probably the DAGR) emit some strange variant of * this message which causes gpsd to crash filtering on impossible * number of satellites avoids this */ n = (int)getzword(14); if ((n < 0) || (n > 12)) return 0; gpsd_zero_satellites(&session->gpsdata); /* ticks = getzlong(6); */ /* sequence = getzword(8); */ session->gpsdata.dop.gdop = (unsigned int)getzword(9) * 1e-2; session->gpsdata.dop.pdop = (unsigned int)getzword(10) * 1e-2; session->gpsdata.dop.hdop = (unsigned int)getzword(11) * 1e-2; session->gpsdata.dop.vdop = (unsigned int)getzword(12) * 1e-2; session->gpsdata.dop.tdop = (unsigned int)getzword(13) * 1e-2; session->gpsdata.satellites_visible = n; for (i = 0; i < ZODIAC_CHANNELS; i++) { if (i < session->gpsdata.satellites_visible) { session->gpsdata.skyview[i].PRN = (short)getzword(15 + (3 * i)); session->gpsdata.skyview[i].azimuth = (short)(((short)getzword(16 + (3 * i))) * RAD_2_DEG * 1e-4); if (session->gpsdata.skyview[i].azimuth < 0) session->gpsdata.skyview[i].azimuth += 360; session->gpsdata.skyview[i].elevation = (short)(((short)getzword(17 + (3 * i))) * RAD_2_DEG * 1e-4); } else { session->gpsdata.skyview[i].PRN = 0; session->gpsdata.skyview[i].azimuth = 0; session->gpsdata.skyview[i].elevation = 0; } } session->gpsdata.skyview_time = NAN; gpsd_log(&session->context->errout, LOG_DATA, "NAVDOP: visible=%d gdop=%.2f pdop=%.2f " "hdop=%.2f vdop=%.2f tdop=%.2f mask={SATELLITE|DOP}\n", session->gpsdata.satellites_visible, session->gpsdata.dop.gdop, session->gpsdata.dop.hdop, session->gpsdata.dop.vdop, session->gpsdata.dop.pdop, session->gpsdata.dop.tdop); return SATELLITE_SET | DOP_SET; }
/*@ +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; if (len == 0) return 0; /* time to unstuff it and discard the header and footer */ cp = buf + 2; tp = buf2; if (*cp == 0x10) cp++; datalen = (size_t) * cp++; gpsd_report(LOG_RAW, "raw EverMore packet type 0x%02x, length %zd: %s\n", *cp, len, gpsd_hexdump_wrapper(buf, len, LOG_RAW)); datalen -= 2; /*@ -usedef @*/ for (i = 0; i < (size_t) datalen; i++) { *tp = *cp++; if (*tp == 0x10) cp++; tp++; } type = (unsigned char)getub(buf2, 1); /*@ +usedef @*/ /*@ -usedef -compdef @*/ gpsd_report(LOG_RAW, "EverMore packet type 0x%02x, length %zd: %s\n", type, datalen, gpsd_hexdump_wrapper(buf2, datalen, LOG_RAW)); /*@ +usedef +compdef @*/ (void)snprintf(session->gpsdata.tag, sizeof(session->gpsdata.tag), "EID%u", type); session->cycle_end_reliable = true; switch (type) { case 0x02: /* Navigation Data Output */ session->context->gps_week = (unsigned short)getleuw(buf2, 2); session->context->gps_tow = (double)getleul(buf2, 4) * 0.01; /*@ ignore @*//*@ splint is confused @ */ session->newdata.time = gpstime_to_unix(session->context->gps_week, session->context->gps_tow) - session->context->leap_seconds; /*@ end @*/ ecef_to_wgs84fix(&session->newdata, &session->gpsdata.separation, getlesl(buf2, 8) * 1.0, getlesl(buf2, 12) * 1.0, getlesl(buf2, 16) * 1.0, getlesw(buf2, 20) / 10.0, getlesw(buf2, 22) / 10.0, getlesw(buf2, 24) / 10.0); used = (unsigned char)getub(buf2, 26) & 0x0f; //visible = (getub(buf2, 26) & 0xf0) >> 4; version = (uint) getleuw(buf2, 27) / 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_IS | CLIMB_IS; } mask |= TIME_IS | LATLON_IS | TRACK_IS | SPEED_IS | MODE_IS; if (session->subtype[0] == '\0') { (void)snprintf(session->subtype, sizeof(session->subtype), "%3.2f", version); mask |= DEVICEID_IS; } gpsd_report(LOG_DATA, "NDO 0x02: time=%.2f, lat=%.2f lon=%.2f alt=%.2f speed=%.2f track=%.2f climb=%.2f mode=%d subtype='%s' mask=%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, gpsd_maskdump(mask)); return mask | CLEAR_IS | REPORT_IS; case 0x04: /* DOP Data Output */ session->context->gps_week = (unsigned short)getleuw(buf2, 2); session->context->gps_tow = (double)getleul(buf2, 4) * 0.01; /*@ ignore @*//*@ splint is confused @ */ session->newdata.time = gpstime_to_unix(session->context->gps_week, session->context->gps_tow) - session->context->leap_seconds; /*@ end @*/ /* * 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)getub(buf2, 8) * 0.1; session->gpsdata.dop.pdop = (double)getub(buf2, 9) * 0.1; session->gpsdata.dop.hdop = (double)getub(buf2, 10) * 0.1; session->gpsdata.dop.vdop = (double)getub(buf2, 11) * 0.1; session->gpsdata.dop.tdop = (double)getub(buf2, 12) * 0.1; switch (getub(buf2, 13)) { 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_IS | DOP_IS | MODE_IS | STATUS_IS; gpsd_report(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->context->gps_week = (unsigned short)getleuw(buf2, 2); session->context->gps_tow = (double)getleul(buf2, 4) * 0.01; /*@ ignore @*//*@ splint is confused @ */ session->gpsdata.skyview_time = gpstime_to_unix(session->context->gps_week, session->context->gps_tow) - session->context->leap_seconds; /*@ end @*/ session->gpsdata.satellites_visible = (int)getub(buf2, 8); gpsd_zero_satellites(&session->gpsdata); memset(session->gpsdata.used, 0, sizeof(session->gpsdata.used)); if (session->gpsdata.satellites_visible > 12) { gpsd_report(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+2) prn = (int)getub(buf2, 7 * i + 7 + 3); if (prn == 0) continue; /* satellite record is not valid */ session->gpsdata.PRN[satcnt] = prn; session->gpsdata.azimuth[satcnt] = (int)getleuw(buf2, 7 * i + 7 + 4); session->gpsdata.elevation[satcnt] = (int)getub(buf2, 7 * i + 7 + 6); session->gpsdata.ss[satcnt] = (float)getub(buf2, 7 * i + 7 + 7); /* * 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 + 8) & 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_IS | USED_IS; gpsd_report(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)getleuw(buf2, 8); clock offset, 29000..29850 ?? */ session->context->gps_week = (unsigned short)getleuw(buf2, 2); session->context->gps_tow = (double)getleul(buf2, 4) * 0.01; /*@ ignore @*//*@ splint is confused @ */ session->newdata.time = gpstime_to_unix(session->context->gps_week, session->context->gps_tow) - session->context->leap_seconds; /*@ end @*/ visible = (unsigned char)getub(buf2, 10); /* * 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((char *)buf, 10 + (sat*14) + s, l) #define UBITS(sat, s, l) ubits((char *)buf, 10 + (sat*14) + s, l) 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_report(LOG_DATA, "MDO 0x04: time=%.2f mask={TIME|RAW}\n", session->newdata.time); return TIME_IS | RAW_IS; case 0x20: /* LogConfig Info, could be used as a probe for EverMore GPS */ gpsd_report(LOG_IO, "LogConfig EverMore packet, length %zd: %s\n", datalen, gpsd_hexdump_wrapper(buf2, datalen, LOG_IO)); return ONLINE_IS; case 0x22: /* LogData */ gpsd_report(LOG_IO, "LogData EverMore packet, length %zd: %s\n", datalen, gpsd_hexdump_wrapper(buf2, datalen, LOG_IO)); return ONLINE_IS; case 0x38: /* ACK */ gpsd_report(LOG_PROG, "EverMore command %02X ACK\n", getub(buf2, 2)); return ONLINE_IS; default: gpsd_report(LOG_WARN, "unknown EverMore packet EID 0x%02x, length %zd: %s\n", buf2[0], datalen, gpsd_hexdump_wrapper(buf2, datalen, LOG_IO)); return 0; } }
/* * decode MID 0xDE, SV and channel status * * max payload: 3 + (Num_sats * 10) = 483 bytes */ static gps_mask_t sky_msg_DE(struct gps_device_t *session, unsigned char *buf, size_t len UNUSED) { int st, nsv; unsigned int i; unsigned int iod; /* Issue of data 0 - 255 */ unsigned int nsvs; /* number of SVs in this packet */ iod = (unsigned int)getub(buf, 1); nsvs = (unsigned int)getub(buf, 2); /* too many sats? */ if ( SKY_CHANNELS < nsvs ) return 0; gpsd_zero_satellites(&session->gpsdata); for (i = st = nsv = 0; i < nsvs; i++) { int off = 3 + (10 * i); /* offset into buffer of start of this sat */ bool good; /* do we have a good record ? */ unsigned short sv_stat; unsigned short chan_stat; unsigned short ura; session->gpsdata.skyview[st].PRN = (short)getub(buf, off + 1); sv_stat = (unsigned short)getub(buf, off + 2); ura = (unsigned short)getub(buf, off + 3); session->gpsdata.skyview[st].ss = (float)getub(buf, off + 4); session->gpsdata.skyview[st].elevation = (short)getbes16(buf, off + 5); session->gpsdata.skyview[st].azimuth = (short)getbes16(buf, off + 7); chan_stat = (unsigned short)getub(buf, off + 9); session->gpsdata.skyview[st].used = (bool)(chan_stat & 0x30); good = session->gpsdata.skyview[st].PRN != 0 && session->gpsdata.skyview[st].azimuth != 0 && session->gpsdata.skyview[st].elevation != 0; gpsd_log(&session->context->errout, LOG_DATA, "Skytraq: PRN=%2d El=%d Az=%d ss=%3.2f stat=%02x,%02x ura=%d %c\n", session->gpsdata.skyview[st].PRN, session->gpsdata.skyview[st].elevation, session->gpsdata.skyview[st].azimuth, session->gpsdata.skyview[st].ss, chan_stat, sv_stat, ura, good ? '*' : ' '); if ( good ) { st += 1; if (session->gpsdata.skyview[st].used) nsv++; } } session->gpsdata.satellites_visible = st; session->gpsdata.satellites_used = nsv; gpsd_log(&session->context->errout, LOG_DATA, "Skytraq: MID 0xDE: nsvs=%u visible=%u iod=%u\n", nsvs, session->gpsdata.satellites_visible, iod); return SATELLITE_SET | USED_IS; }
/* * Decode the navigation solution message */ static gps_mask_t oncore_msg_navsol(struct gps_device_t *session, unsigned char *buf, size_t data_len) { gps_mask_t mask; unsigned char flags; double lat, lon, alt; float speed, track, dop; unsigned int i, j, st, nsv; int Bbused; struct tm unpacked_date; if (data_len != 76) return 0; mask = ONLINE_SET; gpsd_log(&session->context->errout, LOG_DATA, "oncore NAVSOL - navigation data\n"); flags = (unsigned char)getub(buf, 72); /*@ -predboolothers @*/ if (flags & 0x20) { session->gpsdata.status = STATUS_FIX; session->newdata.mode = MODE_3D; } else if (flags & 0x10) { session->gpsdata.status = STATUS_FIX; session->newdata.mode = MODE_2D; } else { gpsd_log(&session->context->errout, LOG_WARN, "oncore NAVSOL no fix - flags 0x%02x\n", flags); session->newdata.mode = MODE_NO_FIX; session->gpsdata.status = STATUS_NO_FIX; } mask |= MODE_SET; /*@ +predboolothers @*/ /* Unless we have seen non-zero utc offset data, the time is GPS time * and not UTC time. Do not use it. */ if (session->context->leap_seconds) { unsigned int nsec; unpacked_date.tm_mon = (int)getub(buf, 4) - 1; unpacked_date.tm_mday = (int)getub(buf, 5); unpacked_date.tm_year = (int)getbeu16(buf, 6) - 1900; unpacked_date.tm_hour = (int)getub(buf, 8); unpacked_date.tm_min = (int)getub(buf, 9); unpacked_date.tm_sec = (int)getub(buf, 10); unpacked_date.tm_isdst = 0; #ifdef S_SPLINT_S unpacked_date.tm_wday = unpacked_date.tm_yday = 0; #endif /* S_SPLINT_S */ nsec = (uint) getbeu32(buf, 11); /*@ -unrecog */ session->newdata.time = (timestamp_t)mkgmtime(&unpacked_date) + nsec * 1e-9; /*@ +unrecog */ mask |= TIME_SET; gpsd_log(&session->context->errout, LOG_DATA, "oncore NAVSOL - time: %04d-%02d-%02d %02d:%02d:%02d.%09d\n", unpacked_date.tm_year + 1900, unpacked_date.tm_mon + 1, unpacked_date.tm_mday, unpacked_date.tm_hour, unpacked_date.tm_min, unpacked_date.tm_sec, nsec); } /*@-type@*/ lat = getbes32(buf, 15) / 3600000.0f; lon = getbes32(buf, 19) / 3600000.0f; alt = getbes32(buf, 23) / 100.0f; speed = getbeu16(buf, 31) / 100.0f; track = getbeu16(buf, 33) / 10.0f; dop = getbeu16(buf, 35) / 10.0f; /*@+type@*/ gpsd_log(&session->context->errout, LOG_DATA, "oncore NAVSOL - %lf %lf %.2lfm-%.2lfm | %.2fm/s %.1fdeg dop=%.1f\n", lat, lon, alt, wgs84_separation(lat, lon), speed, track, (float)dop); session->newdata.latitude = lat; session->newdata.longitude = lon; session->gpsdata.separation = wgs84_separation(session->newdata.latitude, session->newdata.longitude); session->newdata.altitude = alt - session->gpsdata.separation; session->newdata.speed = speed; session->newdata.track = track; mask |= LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET; gpsd_zero_satellites(&session->gpsdata); /* Merge the satellite information from the Bb message. */ Bbused = 0; nsv = 0; for (i = st = 0; i < 8; i++) { int sv, mode, sn, status; unsigned int off; off = 40 + 4 * i; sv = (int)getub(buf, off); mode = (int)getub(buf, off + 1); sn = (int)getub(buf, off + 2); status = (int)getub(buf, off + 3); gpsd_log(&session->context->errout, LOG_DATA, "%2d %2d %2d %3d %02x\n", i, sv, mode, sn, status); if (sn) { session->gpsdata.skyview[st].PRN = (short)sv; session->gpsdata.skyview[st].ss = (double)sn; for (j = 0; (int)j < session->driver.oncore.visible; j++) if (session->driver.oncore.PRN[j] == sv) { session->gpsdata.skyview[st].elevation = (short)session->driver.oncore.elevation[j]; session->gpsdata.skyview[st].azimuth = (short)session->driver.oncore.azimuth[j]; Bbused |= 1 << j; break; } /* bit 7 of the status word: sat used for position */ session->gpsdata.skyview[st].used = false; if (status & 0x80) { session->gpsdata.skyview[st].used = true; nsv++; } /* bit 2 of the status word: using for time solution */ if (status & 0x02) mask |= PPSTIME_IS; /* * The PPSTIME_IS mask bit exists distinctly from TIME_SET exactly * so an OnCore running in time-service mode (and other GPS clocks) * can signal that it's returning time even though no position fixes * have been available. */ st++; } } for (j = 0; (int)j < session->driver.oncore.visible; j++) /*@ -boolops @*/ if (!(Bbused & (1 << j))) { session->gpsdata.skyview[st].PRN = (short)session->driver.oncore.PRN[j]; session->gpsdata.skyview[st].elevation = (short)session->driver.oncore.elevation[j]; session->gpsdata.skyview[st].azimuth = (short)session->driver.oncore.azimuth[j]; st++; } /*@ +boolops @*/ session->gpsdata.skyview_time = session->newdata.time; session->gpsdata.satellites_used = (int)nsv; session->gpsdata.satellites_visible = (int)st; mask |= SATELLITE_SET | USED_IS; /* Some messages can only be polled. As they are not so * important, would be enough to poll e.g. one message per cycle. */ (void)oncore_control_send(session, (char *)pollAs, sizeof(pollAs)); (void)oncore_control_send(session, (char *)pollAt, sizeof(pollAt)); (void)oncore_control_send(session, (char *)pollAy, sizeof(pollAy)); (void)oncore_control_send(session, (char *)pollBo, sizeof(pollBo)); (void)oncore_control_send(session, (char *)pollEn, sizeof(pollEn)); gpsd_log(&session->context->errout, LOG_DATA, "NAVSOL: time=%.2f lat=%.2f lon=%.2f alt=%.2f speed=%.2f track=%.2f mode=%d status=%d visible=%d used=%d\n", session->newdata.time, session->newdata.latitude, session->newdata.longitude, session->newdata.altitude, session->newdata.speed, session->newdata.track, session->newdata.mode, session->gpsdata.status, session->gpsdata.satellites_used, session->gpsdata.satellites_visible); return mask; }