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; }