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.

driver_italk.c 17 KiB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471
  1. /*
  2. * Driver for the iTalk binary protocol used by FasTrax
  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. */
  13. #include "gpsd_config.h" /* must be before all includes */
  14. #include <stdio.h>
  15. #include <stdbool.h>
  16. #include <string.h>
  17. #include <math.h>
  18. #include <unistd.h>
  19. #include "gpsd.h"
  20. #if defined(ITRAX_ENABLE) && defined(BINARY_ENABLE)
  21. #include "bits.h"
  22. #include "driver_italk.h"
  23. #include "timespec.h"
  24. static gps_mask_t italk_parse(struct gps_device_t *, unsigned char *, size_t);
  25. static gps_mask_t decode_itk_navfix(struct gps_device_t *, unsigned char *,
  26. size_t);
  27. static gps_mask_t decode_itk_prnstatus(struct gps_device_t *, unsigned char *,
  28. size_t);
  29. static gps_mask_t decode_itk_utcionomodel(struct gps_device_t *,
  30. unsigned char *, size_t);
  31. static gps_mask_t decode_itk_subframe(struct gps_device_t *, unsigned char *,
  32. size_t);
  33. /* NAVIGATION_MSG, message id 7 */
  34. static gps_mask_t decode_itk_navfix(struct gps_device_t *session,
  35. unsigned char *buf, size_t len)
  36. {
  37. unsigned short flags, pflags;
  38. timespec_t ts_tow;
  39. uint32_t tow; /* Time of week [ms] */
  40. char ts_buf[TIMESPEC_LEN];
  41. gps_mask_t mask = 0;
  42. if (len != 296) {
  43. GPSD_LOG(LOG_PROG, &session->context->errout,
  44. "ITALK: bad NAV_FIX (len %zu, should be 296)\n",
  45. len);
  46. return -1;
  47. }
  48. flags = (unsigned short) getleu16(buf, 7 + 4);
  49. //cflags = (unsigned short) getleu16(buf, 7 + 6);
  50. pflags = (unsigned short) getleu16(buf, 7 + 8);
  51. session->gpsdata.status = STATUS_NO_FIX;
  52. session->newdata.mode = MODE_NO_FIX;
  53. mask = ONLINE_SET | MODE_SET | STATUS_SET | CLEAR_IS;
  54. /* just bail out if this fix is not marked valid */
  55. if (0 != (pflags & FIX_FLAG_MASK_INVALID)
  56. || 0 == (flags & FIXINFO_FLAG_VALID))
  57. return mask;
  58. tow = getleu32(buf, 7 + 84); /* tow in ms */
  59. MSTOTS(&ts_tow, tow);
  60. session->newdata.time = gpsd_gpstime_resolv(session,
  61. (unsigned short) getles16(buf, 7 + 82), ts_tow);
  62. mask |= TIME_SET | NTPTIME_IS;
  63. session->newdata.ecef.x = (double)(getles32(buf, 7 + 96) / 100.0);
  64. session->newdata.ecef.y = (double)(getles32(buf, 7 + 100) / 100.0);
  65. session->newdata.ecef.z = (double)(getles32(buf, 7 + 104) / 100.0);
  66. session->newdata.ecef.vx = (double)(getles32(buf, 7 + 186) / 1000.0);
  67. session->newdata.ecef.vy = (double)(getles32(buf, 7 + 190) / 1000.0);
  68. session->newdata.ecef.vz = (double)(getles32(buf, 7 + 194) / 1000.0);
  69. mask |= ECEF_SET | VECEF_SET;
  70. /* this eph does not look right, badly documented.
  71. * let gpsd_error_model() handle it
  72. * session->newdata.eph = (double)(getles32(buf, 7 + 252) / 100.0);
  73. */
  74. session->newdata.eps = (double)(getles32(buf, 7 + 254) / 100.0);
  75. /* compute epx/epy in gpsd_error_model(), not here */
  76. mask |= HERR_SET;
  77. #define MAX(a,b) (((a) > (b)) ? (a) : (b))
  78. session->gpsdata.satellites_used =
  79. (int)MAX(getleu16(buf, 7 + 12), getleu16(buf, 7 + 14));
  80. mask |= USED_IS;
  81. if (flags & FIX_CONV_DOP_VALID) {
  82. session->gpsdata.dop.hdop = (double)(getleu16(buf, 7 + 56) / 100.0);
  83. session->gpsdata.dop.gdop = (double)(getleu16(buf, 7 + 58) / 100.0);
  84. session->gpsdata.dop.pdop = (double)(getleu16(buf, 7 + 60) / 100.0);
  85. session->gpsdata.dop.vdop = (double)(getleu16(buf, 7 + 62) / 100.0);
  86. session->gpsdata.dop.tdop = (double)(getleu16(buf, 7 + 64) / 100.0);
  87. mask |= DOP_SET;
  88. }
  89. if ((pflags & FIX_FLAG_MASK_INVALID) == 0
  90. && (flags & FIXINFO_FLAG_VALID) != 0) {
  91. if (pflags & FIX_FLAG_3DFIX)
  92. session->newdata.mode = MODE_3D;
  93. else
  94. session->newdata.mode = MODE_2D;
  95. if (pflags & FIX_FLAG_DGPS_CORRECTION)
  96. session->gpsdata.status = STATUS_DGPS_FIX;
  97. else
  98. session->gpsdata.status = STATUS_FIX;
  99. }
  100. GPSD_LOG(LOG_DATA, &session->context->errout,
  101. "NAV_FIX: time=%s, ecef x:%.2f y:%.2f z:%.2f altHAE=%.2f "
  102. "speed=%.2f track=%.2f climb=%.2f mode=%d status=%d gdop=%.2f "
  103. "pdop=%.2f hdop=%.2f vdop=%.2f tdop=%.2f\n",
  104. timespec_str(&session->newdata.time, ts_buf, sizeof(ts_buf)),
  105. session->newdata.ecef.x,
  106. session->newdata.ecef.y, session->newdata.ecef.z,
  107. session->newdata.altHAE, session->newdata.speed,
  108. session->newdata.track, session->newdata.climb,
  109. session->newdata.mode, session->gpsdata.status,
  110. session->gpsdata.dop.gdop, session->gpsdata.dop.pdop,
  111. session->gpsdata.dop.hdop, session->gpsdata.dop.vdop,
  112. session->gpsdata.dop.tdop);
  113. return mask;
  114. }
  115. static gps_mask_t decode_itk_prnstatus(struct gps_device_t *session,
  116. unsigned char *buf, size_t len)
  117. {
  118. gps_mask_t mask;
  119. if (len < 62) {
  120. GPSD_LOG(LOG_PROG, &session->context->errout,
  121. "ITALK: runt PRN_STATUS (len=%zu)\n", len);
  122. mask = 0;
  123. } else {
  124. unsigned int i, nsv, nchan, st;
  125. uint32_t msec = getleu32(buf, 7 + 6);
  126. timespec_t ts_tow;
  127. char ts_buf[TIMESPEC_LEN];
  128. MSTOTS(&ts_tow, msec);
  129. session->gpsdata.skyview_time = gpsd_gpstime_resolv(session,
  130. (unsigned short)getleu16(buf, 7 + 4), ts_tow);
  131. gpsd_zero_satellites(&session->gpsdata);
  132. nchan = (unsigned int)getleu16(buf, 7 + 50);
  133. if (nchan > MAX_NR_VISIBLE_PRNS)
  134. nchan = MAX_NR_VISIBLE_PRNS;
  135. for (i = st = nsv = 0; i < nchan; i++) {
  136. unsigned int off = 7 + 52 + 10 * i;
  137. unsigned short flags;
  138. bool used;
  139. flags = (unsigned short) getleu16(buf, off);
  140. used = (bool)(flags & PRN_FLAG_USE_IN_NAV);
  141. session->gpsdata.skyview[st].PRN = (short)(getleu16(buf, off + 4) & 0xff);
  142. session->gpsdata.skyview[st].elevation =
  143. (double)(getles16(buf, off + 6) & 0xff);
  144. session->gpsdata.skyview[st].azimuth =
  145. (double)(getles16(buf, off + 8) & 0xff);
  146. session->gpsdata.skyview[st].ss =
  147. (double)(getleu16(buf, off + 2) & 0xff);
  148. session->gpsdata.skyview[st].used = used;
  149. if (session->gpsdata.skyview[st].PRN > 0) {
  150. st++;
  151. if (used)
  152. nsv++;
  153. }
  154. }
  155. session->gpsdata.satellites_visible = (int)st;
  156. session->gpsdata.satellites_used = (int)nsv;
  157. mask = USED_IS | SATELLITE_SET;;
  158. GPSD_LOG(LOG_DATA, &session->context->errout,
  159. "PRN_STATUS: time=%s visible=%d used=%d "
  160. "mask={USED|SATELLITE}\n",
  161. timespec_str(&session->newdata.time, ts_buf, sizeof(ts_buf)),
  162. session->gpsdata.satellites_visible,
  163. session->gpsdata.satellites_used);
  164. }
  165. return mask;
  166. }
  167. static gps_mask_t decode_itk_utcionomodel(struct gps_device_t *session,
  168. unsigned char *buf, size_t len)
  169. {
  170. int leap;
  171. unsigned short flags;
  172. timespec_t ts_tow;
  173. uint32_t tow; /* Time of week [ms] */
  174. char ts_buf[TIMESPEC_LEN];
  175. if (len != 64) {
  176. GPSD_LOG(LOG_PROG, &session->context->errout,
  177. "ITALK: bad UTC_IONO_MODEL (len %zu, should be 64)\n",
  178. len);
  179. return 0;
  180. }
  181. flags = (unsigned short) getleu16(buf, 7);
  182. if (0 == (flags & UTC_IONO_MODEL_UTCVALID))
  183. return 0;
  184. leap = (int)getleu16(buf, 7 + 24);
  185. if (session->context->leap_seconds < leap)
  186. session->context->leap_seconds = leap;
  187. tow = getleu32(buf, 7 + 38); /* in ms */
  188. MSTOTS(&ts_tow, tow);
  189. session->newdata.time = gpsd_gpstime_resolv(session,
  190. (unsigned short) getleu16(buf, 7 + 36), ts_tow);
  191. GPSD_LOG(LOG_DATA, &session->context->errout,
  192. "UTC_IONO_MODEL: time=%s mask={TIME}\n",
  193. timespec_str(&session->newdata.time, ts_buf, sizeof(ts_buf)));
  194. return TIME_SET | NTPTIME_IS;
  195. }
  196. static gps_mask_t decode_itk_subframe(struct gps_device_t *session,
  197. unsigned char *buf, size_t len)
  198. {
  199. unsigned short flags, prn, sf;
  200. unsigned int i;
  201. uint32_t words[10];
  202. if (len != 64) {
  203. GPSD_LOG(LOG_PROG, &session->context->errout,
  204. "ITALK: bad SUBFRAME (len %zu, should be 64)\n", len);
  205. return 0;
  206. }
  207. flags = (unsigned short) getleu16(buf, 7 + 4);
  208. prn = (unsigned short) getleu16(buf, 7 + 6);
  209. sf = (unsigned short) getleu16(buf, 7 + 8);
  210. GPSD_LOG(LOG_PROG, &session->context->errout,
  211. "iTalk 50B SUBFRAME prn %u sf %u - decode %s %s\n",
  212. prn, sf,
  213. (flags & SUBFRAME_WORD_FLAG_MASK) ? "error" : "ok",
  214. (flags & SUBFRAME_GPS_PREAMBLE_INVERTED) ? "(inverted)" : "");
  215. if (flags & SUBFRAME_WORD_FLAG_MASK)
  216. return 0; // don't try decode an erroneous packet
  217. /*
  218. * Timo says "SUBRAME message contains decoded navigation message subframe
  219. * words with parity checking done but parity bits still present."
  220. */
  221. for (i = 0; i < 10; i++)
  222. words[i] = (uint32_t)(getleu32(buf, 7 + 14 + 4 * i) >> 6) & 0xffffff;
  223. return gpsd_interpret_subframe(session, prn, words);
  224. }
  225. static gps_mask_t decode_itk_pseudo(struct gps_device_t *session,
  226. unsigned char *buf, size_t len)
  227. {
  228. unsigned short flags, n, i;
  229. unsigned int tow; /* time of week, in ms */
  230. timespec_t ts_tow;
  231. n = (unsigned short) getleu16(buf, 7 + 4);
  232. if ((n < 1) || (n > MAXCHANNELS)){
  233. GPSD_LOG(LOG_INF, &session->context->errout,
  234. "ITALK: bad PSEUDO channel count\n");
  235. return 0;
  236. }
  237. if (len != (size_t)((n+1)*36)) {
  238. GPSD_LOG(LOG_PROG, &session->context->errout,
  239. "ITALK: bad PSEUDO len %zu\n", len);
  240. }
  241. GPSD_LOG(LOG_PROG, &session->context->errout, "iTalk PSEUDO [%u]\n", n);
  242. flags = (unsigned short)getleu16(buf, 7 + 6);
  243. if ((flags & 0x3) != 0x3)
  244. return 0; // bail if measurement time not valid.
  245. tow = (unsigned int)getleu32(buf, 7 + 38);
  246. MSTOTS(&ts_tow, tow);
  247. session->newdata.time = gpsd_gpstime_resolv(session,
  248. (unsigned short int)getleu16((char *)buf, 7 + 8), ts_tow);
  249. session->gpsdata.raw.mtime = session->newdata.time;
  250. /* this is so we can tell which never got set */
  251. for (i = 0; i < MAXCHANNELS; i++)
  252. session->gpsdata.raw.meas[i].svid = 0;
  253. for (i = 0; i < n; i++){
  254. session->gpsdata.skyview[i].PRN =
  255. getleu16(buf, 7 + 26 + (i*36)) & 0xff;
  256. session->gpsdata.skyview[i].ss =
  257. getleu16(buf, 7 + 26 + (i*36 + 2)) & 0x3f;
  258. session->gpsdata.raw.meas[i].satstat =
  259. getleu32(buf, 7 + 26 + (i*36 + 4));
  260. session->gpsdata.raw.meas[i].pseudorange =
  261. getled64((char *)buf, 7 + 26 + (i*36 + 8));
  262. session->gpsdata.raw.meas[i].doppler =
  263. getled64((char *)buf, 7 + 26 + (i*36 + 16));
  264. session->gpsdata.raw.meas[i].carrierphase =
  265. getleu16(buf, 7 + 26 + (i*36 + 28));
  266. session->gpsdata.raw.meas[i].codephase = NAN;
  267. session->gpsdata.raw.meas[i].deltarange = NAN;
  268. }
  269. /* return RAW_IS; The above decode does not give reasonable results */
  270. return 0; /* do not report valid until decode is fixed */
  271. }
  272. static gps_mask_t italk_parse(struct gps_device_t *session,
  273. unsigned char *buf, size_t len)
  274. {
  275. unsigned int type;
  276. gps_mask_t mask = 0;
  277. if (len == 0)
  278. return 0;
  279. type = (unsigned int) getub(buf, 4);
  280. /* we may need to dump the raw packet */
  281. GPSD_LOG(LOG_RAW, &session->context->errout,
  282. "raw italk packet type 0x%02x\n", type);
  283. session->cycle_end_reliable = true;
  284. switch (type) {
  285. case ITALK_NAV_FIX:
  286. GPSD_LOG(LOG_DATA, &session->context->errout,
  287. "iTalk NAV_FIX len %zu\n", len);
  288. mask = decode_itk_navfix(session, buf, len) | (CLEAR_IS | REPORT_IS);
  289. break;
  290. case ITALK_PRN_STATUS:
  291. GPSD_LOG(LOG_DATA, &session->context->errout,
  292. "iTalk PRN_STATUS len %zu\n", len);
  293. mask = decode_itk_prnstatus(session, buf, len);
  294. break;
  295. case ITALK_UTC_IONO_MODEL:
  296. GPSD_LOG(LOG_DATA, &session->context->errout,
  297. "iTalk UTC_IONO_MODEL len %zu\n", len);
  298. mask = decode_itk_utcionomodel(session, buf, len);
  299. break;
  300. case ITALK_ACQ_DATA:
  301. GPSD_LOG(LOG_DATA, &session->context->errout,
  302. "iTalk ACQ_DATA len %zu\n", len);
  303. break;
  304. case ITALK_TRACK:
  305. GPSD_LOG(LOG_DATA, &session->context->errout,
  306. "iTalk TRACK len %zu\n", len);
  307. break;
  308. case ITALK_PSEUDO:
  309. GPSD_LOG(LOG_DATA, &session->context->errout,
  310. "iTalk PSEUDO len %zu\n", len);
  311. mask = decode_itk_pseudo(session, buf, len);
  312. break;
  313. case ITALK_RAW_ALMANAC:
  314. GPSD_LOG(LOG_DATA, &session->context->errout,
  315. "iTalk RAW_ALMANAC len %zu\n", len);
  316. break;
  317. case ITALK_RAW_EPHEMERIS:
  318. GPSD_LOG(LOG_DATA, &session->context->errout,
  319. "iTalk RAW_EPHEMERIS len %zu\n", len);
  320. break;
  321. case ITALK_SUBFRAME:
  322. mask = decode_itk_subframe(session, buf, len);
  323. break;
  324. case ITALK_BIT_STREAM:
  325. GPSD_LOG(LOG_DATA, &session->context->errout,
  326. "iTalk BIT_STREAM len %zu\n", len);
  327. break;
  328. case ITALK_AGC:
  329. case ITALK_SV_HEALTH:
  330. case ITALK_PRN_PRED:
  331. case ITALK_FREQ_PRED:
  332. case ITALK_DBGTRACE:
  333. case ITALK_START:
  334. case ITALK_STOP:
  335. case ITALK_SLEEP:
  336. case ITALK_STATUS:
  337. case ITALK_ITALK_CONF:
  338. case ITALK_SYSINFO:
  339. case ITALK_ITALK_TASK_ROUTE:
  340. case ITALK_PARAM_CTRL:
  341. case ITALK_PARAMS_CHANGED:
  342. case ITALK_START_COMPLETED:
  343. case ITALK_STOP_COMPLETED:
  344. case ITALK_LOG_CMD:
  345. case ITALK_SYSTEM_START:
  346. case ITALK_STOP_SEARCH:
  347. case ITALK_SEARCH:
  348. case ITALK_PRED_SEARCH:
  349. case ITALK_SEARCH_DONE:
  350. case ITALK_TRACK_DROP:
  351. case ITALK_TRACK_STATUS:
  352. case ITALK_HANDOVER_DATA:
  353. case ITALK_CORE_SYNC:
  354. case ITALK_WAAS_RAWDATA:
  355. case ITALK_ASSISTANCE:
  356. case ITALK_PULL_FIX:
  357. case ITALK_MEMCTRL:
  358. case ITALK_STOP_TASK:
  359. GPSD_LOG(LOG_DATA, &session->context->errout,
  360. "iTalk not processing packet: id 0x%02x length %zu\n",
  361. type, len);
  362. break;
  363. default:
  364. GPSD_LOG(LOG_DATA, &session->context->errout,
  365. "iTalk unknown packet: id 0x%02x length %zu\n",
  366. type, len);
  367. }
  368. return mask | ONLINE_SET;
  369. }
  370. static gps_mask_t italk_parse_input(struct gps_device_t *session)
  371. {
  372. if (session->lexer.type == ITALK_PACKET) {
  373. return italk_parse(session, session->lexer.outbuffer,
  374. session->lexer.outbuflen);;
  375. #ifdef NMEA0183_ENABLE
  376. } else if (session->lexer.type == NMEA_PACKET) {
  377. return nmea_parse((char *)session->lexer.outbuffer, session);
  378. #endif /* NMEA0183_ENABLE */
  379. } else
  380. return 0;
  381. }
  382. #ifdef __UNUSED__
  383. static void italk_ping(struct gps_device_t *session)
  384. /* send a "ping". it may help us detect an itrax more quickly */
  385. {
  386. char *ping = "<?>";
  387. (void)gpsd_write(session, ping, 3);
  388. }
  389. #endif /* __UNUSED__ */
  390. /* *INDENT-OFF* */
  391. const struct gps_type_t driver_italk =
  392. {
  393. .type_name = "iTalk", /* full name of type */
  394. .packet_type = ITALK_PACKET, /* associated lexer packet type */
  395. .flags = DRIVER_STICKY, /* no rollover or other flags */
  396. .trigger = NULL, /* recognize the type */
  397. .channels = 12, /* consumer-grade GPS */
  398. .probe_detect = NULL, /* how to detect at startup time */
  399. .get_packet = generic_get, /* use generic packet grabber */
  400. .parse_packet = italk_parse_input,/* parse message packets */
  401. .rtcm_writer = gpsd_write, /* send RTCM data straight */
  402. .init_query = NULL, /* non-perturbing initial query */
  403. .event_hook = NULL, /* lifetime event handler */
  404. #ifdef RECONFIGURE_ENABLE
  405. .speed_switcher = NULL, /* no speed switcher */
  406. .mode_switcher = NULL, /* no mode switcher */
  407. .rate_switcher = NULL, /* no sample-rate switcher */
  408. .min_cycle.tv_sec = 1, /* not relevant, no rate switch */
  409. .min_cycle.tv_nsec = 0, /* not relevant, no rate switch */
  410. #endif /* RECONFIGURE_ENABLE */
  411. #ifdef CONTROLSEND_ENABLE
  412. .control_send = NULL, /* no control string sender */
  413. #endif /* CONTROLSEND_ENABLE */
  414. .time_offset = NULL, /* no method for NTP fudge factor */
  415. };
  416. /* *INDENT-ON* */
  417. #endif /* defined(ITRAX_ENABLE) && defined(BINARY_ENABLE) */