void zodiac_send(int type, unsigned short *dat, int dlen)
{
struct zodiac_header h;
h.flags = 0;
h.sync = 0x81ff;
h.id = (unsigned short) type;
h.ndata = dlen - 1; /* data checksum word doesn't count */
h.csum = zodiac_checksum((unsigned short *) &h, 4);
/* Add data checksum */
dat[dlen - 1] = zodiac_checksum(dat, dlen - 1);
serial_send((char *)&h, sizeof(h));
serial_send((char *)dat, (sizeof(unsigned short) * dlen));
}
static void send_rtcm(struct gps_device_t *session,
const char *rtcmbuf, size_t rtcmbytes)
{
unsigned short data[34];
int n = 1 + (int)(rtcmbytes / 2 + rtcmbytes % 2);
if (session->driver.zodiac.sn++ > 32767)
session->driver.zodiac.sn = 0;
memset(data, 0, sizeof(data));
data[0] = session->driver.zodiac.sn; /* sequence number */
memcpy(&data[1], rtcmbuf, rtcmbytes);
data[n] = zodiac_checksum(data, n);
(void)zodiac_spew(session, 1351, data, n + 1);
}
/* zodiac_spew - Takes a message type, an array of data words, and a length
* for the array, and prepends a 5 word header (including checksum).
* The data words are expected to be checksummed.
*/
static ssize_t zodiac_spew(struct gps_device_t *session, unsigned short type,
unsigned short *dat, int dlen)
{
struct header h;
int i;
char buf[BUFSIZ];
h.sync = 0x81ff;
h.id = (unsigned short)type;
h.ndata = (unsigned short)(dlen - 1);
h.flags = 0;
h.csum = zodiac_checksum((unsigned short *)&h, 4);
if (!BAD_SOCKET(session->gpsdata.gps_fd)) {
size_t hlen, datlen;
hlen = sizeof(h);
datlen = sizeof(unsigned short) * dlen;
if (end_write(session->gpsdata.gps_fd, &h, hlen) != (ssize_t) hlen ||
end_write(session->gpsdata.gps_fd, dat,
datlen) != (ssize_t) datlen) {
gpsd_log(&session->context->errout, LOG_RAW,
"Reconfigure write failed\n");
return -1;
}
}
(void)snprintf(buf, sizeof(buf),
"%04x %04x %04x %04x %04x",
h.sync, h.id, h.ndata, h.flags, h.csum);
for (i = 0; i < dlen; i++)
str_appendf(buf, sizeof(buf), " %04x", dat[i]);
gpsd_log(&session->context->errout, LOG_RAW,
"Sent Zodiac packet: %s\n", buf);
return 0;
}
