/*
* pretend to decode MID 0xE3, Beiduo D2 Subframe data
*
* from Beidou Standard BDS-SIS-ICD-2.0
* D2, with the data rate of 500 bps, is broadcasted by the GEO satellites.
*
* len 31 bytes
*
*/
static gps_mask_t sky_msg_E3(struct gps_device_t *session,
unsigned char *buf, size_t len)
{
int i;
unsigned int prn; /* BeidouPS sat PRN 201-205 */
unsigned int subf; /* subframe 1-5 */
/* the words are preprocessed, not raw, just the 28 bytes of data */
uint8_t bytes[28]; /* raw data */
if ( 31 != len)
return 0;
prn = (unsigned int)getub(buf, 1);
subf = (unsigned int)getub(buf, 2);
for ( i = 0; i < 28; i++ ) {
bytes[i] = getub(buf, 3 + i);
}
/* extra guard prevents expensive hexdump calls */
if (session->context->errout.debug >= LOG_PROG) {
gpsd_log(&session->context->errout, LOG_PROG,
"Skytraq: Beidou D2 subframe PRN %d Subframe %d "
"length %zd byte:%s\n",
prn, subf,
len,
gpsd_hexdump(session->msgbuf, sizeof(session->msgbuf),
(char *)bytes, 28));
}
return ONLINE_SET;
}
const char /*@ observer @*/ *gpsd_packetdump(char *binbuf, size_t binbuflen)
{
char *cp;
bool printable = true;
assert(binbuf != NULL);
for (cp = binbuf; cp < binbuf + binbuflen; cp++)
if (!isprint(*cp) && !isspace(*cp))
printable = false;
if (printable)
return binbuf;
else
return gpsd_hexdump(binbuf, binbuflen);
}
static gps_mask_t rtcm104v2_analyze(struct gps_device_t *session)
{
rtcm2_unpack(&session->gpsdata.rtcm2, (char *)session->packet.isgps.buf);
/* extra guard prevents expensive hexdump calls */
if (session->context->debug >= LOG_RAW)
gpsd_report(LOG_RAW, "RTCM 2.x packet type 0x%02x length %d words from %zd bytes: %s\n",
session->gpsdata.rtcm2.type,
session->gpsdata.rtcm2.length + 2,
session->packet.isgps.buflen,
gpsd_hexdump((char *)session->packet.isgps.buf,
(session->gpsdata.rtcm2.length +
2) * sizeof(isgps30bits_t)));
session->cycle_end_reliable = true;
return RTCM2_SET;
}
const char *gpsd_packetdump(char *scbuf, size_t scbuflen,
char *binbuf, size_t binbuflen)
{
char *cp;
bool printable = true;
assert(binbuf != NULL);
for (cp = binbuf; cp < binbuf + binbuflen; cp++)
if (!isprint((unsigned char) *cp) && !isspace((unsigned char) *cp))
printable = false;
if (printable)
return binbuf;
else
return gpsd_hexdump(scbuf, scbuflen, binbuf, binbuflen);
}
ssize_t gpsd_write(struct gps_device_t * session, const char *buf, size_t len)
{
ssize_t status;
bool ok;
if (session == NULL ||
session->context == NULL || session->context->readonly)
return 0;
status = write(session->gpsdata.gps_fd, buf, len);
ok = (status == (ssize_t) len);
(void)tcdrain(session->gpsdata.gps_fd);
/* extra guard prevents expensive hexdump calls */
if (session->context->debug >= LOG_IO)
gpsd_report(LOG_IO, "=> GPS: %s%s\n",
gpsd_hexdump((char *)buf, len), ok ? "" : " FAILED");
return status;
}
/*@ +charint -usedef -compdef @*/
static bool evermore_write(struct gps_device_t *session, unsigned char *msg, size_t msglen)
{
unsigned int crc;
size_t i, len;
unsigned char stuffed[MAX_PACKET_LENGTH*2], *cp;
/* prepare a DLE-stuffed copy of the message */
cp = stuffed;
*cp++ = 0x10; /* message starts with DLE STX */
*cp++ = 0x02;
len = (size_t)(msglen + 2); /* msglen < 254 !! */
*cp++ = (unsigned char)len; /* message length */
if (len == 0x10) *cp++ = 0x10;
/* payload */
crc = 0;
for (i = 0; i < msglen; i++) {
*cp++ = msg[i];
if (msg[i] == 0x10) *cp++ = 0x10;
crc += msg[i];
}
crc &= 0xff;
/* enter CRC after payload */
*cp++ = crc;
if (crc == 0x10) *cp++ = 0x10;
*cp++ = 0x10; /* message ends with DLE ETX */
*cp++ = 0x03;
len = (size_t)(cp - stuffed);
/* we may need to dump the message */
gpsd_report(LOG_IO, "writing EverMore control type 0x%02x: %s\n", msg[0],
gpsd_hexdump(stuffed, len));
#ifdef ALLOW_RECONFIGURE
return (gpsd_write(session, stuffed, len) == (ssize_t)len);
#else
return 0;
#endif /* ALLOW_RECONFIGURE */
}
/*@ +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 used, visible, satcnt;
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 %d: %s\n", *cp, len, gpsd_hexdump(buf, len));
datalen -= 2;
for (i = 0; i < (size_t)datalen; i++) {
*tp = *cp++;
if (*tp == 0x10) cp++;
tp++;
}
/*@ -usedef -compdef @*/
gpsd_report(LOG_RAW, "EverMore packet type 0x%02x, length %d: %s\n", buf2[0], datalen, gpsd_hexdump(buf2, datalen));
/*@ +usedef +compdef @*/
(void)snprintf(session->gpsdata.tag, sizeof(session->gpsdata.tag),
"EID%d",(int)buf2[0]);
switch (getub(buf2, 1))
{
case 0x02: /* Navigation Data Output */
session->gpsdata.fix.time = session->gpsdata.sentence_time
= gpstime_to_unix((int)getuw(buf2, 2), getul(buf2, 4)*0.01) - session->context->leap_seconds;
ecef_to_wgs84fix(&session->gpsdata,
getsl(buf2, 8)*1.0, getsl(buf2, 12)*1.0, getsl(buf2, 16)*1.0,
getsw(buf2, 20)/10.0, getsw(buf2, 22)/10.0, getsw(buf2, 24)/10.0);
used = getub(buf2, 26) & 0x0f;
visible = (getub(buf2, 26) & 0xf0) >> 4;
version = getuw(buf2, 27)/100.0;
/* that's all the information in this packet */
if (used < 3)
session->gpsdata.fix.mode = MODE_NO_FIX;
else if (used == 3)
session->gpsdata.fix.mode = MODE_2D;
else {
session->gpsdata.fix.mode = MODE_3D;
mask |= ALTITUDE_SET | CLIMB_SET;
}
gpsd_report(LOG_PROG, "NDO 0x02: version %3.2f, mode=%d, status=%d, visible=%d, used=%d\n",
version,
session->gpsdata.fix.mode,
session->gpsdata.status,
visible,
used);
mask |= TIME_SET | LATLON_SET | TRACK_SET | SPEED_SET | MODE_SET | CYCLE_START_SET;
if (session->subtype[0] == '\0') {
(void)snprintf(session->subtype, sizeof(session->subtype),
"%3.2f", version);
mask |= DEVICEID_SET;
}
return mask;
case 0x04: /* DOP Data Output */
session->gpsdata.fix.time = session->gpsdata.sentence_time
= gpstime_to_unix((int)getuw(buf2, 2), getul(buf2, 4)*0.01) - session->context->leap_seconds;
session->gpsdata.gdop = (double)getub(buf2, 8)*0.1;
session->gpsdata.pdop = (double)getub(buf2, 9)*0.1;
session->gpsdata.hdop = (double)getub(buf2, 10)*0.1;
session->gpsdata.vdop = (double)getub(buf2, 11)*0.1;
session->gpsdata.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->gpsdata.fix.mode = MODE_NO_FIX;
break;
case 2: /* 2D navigation */
session->gpsdata.status = STATUS_FIX;
session->gpsdata.fix.mode = MODE_2D;
break;
case 3: /* 3D navigation */
session->gpsdata.status = STATUS_FIX;
session->gpsdata.fix.mode = MODE_3D;
break;
case 4: /* 3D navigation with DGPS */
session->gpsdata.status = STATUS_DGPS_FIX;
session->gpsdata.fix.mode = MODE_3D;
break;
}
/* that's all the information in this packet */
gpsd_report(LOG_PROG, "DDO 0x04: mode=%d, status=%d\n",
session->gpsdata.fix.mode,
session->gpsdata.status);
return TIME_SET | DOP_SET | MODE_SET | STATUS_SET;
case 0x06: /* Channel Status Output */
session->gpsdata.fix.time = session->gpsdata.sentence_time
= gpstime_to_unix((int)getuw(buf2, 2), getul(buf2, 4)*0.01) - session->context->leap_seconds;
session->gpsdata.satellites = (int)getub(buf2, 8);
session->gpsdata.satellites_used = 0;
memset(session->gpsdata.used, 0, sizeof(session->gpsdata.used));
if (session->gpsdata.satellites > 12) {
gpsd_report(LOG_WARN, "Warning: EverMore packet has information about %d satellites!\n",
session->gpsdata.satellites);
}
if (session->gpsdata.satellites > EVERMORE_CHANNELS) session->gpsdata.satellites = EVERMORE_CHANNELS;
satcnt = 0;
for (i = 0; i < (size_t)session->gpsdata.satellites; 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)getuw(buf2, 7*i+7+4);
session->gpsdata.elevation[satcnt] = (int)getub(buf2, 7*i+7+6);
session->gpsdata.ss[satcnt] = (int)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 = (int)satcnt;
/* that's all the information in this packet */
gpsd_report(LOG_PROG, "CSO 0x04: %d satellites used\n",
session->gpsdata.satellites_used);
return TIME_SET | SATELLITE_SET | USED_SET;
case 0x08: /* Measurement Data Output */
/* clock offset is a manufacturer diagnostic */
/* (int)getuw(buf2, 8); clock offset, 29000..29850 ?? */
session->gpsdata.fix.time = session->gpsdata.sentence_time
= gpstime_to_unix((int)getuw(buf2, 2), getul(buf2, 4)*0.01) - session->context->leap_seconds;
visible = getub(buf2, 10);
/* FIXME: read full statellite status for each channel */
/* we can get pseudo range (m), delta-range (m/s), doppler (Hz) and status for each channel */
/* gpsd_report(LOG_PROG, "MDO 0x04: visible=%d\n", visible); */
gpsd_report(LOG_PROG, "MDO 0x04:\n");
return TIME_SET;
case 0x20: /* LogConfig Info, could be used as a probe for EverMore GPS */
gpsd_report(LOG_IO, "LogConfig EverMore packet, length %d: %s\n", datalen, gpsd_hexdump(buf2, datalen));
return ONLINE_SET;
case 0x22: /* LogData */
gpsd_report(LOG_IO, "LogData EverMore packet, length %d: %s\n", datalen, gpsd_hexdump(buf2, datalen));
return ONLINE_SET;
case 0x38: /* ACK */
gpsd_report(LOG_PROG, "EverMore command %02X ACK\n", getub(buf2, 2));
return ONLINE_SET;
default:
gpsd_report(LOG_WARN, "unknown EverMore packet id 0x%02x, length %d: %s\n", buf2[0], datalen, gpsd_hexdump(buf2, datalen));
return 0;
}
}
