Devuan fork of gpsd
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 
 
 

499 lines
17 KiB

  1. /*
  2. * Handle the Rockwell binary packet format supported by the old Zodiac chipset
  3. *
  4. * Week counters are not limited to 10 bits. It's unknown what
  5. * the firmware is doing to disambiguate them, if anything; it might just
  6. * be adding a fixed offset based on a hidden epoch value, in which case
  7. * unhappy things will occur on the next rollover.
  8. *
  9. * This file is Copyright (c) 2010-2018 by the GPSD project
  10. * SPDX-License-Identifier: BSD-2-clause
  11. */
  12. #include "gpsd_config.h" /* must be before all includes */
  13. #include <stdio.h>
  14. #include <stdbool.h>
  15. #include <string.h>
  16. #include <math.h>
  17. #include <unistd.h>
  18. #include "gpsd.h"
  19. #include "bits.h"
  20. #include "strfuncs.h"
  21. /* Zodiac protocol description uses 1-origin indexing by little-endian word */
  22. #define get16z(buf, n) ( (buf[2*(n)-2]) \
  23. | (buf[2*(n)-1] << 8))
  24. #define get32z(buf, n) ( (buf[2*(n)-2]) \
  25. | (buf[2*(n)-1] << 8) \
  26. | (buf[2*(n)+0] << 16) \
  27. | (buf[2*(n)+1] << 24))
  28. #define getstringz(to, from, s, e) \
  29. (void)memcpy(to, from+2*(s)-2, 2*((e)-(s)+1))
  30. #ifdef ZODIAC_ENABLE
  31. struct header
  32. {
  33. unsigned short sync;
  34. unsigned short id;
  35. unsigned short ndata;
  36. unsigned short flags;
  37. unsigned short csum;
  38. };
  39. static unsigned short zodiac_checksum(unsigned short *w, int n)
  40. {
  41. unsigned short csum = 0;
  42. while (n-- > 0)
  43. csum += *(w++);
  44. return -csum;
  45. }
  46. static ssize_t end_write(int fd, void *d, size_t len)
  47. /* write an array of shorts in little-endian format */
  48. {
  49. unsigned char buf[BUFSIZ];
  50. short *data = (short *)d;
  51. size_t n;
  52. for (n = 0; n < (size_t)(len/2); n++)
  53. putle16(buf, n*2, data[n]);
  54. return write(fd, (char*)buf, len);
  55. }
  56. /* zodiac_spew - Takes a message type, an array of data words, and a length
  57. * for the array, and prepends a 5 word header (including checksum).
  58. * The data words are expected to be checksummed.
  59. */
  60. static ssize_t zodiac_spew(struct gps_device_t *session, unsigned short type,
  61. unsigned short *dat, int dlen)
  62. {
  63. struct header h;
  64. int i;
  65. char buf[BUFSIZ];
  66. h.sync = 0x81ff;
  67. h.id = (unsigned short)type;
  68. h.ndata = (unsigned short)(dlen - 1);
  69. h.flags = 0;
  70. h.csum = zodiac_checksum((unsigned short *)&h, 4);
  71. if (!BAD_SOCKET(session->gpsdata.gps_fd)) {
  72. size_t hlen, datlen;
  73. hlen = sizeof(h);
  74. datlen = sizeof(unsigned short) * dlen;
  75. if (end_write(session->gpsdata.gps_fd, &h, hlen) != (ssize_t) hlen ||
  76. end_write(session->gpsdata.gps_fd, dat,
  77. datlen) != (ssize_t) datlen) {
  78. GPSD_LOG(LOG_INFO, &session->context->errout,
  79. "Reconfigure write failed\n");
  80. return -1;
  81. }
  82. }
  83. (void)snprintf(buf, sizeof(buf),
  84. "%04x %04x %04x %04x %04x",
  85. h.sync, h.id, h.ndata, h.flags, h.csum);
  86. for (i = 0; i < dlen; i++)
  87. str_appendf(buf, sizeof(buf), " %04x", dat[i]);
  88. GPSD_LOG(LOG_RAW, &session->context->errout,
  89. "Sent Zodiac packet: %s\n", buf);
  90. return 0;
  91. }
  92. static void send_rtcm(struct gps_device_t *session,
  93. const char *rtcmbuf, size_t rtcmbytes)
  94. {
  95. unsigned short data[34];
  96. int n = 1 + (int)(rtcmbytes / 2 + rtcmbytes % 2);
  97. if (session->driver.zodiac.sn++ > 32767)
  98. session->driver.zodiac.sn = 0;
  99. memset(data, 0, sizeof(data));
  100. data[0] = session->driver.zodiac.sn; /* sequence number */
  101. memcpy(&data[1], rtcmbuf, rtcmbytes);
  102. data[n] = zodiac_checksum(data, n);
  103. (void)zodiac_spew(session, 1351, data, n + 1);
  104. }
  105. static ssize_t zodiac_send_rtcm(struct gps_device_t *session,
  106. const char *rtcmbuf, size_t rtcmbytes)
  107. {
  108. while (rtcmbytes > 0) {
  109. size_t len = (size_t) (rtcmbytes > 64 ? 64 : rtcmbytes);
  110. send_rtcm(session, rtcmbuf, len);
  111. rtcmbytes -= len;
  112. rtcmbuf += len;
  113. }
  114. return 1;
  115. }
  116. #define getzword(n) get16z(session->lexer.outbuffer, n)
  117. #define getzlong(n) get32z(session->lexer.outbuffer, n)
  118. static gps_mask_t handle1000(struct gps_device_t *session)
  119. /* time-position-velocity report */
  120. {
  121. gps_mask_t mask;
  122. struct tm unpacked_date;
  123. int datum;
  124. char ts_buf[TIMESPEC_LEN];
  125. /* ticks = getzlong(6); */
  126. /* sequence = getzword(8); */
  127. /* measurement_sequence = getzword(9); */
  128. session->gpsdata.status = (getzword(10) & 0x1c) ? 0 : 1;
  129. if (session->gpsdata.status != 0)
  130. session->newdata.mode = (getzword(10) & 1) ? MODE_2D : MODE_3D;
  131. else
  132. session->newdata.mode = MODE_NO_FIX;
  133. /* solution_type = getzword(11); */
  134. session->gpsdata.satellites_used = (int)getzword(12);
  135. /* polar_navigation = getzword(13); */
  136. session->context->gps_week = (unsigned short)getzword(14);
  137. /* gps_seconds = getzlong(15); */
  138. /* gps_nanoseconds = getzlong(17); */
  139. unpacked_date.tm_mday = (int)getzword(19);
  140. unpacked_date.tm_mon = (int)getzword(20) - 1;
  141. unpacked_date.tm_year = (int)getzword(21) - 1900;
  142. unpacked_date.tm_hour = (int)getzword(22);
  143. unpacked_date.tm_min = (int)getzword(23);
  144. unpacked_date.tm_sec = (int)getzword(24);
  145. unpacked_date.tm_isdst = 0;
  146. session->newdata.time.tv_sec = mkgmtime(&unpacked_date);
  147. session->newdata.time.tv_nsec = getzlong(25);
  148. session->newdata.latitude = ((long)getzlong(27)) * RAD_2_DEG * 1e-8;
  149. session->newdata.longitude = ((long)getzlong(29)) * RAD_2_DEG * 1e-8;
  150. /*
  151. * The Rockwell Jupiter TU30-D140 reports altitude as uncorrected height
  152. * above WGS84 geoid. The Zodiac binary protocol manual does not
  153. * specify whether word 31 is geodetic or WGS 84.
  154. * Here we assume altitude is always wgs84.
  155. */
  156. session->newdata.altHAE = ((long)getzlong(31)) * 1e-2;
  157. session->newdata.geoid_sep = ((short)getzword(33)) * 1e-2;
  158. session->newdata.speed = (int)getzlong(34) * 1e-2;
  159. session->newdata.track = (int)getzword(36) * RAD_2_DEG * 1e-3;
  160. session->newdata.magnetic_var = ((short)getzword(37)) * RAD_2_DEG * 1e-4;
  161. session->newdata.climb = ((short)getzword(38)) * 1e-2;
  162. datum = getzword(39);
  163. datum_code_string(datum, session->newdata.datum,
  164. sizeof(session->newdata.datum));
  165. /*
  166. * The manual says these are 1-sigma. Device reports only eph, circular
  167. * error. Let gpsd_model_error() do the rest
  168. */
  169. session->newdata.eph = (int)getzlong(40) * 1e-2 * GPSD_CONFIDENCE;
  170. session->newdata.epv = (int)getzlong(42) * 1e-2 * GPSD_CONFIDENCE;
  171. session->newdata.ept = (int)getzlong(44) * 1e-2 * GPSD_CONFIDENCE;
  172. session->newdata.eps = (int)getzword(46) * 1e-2 * GPSD_CONFIDENCE;
  173. /* clock_bias = (int)getzlong(47) * 1e-2; */
  174. /* clock_bias_sd = (int)getzlong(49) * 1e-2; */
  175. /* clock_drift = (int)getzlong(51) * 1e-2; */
  176. /* clock_drift_sd = (int)getzlong(53) * 1e-2; */
  177. mask = TIME_SET | NTPTIME_IS | LATLON_SET | ALTITUDE_SET | CLIMB_SET |
  178. SPEED_SET | TRACK_SET | STATUS_SET | MODE_SET |
  179. HERR_SET | SPEEDERR_SET | VERR_SET;
  180. GPSD_LOG(LOG_DATA, &session->context->errout,
  181. "1000: time=%s lat=%.2f lon=%.2f altHAE=%.2f track=%.2f "
  182. "speed=%.2f climb=%.2f mode=%d status=%d\n",
  183. timespec_str(&session->newdata.time, ts_buf, sizeof(ts_buf)),
  184. session->newdata.latitude,
  185. session->newdata.longitude, session->newdata.altHAE,
  186. session->newdata.track, session->newdata.speed,
  187. session->newdata.climb, session->newdata.mode,
  188. session->gpsdata.status);
  189. return mask;
  190. }
  191. /* Message 1002: Channel Summary Message */
  192. static gps_mask_t handle1002(struct gps_device_t *session)
  193. {
  194. int i;
  195. timespec_t ts_tow;
  196. /* ticks = getzlong(6); */
  197. /* sequence = getzword(8); */
  198. /* measurement_sequence = getzword(9); */
  199. int gps_week = getzword(10);
  200. time_t gps_seconds = getzlong(11);
  201. long gps_nanoseconds = getzlong(13);
  202. char ts_buf[TIMESPEC_LEN];
  203. /* Note: this week counter is not limited to 10 bits. */
  204. session->context->gps_week = (unsigned short)gps_week;
  205. session->gpsdata.satellites_used = 0;
  206. for (i = 0; i < ZODIAC_CHANNELS; i++) {
  207. int status, prn;
  208. session->driver.zodiac.Zv[i] = status = (int)getzword(15 + (3 * i));
  209. session->driver.zodiac.Zs[i] = prn = (int)getzword(16 + (3 * i));
  210. if (status & 1)
  211. session->gpsdata.satellites_used++;
  212. session->gpsdata.skyview[i].PRN = (short)prn;
  213. session->gpsdata.skyview[i].ss = (float)getzword(17 + (3 * i));
  214. session->gpsdata.skyview[i].used = (bool)(status & 1);
  215. }
  216. ts_tow.tv_sec = gps_seconds;
  217. ts_tow.tv_nsec = gps_nanoseconds;
  218. session->gpsdata.skyview_time = gpsd_gpstime_resolv(session,
  219. (unsigned short)gps_week,
  220. ts_tow);
  221. GPSD_LOG(LOG_DATA, &session->context->errout,
  222. "1002: visible=%d used=%d mask={SATELLITE|USED} time %s\n",
  223. session->gpsdata.satellites_visible,
  224. session->gpsdata.satellites_used,
  225. timespec_str(&session->gpsdata.skyview_time, ts_buf,
  226. sizeof(ts_buf)));
  227. return SATELLITE_SET | USED_IS;
  228. }
  229. static gps_mask_t handle1003(struct gps_device_t *session)
  230. /* skyview report */
  231. {
  232. int i, n;
  233. /* The Polaris (and probably the DAGR) emit some strange variant of
  234. * this message which causes gpsd to crash filtering on impossible
  235. * number of satellites avoids this */
  236. n = (int)getzword(14);
  237. if ((n < 0) || (n > 12))
  238. return 0;
  239. gpsd_zero_satellites(&session->gpsdata);
  240. /* ticks = getzlong(6); */
  241. /* sequence = getzword(8); */
  242. session->gpsdata.dop.gdop = (unsigned int)getzword(9) * 1e-2;
  243. session->gpsdata.dop.pdop = (unsigned int)getzword(10) * 1e-2;
  244. session->gpsdata.dop.hdop = (unsigned int)getzword(11) * 1e-2;
  245. session->gpsdata.dop.vdop = (unsigned int)getzword(12) * 1e-2;
  246. session->gpsdata.dop.tdop = (unsigned int)getzword(13) * 1e-2;
  247. session->gpsdata.satellites_visible = n;
  248. for (i = 0; i < ZODIAC_CHANNELS; i++) {
  249. if (i < session->gpsdata.satellites_visible) {
  250. session->gpsdata.skyview[i].PRN = (short)getzword(15 + (3 * i));
  251. session->gpsdata.skyview[i].azimuth =
  252. (((double)getzword(16 + (3 * i))) * RAD_2_DEG * 1e-4);
  253. if (session->gpsdata.skyview[i].azimuth < 0)
  254. session->gpsdata.skyview[i].azimuth += 360;
  255. session->gpsdata.skyview[i].elevation =
  256. (((double)getzword(17 + (3 * i))) * RAD_2_DEG * 1e-4);
  257. } else {
  258. session->gpsdata.skyview[i].PRN = 0;
  259. session->gpsdata.skyview[i].azimuth = NAN;
  260. session->gpsdata.skyview[i].elevation = NAN;
  261. session->gpsdata.skyview[i].ss = NAN;
  262. }
  263. }
  264. session->gpsdata.skyview_time.tv_sec = 0;
  265. session->gpsdata.skyview_time.tv_nsec = 0;
  266. GPSD_LOG(LOG_DATA, &session->context->errout,
  267. "NAVDOP: visible=%d gdop=%.2f pdop=%.2f "
  268. "hdop=%.2f vdop=%.2f tdop=%.2f mask={SATELLITE|DOP}\n",
  269. session->gpsdata.satellites_visible,
  270. session->gpsdata.dop.gdop,
  271. session->gpsdata.dop.hdop,
  272. session->gpsdata.dop.vdop,
  273. session->gpsdata.dop.pdop, session->gpsdata.dop.tdop);
  274. return SATELLITE_SET | DOP_SET;
  275. }
  276. static void handle1005(struct gps_device_t *session UNUSED)
  277. /* fix quality report */
  278. {
  279. /* ticks = getzlong(6); */
  280. /* sequence = getzword(8); */
  281. int numcorrections = (int)getzword(12);
  282. if (session->newdata.mode == MODE_NO_FIX)
  283. session->gpsdata.status = STATUS_NO_FIX;
  284. else if (numcorrections == 0)
  285. session->gpsdata.status = STATUS_FIX;
  286. else
  287. session->gpsdata.status = STATUS_DGPS_FIX;
  288. }
  289. static gps_mask_t handle1011(struct gps_device_t *session)
  290. /* version report */
  291. {
  292. /*
  293. * This is UNTESTED -- but harmless if buggy. Added to support
  294. * client querying of the ID with firmware version in 2006.
  295. * The Zodiac is supposed to send one of these messages on startup.
  296. */
  297. getstringz(session->subtype, session->lexer.outbuffer, 19, 28); /* software version field */
  298. GPSD_LOG(LOG_DATA, &session->context->errout,
  299. "1011: subtype=%s mask={DEVICEID}\n",
  300. session->subtype);
  301. return DEVICEID_SET;
  302. }
  303. static void handle1108(struct gps_device_t *session)
  304. /* leap-second correction report */
  305. {
  306. /* ticks = getzlong(6); */
  307. /* sequence = getzword(8); */
  308. /* utc_week_seconds = getzlong(14); */
  309. /* leap_nanoseconds = getzlong(17); */
  310. if ((int)(getzword(19) & 3) == 3) {
  311. session->context->valid |= LEAP_SECOND_VALID;
  312. session->context->leap_seconds = (int)getzword(16);
  313. }
  314. }
  315. static gps_mask_t zodiac_analyze(struct gps_device_t *session)
  316. {
  317. unsigned int id =
  318. (unsigned int)((session->lexer.outbuffer[3] << 8) |
  319. session->lexer.outbuffer[2]);
  320. /*
  321. * The guard looks superfluous, but it keeps the rather expensive
  322. * gpsd_packetdump() function from being called even when the debug
  323. * level does not actually require it.
  324. */
  325. if (session->context->errout.debug >= LOG_RAW)
  326. GPSD_LOG(LOG_RAW, &session->context->errout,
  327. "Raw Zodiac packet type %d length %zd: %s\n",
  328. id, session->lexer.outbuflen, gpsd_prettydump(session));
  329. if (session->lexer.outbuflen < 10)
  330. return 0;
  331. /*
  332. * Normal cycle for these devices is 1001 1002.
  333. * We count 1001 as end of cycle because 1002 doesn't
  334. * carry fix information.
  335. */
  336. session->cycle_end_reliable = true;
  337. switch (id) {
  338. case 1000:
  339. return handle1000(session) | (CLEAR_IS | REPORT_IS);
  340. case 1002:
  341. return handle1002(session);
  342. case 1003:
  343. return handle1003(session);
  344. case 1005:
  345. handle1005(session);
  346. return 0;
  347. case 1011:
  348. return handle1011(session);
  349. case 1108:
  350. handle1108(session);
  351. return 0;
  352. default:
  353. return 0;
  354. }
  355. }
  356. #ifdef CONTROLSEND_ENABLE
  357. static ssize_t zodiac_control_send(struct gps_device_t *session,
  358. char *msg, size_t len)
  359. {
  360. unsigned short shortwords[256];
  361. #define min(x,y) ((x) < (y) ? x : y)
  362. /*
  363. * We used to just cast msg to an unsigned short pointer.
  364. * This can fail on word-oriented achitectures like a SPARC.
  365. */
  366. memcpy((char *)shortwords, msg, min(sizeof(shortwords), len));
  367. /* and if len isn't even, it's your own fault */
  368. return zodiac_spew(session, shortwords[0], shortwords + 1,
  369. (int)(len / 2 - 1));
  370. }
  371. #endif /* CONTROLSEND_ENABLE */
  372. #ifdef RECONFIGURE_ENABLE
  373. static bool zodiac_speed_switch(struct gps_device_t *session,
  374. speed_t speed, char parity, int stopbits)
  375. {
  376. unsigned short data[15];
  377. if (session->driver.zodiac.sn++ > 32767)
  378. session->driver.zodiac.sn = 0;
  379. switch (parity) {
  380. case 'E':
  381. case 2:
  382. parity = (char)2;
  383. break;
  384. case 'O':
  385. case 1:
  386. parity = (char)1;
  387. break;
  388. case 'N':
  389. case 0:
  390. default:
  391. parity = (char)0;
  392. break;
  393. }
  394. memset(data, 0, sizeof(data));
  395. /* data is the part of the message starting at word 6 */
  396. data[0] = session->driver.zodiac.sn; /* sequence number */
  397. data[1] = 1; /* port 1 data valid */
  398. data[2] = (unsigned short)parity; /* port 1 character width (8 bits) */
  399. data[3] = (unsigned short)(stopbits - 1); /* port 1 stop bits (1 stopbit) */
  400. data[4] = 0; /* port 1 parity (none) */
  401. data[5] = (unsigned short)(round(log((double)speed / 300) / GPS_LN2) + 1); /* port 1 speed */
  402. data[14] = zodiac_checksum(data, 14);
  403. (void)zodiac_spew(session, 1330, data, 15);
  404. return true; /* it would be nice to error-check this */
  405. }
  406. #endif /* RECONFIGURE_ENABLE */
  407. static double zodiac_time_offset(struct gps_device_t *session UNUSED)
  408. {
  409. /* Removing/changing the magic number below is likely to disturb
  410. * the handling of the 1pps signal from the gps device. The regression
  411. * tests and simple gps applications do not detect this. A live test
  412. * with the 1pps signal active is required. */
  413. return 1.1;
  414. }
  415. /* this is everything we export */
  416. /* *INDENT-OFF* */
  417. const struct gps_type_t driver_zodiac =
  418. {
  419. .type_name = "Zodiac", /* full name of type */
  420. .packet_type = ZODIAC_PACKET, /* associated lexer packet type */
  421. .flags = DRIVER_STICKY, /* no flags set */
  422. .trigger = NULL, /* no trigger */
  423. .channels = 12, /* consumer-grade GPS */
  424. .probe_detect = NULL, /* no probe */
  425. .get_packet = generic_get, /* use the generic packet getter */
  426. .parse_packet = zodiac_analyze, /* parse message packets */
  427. .rtcm_writer = zodiac_send_rtcm, /* send DGPS correction */
  428. .init_query = NULL, /* non-perturbing initial query */
  429. .event_hook = NULL, /* no configuration */
  430. #ifdef RECONFIGURE_ENABLE
  431. .speed_switcher = zodiac_speed_switch,/* we can change baud rate */
  432. .mode_switcher = NULL, /* no mode switcher */
  433. .rate_switcher = NULL, /* no sample-rate switcher */
  434. .min_cycle.tv_sec = 1, /* not relevant, no rate switch */
  435. .min_cycle.tv_nsec = 0, /* not relevant, no rate switch */
  436. #endif /* RECONFIGURE_ENABLE */
  437. #ifdef CONTROLSEND_ENABLE
  438. .control_send = zodiac_control_send, /* for gpsctl and friends */
  439. #endif /* CONTROLSEND_ENABLE */
  440. .time_offset = zodiac_time_offset, /* compute NTO fudge factor */
  441. };
  442. /* *INDENT-ON* */
  443. #endif /* ZODIAC_ENABLE */