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.

1275 lines
44KB

  1. /*
  2. * Driver for Navcom receivers using proprietary NCT messages, a binary protocol.
  3. *
  4. * Vendor website: http://www.navcomtech.com/
  5. * Technical references: Technical Reference Manual P/N 96-3120001-3001
  6. *
  7. * Tested with two SF-2040G models
  8. *
  9. * At this stage, this driver implements the following commands:
  10. *
  11. * 0x20: Data Request (tell the unit which responses you want)
  12. * 0x3f: LED Configuration (controls the front panel LEDs -- for testing)
  13. * 0x1c: Test Support Block (again, blinks the front panel lights)
  14. *
  15. * and it understands the following responses:
  16. *
  17. * 0x06: Acknowledgement (without error)
  18. * 0x15: Negative Acknowledge
  19. * 0x86: Channel Status
  20. * 0xae: Identification Block
  21. * 0xb0: Raw Meas. Data Block
  22. * 0xb1: PVT Block
  23. * 0xb5: Pseudorange Noise Statistics
  24. * 0xd3: LBM DSP Status Block
  25. * 0xef: Clock Drift and Offset
  26. *
  27. * By Diego Berge. Contact via web form at http://www.navlost.eu/contact
  28. *
  29. * Week counters are not limited to 10 bits. It's unknown what
  30. * the firmware is doing to disambiguate them, if anything; it might just
  31. * be adding a fixed offset based on a hidden epoch value, in which case
  32. * unhappy things will occur on the next rollover.
  33. *
  34. * This file is Copyright (c) 2010-2018 by the GPSD project
  35. * SPDX-License-Identifier: BSD-2-clause
  36. */
  37. #include "gpsd_config.h" /* must be before all includes */
  38. #include <stdio.h>
  39. #include <stdbool.h>
  40. #include <string.h>
  41. #include <math.h>
  42. #include "gpsd.h"
  43. #if defined(NAVCOM_ENABLE)
  44. #include "bits.h"
  45. #include "timespec.h"
  46. /* Have data which is 24 bits long */
  47. #define getles3224(buf,off) (int32_t)(((uint32_t)getub((buf), (off)+2)<<24 | (uint32_t)getub((buf), (off)+1)<<16 | (uint32_t)getub((buf), (off))<<8)>>8)
  48. #define getleu3224(buf,off) (uint32_t)(((uint32_t)getub((buf), (off)+2)<<24 | (uint32_t)getub((buf), (off)+1)<<16 | (uint32_t)getub((buf), (off))<<8)>>8)
  49. /* And just to be difficult, Navcom is little endian but the GPS data stream
  50. is big endian. Some messages contain raw GPS data */
  51. #define getles16_be(buf, off) (int16_t)((((uint16_t)getub(buf, (off)) << 8) \
  52. | (uint16_t)getub(buf, (off)+1)))
  53. #define getleu16_be(buf, off) (uint16_t)((((uint16_t)getub(buf, (off)) << 8) \
  54. | (uint16_t)getub(buf, (off)+1)))
  55. #define getles32_be(buf, off) (int32_t)((((uint16_t)getleu16_be(buf, (off)) << 16) \
  56. | getleu16_be(buf, (off)+2)))
  57. #define getleu32_be(buf, off) (uint32_t)((((uint16_t)getleu16_be(buf, (off)) << 16) \
  58. | getleu16_be(buf, (off)+2)))
  59. #define getles64_be(buf, off) (int64_t)((((uint64_t)getleu32_be(buf, (off)) << 32) \
  60. | getleu32_be(buf, (off)+4)))
  61. #define getleu64_be(buf, off) (uint64_t)((((uint64_t)getleu32_be(buf, (off)) << 32) \
  62. | getleu32_be(buf, (off)+4)))
  63. #define getles3224_be(buf,off) (int32_t)(((uint32_t)getub((buf), (off))<<24 \
  64. | (uint32_t)getub((buf), (off)+1)<<16 \
  65. | (uint32_t)getub((buf), (off)+2)<<8)>>8)
  66. #define NAVCOM_CHANNELS 12
  67. static uint8_t checksum(unsigned char *buf, size_t len)
  68. {
  69. size_t n;
  70. uint8_t csum = (uint8_t) 0x00;
  71. for (n = 0; n < len; n++)
  72. csum ^= buf[n];
  73. return csum;
  74. }
  75. static bool navcom_send_cmd(struct gps_device_t *session, unsigned char *cmd,
  76. size_t len)
  77. {
  78. return (gpsd_write(session, (const char *)cmd, len) == (ssize_t) len);
  79. }
  80. /* Data Request */
  81. static void navcom_cmd_0x20(struct gps_device_t *session, uint8_t block_id,
  82. uint16_t rate)
  83. {
  84. unsigned char msg[18];
  85. putbyte(msg, 0, 0x02);
  86. putbyte(msg, 1, 0x99);
  87. putbyte(msg, 2, 0x66);
  88. putbyte(msg, 3, 0x20); /* Cmd ID */
  89. putle16(msg, 4, 0x000e); /* Length */
  90. putbyte(msg, 6, 0x00); /* Action */
  91. putbyte(msg, 7, 0x01); /* Count of blocks */
  92. putbyte(msg, 8, block_id); /* Data Block ID */
  93. putbyte(msg, 9, 0x02); /* Logical Ports */
  94. putle16(msg, 10, rate); /* Data rate */
  95. putbyte(msg, 12, 0x71);
  96. putbyte(msg, 13, 0x00);
  97. putle16(msg, 14, 0x0000);
  98. putbyte(msg, 16, checksum(msg + 3, 13));
  99. putbyte(msg, 17, 0x03);
  100. (void)navcom_send_cmd(session, msg, 18);
  101. GPSD_LOG(LOG_PROG, &session->context->errout,
  102. "Navcom: sent command 0x20 (Data Request) "
  103. "- data block id = %02x at rate %02x\n", block_id, rate);
  104. }
  105. // cppcheck-suppress unusedFunction
  106. static void UNUSED navcom_cmd_0x3f(struct gps_device_t *session)
  107. /* Changes the LED settings in the receiver */
  108. {
  109. unsigned char msg[12];
  110. putbyte(msg, 0, 0x02);
  111. putbyte(msg, 1, 0x99);
  112. putbyte(msg, 2, 0x66);
  113. putbyte(msg, 3, 0x3f); /* Cmd ID */
  114. putle16(msg, 4, 0x0008);
  115. putbyte(msg, 6, 0x01); /* Action */
  116. putbyte(msg, 7, 0x00); /* Reserved */
  117. putbyte(msg, 8, 0x02); /* Link LED setting */
  118. putbyte(msg, 9, 0x0a); /* Battery LED setting */
  119. putbyte(msg, 10, checksum(msg + 3, 7));
  120. putbyte(msg, 11, 0x03);
  121. (void)navcom_send_cmd(session, msg, 12);
  122. GPSD_LOG(LOG_PROG, &session->context->errout,
  123. "Navcom: sent command 0x3f (LED Configuration Block)\n");
  124. }
  125. /* Test Support Block - Blinks the LEDs */
  126. static void navcom_cmd_0x1c(struct gps_device_t *session, uint8_t mode,
  127. uint8_t length)
  128. {
  129. unsigned char msg[12];
  130. putbyte(msg, 0, 0x02);
  131. putbyte(msg, 1, 0x99);
  132. putbyte(msg, 2, 0x66);
  133. putbyte(msg, 3, 0x1c); /* Cmd ID */
  134. putle16(msg, 4, 0x0008);
  135. putbyte(msg, 6, 0x04); /* Use ACK/NAK */
  136. putbyte(msg, 7, mode); /* 0x01 or 0x02 */
  137. putbyte(msg, 8, length); /* Only if mode == 0x01 */
  138. putbyte(msg, 9, 0x00);
  139. putbyte(msg, 10, checksum(msg + 3, 7));
  140. putbyte(msg, 11, 0x03);
  141. (void)navcom_send_cmd(session, msg, 12);
  142. GPSD_LOG(LOG_PROG, &session->context->errout,
  143. "Navcom: sent command 0x1c (Test Support Block)\n");
  144. GPSD_LOG(LOG_DATA, &session->context->errout,
  145. "Navcom: command 0x1c mode = %02x, length = %u\n",
  146. mode, length);
  147. }
  148. #ifdef RECONFIGURE_ENABLE
  149. /* Serial Port Configuration */
  150. static void navcom_cmd_0x11(struct gps_device_t *session,
  151. uint8_t port_selection)
  152. {
  153. /* NOTE - We only allow changing one port at a time,
  154. * although the message supports doing both at once. */
  155. unsigned char msg[12];
  156. putbyte(msg, 0, 0x02);
  157. putbyte(msg, 1, 0x99);
  158. putbyte(msg, 2, 0x66);
  159. putbyte(msg, 3, 0x11); /* Cmd ID */
  160. putle16(msg, 4, 0x0008); /* Length */
  161. putbyte(msg, 6, 0x04); /* Action - Use ACK/NAK) */
  162. putbyte(msg, 7, port_selection);
  163. putbyte(msg, 8, 0x00); /* Reserved */
  164. putbyte(msg, 9, 0x00); /* Reserved */
  165. putbyte(msg, 10, checksum(msg + 3, 7));
  166. putbyte(msg, 11, 0x03);
  167. (void)navcom_send_cmd(session, msg, 12);
  168. GPSD_LOG(LOG_PROG, &session->context->errout,
  169. "Navcom: sent command 0x11 (Serial Port Configuration)\n");
  170. GPSD_LOG(LOG_DATA, &session->context->errout,
  171. "Navcom: serial port selection: 0x%02x\n", port_selection);
  172. }
  173. #endif /* RECONFIGURE_ENABLE */
  174. static void navcom_event_hook(struct gps_device_t *session, event_t event)
  175. {
  176. if (session->context->readonly)
  177. return;
  178. /* Request the following messages: */
  179. if (event == event_identified) {
  180. /* NOTE - Channel Status allows us to know into which of the
  181. * unit's various serial ports we are connected.
  182. * Its value gets updated every time we receive a 0x06 (Ack)
  183. * message. Note that if commands are being fed into the
  184. * unit from more than one port (which is entirely possible
  185. * although not necessarily a bright idea), there is a good
  186. * chance that we might misidentify our port */
  187. navcom_cmd_0x1c(session, 0x02, 0); /* Test Support Block */
  188. navcom_cmd_0x20(session, 0xae, 0x0000); /* Identification Block */
  189. navcom_cmd_0x20(session, 0x86, 0x000a); /* Channel Status */
  190. navcom_cmd_0x1c(session, 0x01, 5); /* Blink LEDs on receiver */
  191. navcom_cmd_0x20(session, 0xae, 0x1770); /* Identification Block - send every 10 min */
  192. navcom_cmd_0x20(session, 0xb1, 0x4000); /* PVT Block */
  193. navcom_cmd_0x20(session, 0xb5, 0x00c8); /* Pseudorange Noise Statistics - send every 20s */
  194. navcom_cmd_0x20(session, 0xb0, 0x4000); /* Raw Meas Data Block */
  195. navcom_cmd_0x20(session, 0x81, 0x0000); /* Packed Ephemeris Data - send once */
  196. navcom_cmd_0x20(session, 0x81, 0x4000); /* Packed Ephemeris Data */
  197. navcom_cmd_0x20(session, 0x86, 0x4000); /* Channel Status */
  198. navcom_cmd_0x20(session, 0x83, 0x4000); /* Ionosphere and UTC Data */
  199. navcom_cmd_0x20(session, 0xef, 0x0bb8); /* Clock Drift - send every 5 min */
  200. }
  201. }
  202. /* Ionosphere and UTC Data */
  203. static gps_mask_t handle_0x83(struct gps_device_t *session)
  204. {
  205. /* NOTE - At the present moment this is only being used
  206. * for determining the GPS-UTC time difference,
  207. * for which the iono data is not needed as far
  208. * as we are concerned. However, I am still
  209. * reporting it (if debuglevel >= LOG_DATA) as a
  210. * matter of interest */
  211. /* 2^-30 */
  212. #define SF_A0 (0.000000000931322574615478515625)
  213. /* 2^-50 */
  214. #define SF_A1 (0.000000000000000888178419700125)
  215. /* 2^12 */
  216. #define SF_TOT (4096)
  217. /* 2^-30 */
  218. #define SF_ALPHA0 (0.000000000931322574615478515625)
  219. /* 2^-27 */
  220. #define SF_ALPHA1 (0.000000007450580596923828125)
  221. /* 2^-24 */
  222. #define SF_ALPHA2 (0.000000059604644775390625)
  223. /* 2^-24 */
  224. #define SF_ALPHA3 (0.000000059604644775390625)
  225. /* 2^11 */
  226. #define SF_BETA0 (2048)
  227. /* 2^14 */
  228. #define SF_BETA1 (16384)
  229. /* 2^16 */
  230. #define SF_BETA2 (65536)
  231. /* 2^16 */
  232. #define SF_BETA3 (65536)
  233. unsigned char *buf = session->lexer.outbuffer + 3;
  234. uint16_t week = getleu16(buf, 3);
  235. uint32_t tow = getleu32(buf, 5);
  236. int8_t alpha0 = getsb(buf, 9);
  237. int8_t alpha1 = getsb(buf, 10);
  238. int8_t alpha2 = getsb(buf, 11);
  239. int8_t alpha3 = getsb(buf, 12);
  240. int8_t beta0 = getsb(buf, 13);
  241. int8_t beta1 = getsb(buf, 14);
  242. int8_t beta2 = getsb(buf, 15);
  243. int8_t beta3 = getsb(buf, 16);
  244. int32_t a1 = getles32(buf, 17);
  245. int32_t a0 = getles32(buf, 21);
  246. uint8_t tot = getub(buf, 25);
  247. uint8_t wnt = getub(buf, 26);
  248. int8_t dtls = getsb(buf, 27);
  249. uint8_t wnlsf = getub(buf, 28);
  250. uint8_t dn = getub(buf, 29);
  251. int8_t dtlsf = getsb(buf, 30);
  252. /* Ref.: ICD-GPS-200C 20.3.3.5.2.4 */
  253. if ((week % 256) * 604800 + tow / 1000.0 < wnlsf * 604800 + dn * 86400) {
  254. /* Effectivity time is in the future, use dtls */
  255. session->context->leap_seconds = (int)dtls;
  256. } else {
  257. /* Effectivity time is not in the future, use dtlsf */
  258. session->context->leap_seconds = (int)dtlsf;
  259. }
  260. GPSD_LOG(LOG_PROG, &session->context->errout,
  261. "Navcom: received packet type 0x83 (Ionosphere and UTC Data)\n");
  262. GPSD_LOG(LOG_DATA, &session->context->errout,
  263. "Navcom: Scaled parameters follow:\n");
  264. GPSD_LOG(LOG_DATA, &session->context->errout,
  265. "Navcom: GPS Week: %u, GPS Time of Week: %u (GPS Time: %f)\n",
  266. week, tow, week * 604800 + tow / 1000.0);
  267. GPSD_LOG(LOG_DATA, &session->context->errout,
  268. "Navcom: a0: %12.4E, a1: %12.4E, a2: %12.4E, a3: %12.4E, "
  269. "b0: %12.4E, b1: %12.4E, b2: %12.4E, b3: %12.4E\n",
  270. (double)alpha0 * SF_ALPHA0, (double)alpha1 * SF_ALPHA1,
  271. (double)alpha2 * SF_ALPHA2, (double)alpha3 * SF_ALPHA3,
  272. (double)beta0 * SF_BETA0, (double)beta1 * SF_BETA1,
  273. (double)beta2 * SF_BETA2, (double)beta3 * SF_BETA3);
  274. GPSD_LOG(LOG_DATA, &session->context->errout,
  275. "Navcom: A0: %19.12E, A1: %19.12E\n", (double)a0 * SF_A0,
  276. (double)a1 * SF_A1);
  277. GPSD_LOG(LOG_DATA, &session->context->errout,
  278. "Navcom: UTC Ref. Time: %lu, UTC Ref. Week: %u, dTls: %d\n",
  279. (unsigned long)tot * SF_TOT, wnt, dtls);
  280. GPSD_LOG(LOG_DATA, &session->context->errout,
  281. "Navcom: Week of leap seconds: %u, Day number of leap seconds: %u, dTlsf: %d\n",
  282. wnlsf, dn, dtlsf);
  283. return 0; /* No flag for update of leap seconds (Not part of a fix) */
  284. #undef SF_A0
  285. #undef SF_A1
  286. #undef SF_TOT
  287. #undef SF_ALPHA0
  288. #undef SF_ALPHA1
  289. #undef SF_ALPHA2
  290. #undef SF_ALPHA3
  291. #undef SF_BETA0
  292. #undef SF_BETA1
  293. #undef SF_BETA2
  294. #undef SF_BETA3
  295. }
  296. /* Acknowledgement (without error) */
  297. static gps_mask_t handle_0x06(struct gps_device_t *session)
  298. {
  299. unsigned char *buf = session->lexer.outbuffer + 3;
  300. uint8_t cmd_id = getub(buf, 3);
  301. uint8_t port = getub(buf, 4);
  302. session->driver.navcom.physical_port = port; /* This tells us which serial port was used last */
  303. GPSD_LOG(LOG_PROG, &session->context->errout,
  304. "Navcom: received packet type 0x06 (Acknowledgement (without error))\n");
  305. GPSD_LOG(LOG_DATA, &session->context->errout,
  306. "Navcom: acknowledged command id 0x%02x on port %c\n",
  307. cmd_id, (port == 0 ? 'A' : (port == 1 ? 'B' : '?')));
  308. return 0; /* Nothing updated */
  309. }
  310. /* Negative Acknowledge */
  311. static gps_mask_t handle_0x15(struct gps_device_t *session)
  312. {
  313. size_t n;
  314. unsigned char *buf = session->lexer.outbuffer + 3;
  315. size_t msg_len = (size_t) getleu16(buf, 1);
  316. uint8_t port, cmd_id = getub(buf, 3);
  317. GPSD_LOG(LOG_PROG, &session->context->errout,
  318. "Navcom: received packet type 0x15 (Negative Acknowledge)\n");
  319. /* coverity_submit[tainted_data] */
  320. for (n = 4; n < (msg_len - 2); n += 2) {
  321. uint8_t err_id = getub(buf, n);
  322. uint8_t err_desc = getub(buf, n + 1);
  323. GPSD_LOG(LOG_DATA, &session->context->errout,
  324. "Navcom: error id = 0x%02x, error description = 0x%02x\n",
  325. err_id, err_desc);
  326. }
  327. port = getub(buf, n);
  328. GPSD_LOG(LOG_DATA, &session->context->errout,
  329. "Navcom: negative acknowledge was for command id 0x%02x on port %c\n",
  330. cmd_id, (port == 0 ? 'A' : (port == 1 ? 'B' : '?')));
  331. return 0; /* Nothing updated */
  332. }
  333. /* PVT Block */
  334. static gps_mask_t handle_0xb1(struct gps_device_t *session)
  335. {
  336. gps_mask_t mask;
  337. unsigned char *buf = session->lexer.outbuffer + 3;
  338. uint16_t week;
  339. uint32_t tow;
  340. timespec_t ts_tow;
  341. int32_t lat, lon;
  342. char ts_buf[TIMESPEC_LEN];
  343. /* Resolution of lat/lon values (2^-11) */
  344. #define LL_RES (0.00048828125)
  345. uint8_t lat_fraction, lon_fraction;
  346. /* Resolution of lat/lon fractions (2^-15) */
  347. #define LL_FRAC_RES (0.000030517578125)
  348. uint8_t nav_mode;
  349. int32_t ellips_height, altitude;
  350. /* Resolution of height and altitude values (2.0^-10) */
  351. #define EL_RES (0.0009765625)
  352. double vel_north, vel_east, vel_up;
  353. uint8_t gdop, pdop, hdop, vdop, tdop;
  354. /* This value means "undefined" */
  355. #define DOP_UNDEFINED (255)
  356. int16_t ant_height_adj;
  357. int32_t set_delta_up;
  358. /* Resolution of delta north, east, and up,
  359. * and ant. height adjustment values (1mm) */
  360. #define D_RES (0.001)
  361. #ifdef __UNUSED__
  362. /* Other values provided by the PVT block which we
  363. * may want to provide in the future.
  364. */
  365. uint8_t dgps_conf;
  366. uint16_t max_dgps_age;
  367. uint8_t ext_nav_mode;
  368. int32_t set_delta_north, set_delta_east;
  369. uint8_t nav_failure_code;
  370. #endif /* __UNUSED__ */
  371. /* Timestamp */
  372. week = (uint16_t) getleu16(buf, 3);
  373. tow = (uint32_t) getleu32(buf, 5); /* tow in ms */
  374. MSTOTS(&ts_tow, tow);
  375. session->newdata.time = gpsd_gpstime_resolv(session, week, ts_tow);
  376. /* Get latitude, longitude */
  377. lat = getles32(buf, 13);
  378. lon = getles32(buf, 17);
  379. lat_fraction = (uint8_t) (getub(buf, 21) >> 4);
  380. lon_fraction = (uint8_t) (getub(buf, 21) & 0x0f);
  381. session->newdata.latitude =
  382. (double)(lat * LL_RES + lat_fraction * LL_FRAC_RES) / 3600;
  383. session->newdata.longitude =
  384. (double)(lon * LL_RES + lon_fraction * LL_FRAC_RES) / 3600;
  385. /* Nav mode */
  386. nav_mode = (uint8_t) getub(buf, 22);
  387. if (-nav_mode & 0x80) {
  388. session->gpsdata.status = STATUS_NO_FIX;
  389. session->newdata.mode = MODE_NO_FIX;
  390. } else {
  391. session->newdata.mode = ((nav_mode & 0x40)!=0 ? MODE_3D : MODE_2D);
  392. session->gpsdata.status =
  393. ((nav_mode & 0x03)!=0 ? STATUS_DGPS_FIX : STATUS_FIX);
  394. }
  395. /* altHAE */
  396. ellips_height = getles32(buf, 23);
  397. /* altMSL */
  398. altitude = getles32(buf, 27);
  399. ant_height_adj = getles16(buf, 51);
  400. set_delta_up = getles32(buf, 79);
  401. session->newdata.altMSL = (double)(altitude * EL_RES)
  402. + (ant_height_adj * D_RES) + (set_delta_up * D_RES);
  403. session->newdata.altHAE = (double)(ellips_height) * EL_RES
  404. + (ant_height_adj * D_RES) + (set_delta_up * D_RES);
  405. /* Let gpsd_error_model() deal with geoid_sep */
  406. /* Speed Data */
  407. vel_north = (double)getles3224(buf, 31);
  408. vel_east = (double)getles3224(buf, 34);
  409. vel_up = (double)getles3224(buf, 37);
  410. session->newdata.NED.velN = vel_north * 0.1;
  411. session->newdata.NED.velE = vel_east * 0.1;
  412. session->newdata.NED.velD = -vel_up * 0.1;
  413. /* Quality indicators */
  414. /* UNUSED fom = getub(buf, 40); * FOM is DRMS */
  415. gdop = getub(buf, 41);
  416. pdop = getub(buf, 42);
  417. hdop = getub(buf, 43);
  418. vdop = getub(buf, 44);
  419. tdop = getub(buf, 45);
  420. /* UNUSED tfom = getub(buf, 46); * tfom == 10 * TDOP */
  421. /* let gpsd_error_model() do the error estimates */
  422. if (gdop != DOP_UNDEFINED)
  423. session->gpsdata.dop.gdop = gdop / 10.0;
  424. if (pdop != DOP_UNDEFINED)
  425. session->gpsdata.dop.pdop = pdop / 10.0;
  426. if (hdop != DOP_UNDEFINED)
  427. session->gpsdata.dop.hdop = hdop / 10.0;
  428. if (vdop != DOP_UNDEFINED)
  429. session->gpsdata.dop.vdop = vdop / 10.0;
  430. if (tdop != DOP_UNDEFINED)
  431. session->gpsdata.dop.tdop = tdop / 10.0;
  432. GPSD_LOG(LOG_PROG, &session->context->errout,
  433. "Navcom: received packet type 0xb1 (PVT Report)\n");
  434. GPSD_LOG(LOG_DATA, &session->context->errout,
  435. "Navcom: navigation mode %s (0x%02x) - %s - %s\n",
  436. ((-nav_mode & 0x80)!='\0' ? "invalid" : "valid"), nav_mode,
  437. ((nav_mode & 0x40)!='\0' ? "3D" : "2D"),
  438. ((nav_mode & 0x03)!='\0' ? "DGPS" : "GPS"));
  439. GPSD_LOG(LOG_DATA, &session->context->errout,
  440. "Navcom: velocities: north = %f east = %f up = %f\n",
  441. session->newdata.NED.velN,
  442. session->newdata.NED.velE,
  443. -session->newdata.NED.velD);
  444. #undef D_RES
  445. #undef LL_RES
  446. #undef LL_FRAC_RES
  447. #undef EL_RES
  448. #undef VEL_RES
  449. #undef DOP_UNDEFINED
  450. mask = LATLON_SET | ALTITUDE_SET | STATUS_SET | MODE_SET | USED_IS |
  451. HERR_SET | TIMERR_SET | DOP_SET | VNED_SET | TIME_SET | NTPTIME_IS;
  452. GPSD_LOG(LOG_DATA, &session->context->errout,
  453. "PVT 0xb1: time=%s, lat=%.2f lon=%.2f altHAE=%.2f "
  454. "altMSL %.2f mode=%d status=%d gdop=%.2f pdop=%.2f hdop=%.2f "
  455. "vdop=%.2f tdop=%.2f mask={%s}\n",
  456. timespec_str(&session->newdata.time, ts_buf, sizeof(ts_buf)),
  457. session->newdata.latitude,
  458. session->newdata.longitude,
  459. session->newdata.altHAE,
  460. session->newdata.altMSL,
  461. session->newdata.mode,
  462. session->gpsdata.status,
  463. session->gpsdata.dop.gdop,
  464. session->gpsdata.dop.pdop,
  465. session->gpsdata.dop.hdop,
  466. session->gpsdata.dop.vdop, session->gpsdata.dop.tdop,
  467. gps_maskdump(mask));
  468. return mask;
  469. }
  470. /* Packed Ephemeris Data */
  471. static gps_mask_t handle_0x81(struct gps_device_t *session)
  472. {
  473. /* Scale factors for everything */
  474. /* 2^-31 */
  475. #define SF_TGD (.000000000465661287307739257812)
  476. /* 2^4 */
  477. #define SF_TOC (16)
  478. /* 2^-55 */
  479. #define SF_AF2 (.000000000000000027755575615628)
  480. /* 2^-43 */
  481. #define SF_AF1 (.000000000000113686837721616029)
  482. /* 2^-31 */
  483. #define SF_AF0 (.000000000465661287307739257812)
  484. /* 2^-5 */
  485. #define SF_CRS (.031250000000000000000000000000)
  486. /* 2^-43 */
  487. #define SF_DELTA_N (.000000000000113686837721616029)
  488. /* 2^-31 */
  489. #define SF_M0 (.000000000465661287307739257812)
  490. /* 2^-29 */
  491. #define SF_CUC (.000000001862645149230957031250)
  492. /* 2^-33 */
  493. #define SF_E (.000000000116415321826934814453)
  494. /* 2^-29 */
  495. #define SF_CUS (.000000001862645149230957031250)
  496. /* 2^-19 */
  497. #define SF_SQRT_A (.000001907348632812500000000000)
  498. /* 2^4 */
  499. #define SF_TOE (16)
  500. /* 2^-29 */
  501. #define SF_CIC (.000000001862645149230957031250)
  502. /* 2^-31 */
  503. #define SF_OMEGA0 (.000000000465661287307739257812)
  504. /* 2^-29 */
  505. #define SF_CIS (.000000001862645149230957031250)
  506. /* 2^-31 */
  507. #define SF_I0 (.000000000465661287307739257812)
  508. /* 2^-5 */
  509. #define SF_CRC (.031250000000000000000000000000)
  510. /* 2^-31 */
  511. #define SF_OMEGA (.000000000465661287307739257812)
  512. /* 2^-43 */
  513. #define SF_OMEGADOT (.000000000000113686837721616029)
  514. /* 2^-43 */
  515. #define SF_IDOT (.000000000000113686837721616029)
  516. char ts_buf[TIMESPEC_LEN];
  517. unsigned char *buf = session->lexer.outbuffer + 3;
  518. uint8_t prn = getub(buf, 3);
  519. uint16_t week = getleu16(buf, 4);
  520. uint32_t tow = getleu32(buf, 6);
  521. uint16_t iodc = getleu16(buf, 10);
  522. /* And now the fun starts... everything that follows is
  523. * raw GPS data minus parity */
  524. /* Subframe 1, words 3 to 10 minus parity */
  525. uint16_t wn = (getleu16_be(buf, 12) & 0xffc0) >> 6;
  526. uint8_t cl2 = (getub(buf, 13) & 0x30) >> 4;
  527. uint8_t ura = getub(buf, 13) & 0x0f;
  528. uint8_t svh = (getub(buf, 14) & 0xfc) >> 2;
  529. /* We already have IODC from earlier in the message, so
  530. * we do not decode again */
  531. /* uint16_t iodc = (getub(buf, 14)&0x03)<<8;*/
  532. uint8_t l2pd = (getub(buf, 15) & 0x80) >> 7;
  533. int8_t tgd = getsb(buf, 26);
  534. /* iodc |= getub(buf, 27);*/
  535. uint16_t toc = getleu16_be(buf, 28);
  536. int8_t af2 = getsb(buf, 30);
  537. int16_t af1 = getles16_be(buf, 31);
  538. int32_t af0 = getles3224_be(buf, 33) >> 2;
  539. /* Subframe 2, words 3 to 10 minus parity */
  540. uint8_t iode = getub(buf, 36);
  541. int16_t crs = getles16_be(buf, 37);
  542. int16_t delta_n = getles16_be(buf, 39);
  543. int32_t m0 = getles32_be(buf, 41);
  544. int16_t cuc = getles16_be(buf, 45);
  545. uint32_t e = getleu32_be(buf, 47);
  546. int16_t cus = getles16_be(buf, 51);
  547. uint32_t sqrt_a = getleu32_be(buf, 53);
  548. uint16_t toe = getleu16_be(buf, 57);
  549. /* NOTE - Fit interval & AODO not collected */
  550. /* Subframe 3, words 3 to 10 minus parity */
  551. int16_t cic = getles16_be(buf, 60);
  552. int32_t Omega0 = getles32_be(buf, 62);
  553. int16_t cis = getles16_be(buf, 66);
  554. int32_t i0 = getles32_be(buf, 68);
  555. int16_t crc = getles16_be(buf, 72);
  556. int32_t omega = getles32_be(buf, 74);
  557. int32_t Omegadot = getles3224_be(buf, 78);
  558. /* Question: What is the proper way of shifting a signed int 2 bits to
  559. * the right, preserving sign? Answer: integer division by 4. */
  560. int16_t idot = (int16_t) (((getles16_be(buf, 82) & 0xfffc) / 4) |
  561. ((getub(buf, 82) & 80) ? 0xc000 : 0x0000));
  562. session->context->gps_week = (unsigned short)wn;
  563. DTOTS(&session->context->gps_tow, (double)(toc * SF_TOC));
  564. /* leap second? */
  565. GPSD_LOG(LOG_PROG, &session->context->errout,
  566. "Navcom: received packet type 0x81 (Packed Ephemeris Data)\n");
  567. GPSD_LOG(LOG_DATA, &session->context->errout,
  568. "Navcom: PRN: %u, Week: %u, TOW: %s "
  569. "SV clock bias/drift/drift rate: %#19.12E/%#19.12E/%#19.12E\n",
  570. prn,
  571. session->context->gps_week,
  572. timespec_str(&session->context->gps_tow, ts_buf, sizeof(ts_buf)),
  573. ((double)af0) * SF_AF0,
  574. ((double)af1) * SF_AF1, ((double)af2) * SF_AF2);
  575. GPSD_LOG(LOG_DATA, &session->context->errout,
  576. "Navcom: IODE (!AODE): %u Crs: %19.12e, Delta n: %19.12e, M0: %19.12e\n",
  577. iode, (double)crs * SF_CRS,
  578. (double)delta_n * SF_DELTA_N * GPS_PI,
  579. (double)m0 * SF_M0 * GPS_PI);
  580. GPSD_LOG(LOG_DATA, &session->context->errout,
  581. "Navcom: Cuc: %19.12e, Eccentricity: %19.12e, Cus: %19.12e, A^1/2: %19.12e\n",
  582. (double)cuc * SF_CUC, (double)e * SF_E, (double)cus * SF_CUS,
  583. (double)sqrt_a * SF_SQRT_A);
  584. GPSD_LOG(LOG_DATA, &session->context->errout,
  585. "Navcom: TOE: %u, Cic: %19.12e, Omega %19.12e, Cis: %19.12e\n",
  586. toe * SF_TOE, (double)cic * SF_CIC,
  587. (double)Omega0 * SF_OMEGA0 * GPS_PI, (double)cis * SF_CIS);
  588. GPSD_LOG(LOG_DATA, &session->context->errout,
  589. "Navcom: i0: %19.12e, Crc: %19.12e, omega: %19.12e, Omega dot: %19.12e\n",
  590. (double)i0 * SF_I0 * GPS_PI, (double)crc * SF_CRC,
  591. (double)omega * SF_OMEGA * GPS_PI,
  592. (double)Omegadot * SF_OMEGADOT * GPS_PI);
  593. GPSD_LOG(LOG_DATA, &session->context->errout,
  594. "Navcom: IDOT: %19.12e, Codes on L2: 0x%x, GPS Week: %u, L2 P data flag: %x\n",
  595. (double)idot * SF_IDOT * GPS_PI, cl2,
  596. week - (week % 1024) + wn, l2pd);
  597. GPSD_LOG(LOG_DATA, &session->context->errout,
  598. "Navcom: SV accuracy: 0x%x, SV health: 0x%x, TGD: %f, IODC (!AODC): %u\n",
  599. ura, svh, (double)tgd * SF_TGD, iodc);
  600. GPSD_LOG(LOG_DATA, &session->context->errout,
  601. "Navcom: Transmission time: %u\n", tow);
  602. #undef SF_TGD
  603. #undef SF_TOC
  604. #undef SF_AF2
  605. #undef SF_AF1
  606. #undef SF_AF0
  607. #undef SF_CRS
  608. #undef SF_DELTA_N
  609. #undef SF_M0
  610. #undef SF_CUC
  611. #undef SF_E
  612. #undef SF_CUS
  613. #undef SF_SQRT_A
  614. #undef SF_TOE
  615. #undef SF_CIC
  616. #undef SF_OMEGA0
  617. #undef SF_CIS
  618. #undef SF_I0
  619. #undef SF_CRC
  620. #undef SF_OMEGA
  621. #undef SF_OMEGADOT
  622. #undef SF_IDOT
  623. return 0;
  624. }
  625. /* Channel Status */
  626. static gps_mask_t handle_0x86(struct gps_device_t *session)
  627. {
  628. size_t n, i, nsu;
  629. unsigned char *buf = session->lexer.outbuffer + 3;
  630. size_t msg_len = (size_t) getleu16(buf, 1);
  631. uint16_t week = getleu16(buf, 3);
  632. uint32_t tow = getleu32(buf, 5);
  633. uint8_t eng_status = getub(buf, 9);
  634. uint16_t sol_status = getleu16(buf, 10);
  635. uint8_t sats_visible = getub(buf, 12);
  636. //uint8_t sats_tracked = getub(buf, 13);
  637. //uint8_t used_sats = getub(buf, 14);
  638. //uint8_t pdop = getub(buf, 15);
  639. timespec_t ts_tow;
  640. MSTOTS(&ts_tow, tow);
  641. /* Timestamp */
  642. session->gpsdata.skyview_time = gpsd_gpstime_resolv(session,
  643. (unsigned short)week,
  644. ts_tow);
  645. /* Give this driver a single point of truth about DOPs */
  646. //session->gpsdata.dop.pdop = (int)pdop / 10.0;
  647. /* Satellite count */
  648. session->gpsdata.satellites_visible = (int)sats_visible;
  649. /* Fix mode */
  650. switch (sol_status & 0x05) {
  651. case 0x05:
  652. session->gpsdata.status = STATUS_DGPS_FIX;
  653. break;
  654. case 0x01:
  655. session->gpsdata.status = STATUS_FIX;
  656. break;
  657. default:
  658. session->gpsdata.status = STATUS_NO_FIX;
  659. }
  660. GPSD_LOG(LOG_DATA, &session->context->errout,
  661. "Navcom: engine status 0x%x almanac %s time 0x%x pos 0x%x\n",
  662. eng_status & 0x07, ((eng_status & 0x08) ? "valid" : "invalid"),
  663. eng_status & 0x30 >> 4, eng_status & 0xc0 >> 6);
  664. /* Satellite details */
  665. i = nsu = 0;
  666. /* coverity_submit[tainted_data] */
  667. for (n = 17; n < msg_len; n += 14) {
  668. uint8_t prn, ele, ca_snr, p2_snr, log_channel, hw_channel, s, stat;
  669. uint16_t azm, dgps_age;
  670. if (i >= MAXCHANNELS) {
  671. GPSD_LOG(LOG_ERROR, &session->context->errout,
  672. "Navcom: packet type 0x86: too many satellites!\n");
  673. gpsd_zero_satellites(&session->gpsdata);
  674. return 0;
  675. }
  676. prn = getub(buf, n);
  677. /*
  678. * This field is described in the Technical Reference as follows:
  679. *
  680. * Channel Tracking Status:
  681. * B0-B1: C/A tracking status
  682. * B2-B3: P1 tracking status
  683. * B4-B5: P2 tracking status
  684. * 00 Acquisition or reacquisition
  685. * 01 Code loop locked
  686. * 02 Costas loop locked
  687. * 11 Full tracking with aiding and active
  688. * multipath reduction - all data is valid
  689. * B6=1: C/A Bit sync
  690. * B7=1: C/A Frame sync
  691. *
  692. * By observation, the satellite is in use if this status is 0xff.
  693. * But errors here are not very serious, all they can affect is
  694. * the coverance-matrix calculation for error modeling.
  695. */
  696. stat = getub(buf, n + 1);
  697. log_channel = getub(buf, n + 2);
  698. ele = getub(buf, n + 5);
  699. azm = getleu16(buf, n + 6);
  700. ca_snr = getub(buf, n + 8);
  701. p2_snr = getub(buf, n + 10);
  702. dgps_age = getleu16(buf, n + 11);
  703. hw_channel = getub(buf, n + 13);
  704. s = (unsigned char)0;
  705. /* NOTE - In theory, I think one would check for hw channel number to
  706. * see if one is dealing with a GPS or other satellite, but the
  707. * channel numbers reported bear no resemblance to what the spec
  708. * says should be. So I check for the fact that if all three
  709. * values below are zero, one is not interested in this satellite */
  710. if (!(ele == 0 && azm == 0 && dgps_age == 0)) {
  711. session->gpsdata.skyview[i].PRN = (short)prn;
  712. session->gpsdata.skyview[i].elevation = (double)ele;
  713. session->gpsdata.skyview[i].azimuth = (double)azm;
  714. s = session->gpsdata.skyview[i].ss =
  715. (p2_snr ? p2_snr : ca_snr) / 4.0;
  716. session->gpsdata.skyview[i++].used = (stat == 0xff);
  717. if (stat == 0xff)
  718. nsu++;
  719. }
  720. session->gpsdata.satellites_used = (int)nsu;
  721. GPSD_LOG(LOG_DATA, &session->context->errout,
  722. "Navcom: prn = %3u, ele = %02u, azm = %03u, snr = %d (%s), "
  723. "dgps age = %.1fs, log ch = %d, hw ch = 0x%02x\n",
  724. prn, ele, azm, s, (p2_snr ? "P2" : "C/A"),
  725. (double)dgps_age * 0.1, log_channel & 0x3f, hw_channel);
  726. GPSD_LOG(LOG_DATA, &session->context->errout,
  727. "Navcom: sol. valid = %c, clock = %s, pos. = %s, "
  728. "height = %s, err. code = 0x%x\n",
  729. ((sol_status & 0x01) ? 'Y' : 'N'),
  730. ((sol_status & 0x02) ? "stable" : "unstable"),
  731. ((sol_status & 0x04) ? "dgps" : "unaided"),
  732. ((sol_status & 0x08) ? "solved" : "constrained"),
  733. ((sol_status & 0x01) ? 0x00 : sol_status & 0x0f00 >> 8));
  734. }
  735. GPSD_LOG(LOG_DATA, &session->context->errout,
  736. "CS 0x86: visible=%d, used=%d, mask={SATELLITE|STATUS}\n",
  737. session->gpsdata.satellites_visible,
  738. session->gpsdata.satellites_used);
  739. return SATELLITE_SET | STATUS_SET;
  740. }
  741. /* Raw Meas. Data Block */
  742. static gps_mask_t handle_0xb0(struct gps_device_t *session)
  743. {
  744. char ts_buf[TIMESPEC_LEN];
  745. /* L1 wavelength (299792458m/s / 1575420000Hz) */
  746. #define LAMBDA_L1 (.190293672798364880476317426464)
  747. size_t n;
  748. unsigned char *buf = session->lexer.outbuffer + 3;
  749. size_t msg_len = (size_t) getleu16(buf, 1);
  750. uint16_t week = getleu16(buf, 3);
  751. uint32_t tow = getleu32(buf, 5);
  752. uint8_t tm_slew_acc = getub(buf, 9);
  753. uint8_t status = getub(buf, 10);
  754. session->context->gps_week = (unsigned short)week;
  755. MSTOTS(&session->context->gps_tow, tow);
  756. GPSD_LOG(LOG_PROG, &session->context->errout,
  757. "Navcom: received packet type 0xb0 (Raw Meas. Data Block)\n");
  758. GPSD_LOG(LOG_DATA, &session->context->errout,
  759. "Navcom: week = %u, tow = %s "
  760. "time slew accumulator = %u (1/1023mS), status = 0x%02x "
  761. "(%sclock %s - %u blocks follow)\n",
  762. session->context->gps_week,
  763. timespec_str(&session->context->gps_tow, ts_buf, sizeof(ts_buf)),
  764. tm_slew_acc, status,
  765. ((status & 0x80) ? "channel time set - " : ""),
  766. ((status & 0x40) ? "stable" : "not stable"), status & 0x0f);
  767. /* coverity_submit[tainted_data] */
  768. for (n = 11; n < msg_len - 1; n += 16) {
  769. uint8_t sv_status = getub(buf, n);
  770. uint8_t ch_status = getub(buf, n + 1);
  771. uint32_t ca_pseudorange = getleu32(buf, n + 2);
  772. /* integer division by 16 is a sign-preserving right shift of 4 bits */
  773. int32_t l1_phase = getles3224(buf, n + 6) / 16;
  774. uint8_t l1_slips = (uint8_t) (getles3224(buf, n + 6) & 0x0f);
  775. int16_t p1_ca_pseudorange = getles16(buf, n + 9);
  776. int16_t p2_ca_pseudorange = getles16(buf, n + 11);
  777. int32_t l2_phase = getles3224(buf, n + 13) / 16;
  778. uint8_t l2_slips = (uint8_t) (getles3224(buf, n + 13) & 0x0f);
  779. double c1 =
  780. ((sv_status & 0x80) ? (double)ca_pseudorange / 16.0 *
  781. LAMBDA_L1 : NAN);
  782. double l1 =
  783. ((sv_status & 0x80) ? (double)ca_pseudorange / 16.0 +
  784. (double)l1_phase / 256.0 : NAN);
  785. double l2 =
  786. ((sv_status & 0x20)
  787. ? ((double)ca_pseudorange / 16.0 +
  788. (double)p2_ca_pseudorange / 16.0) * (120.0 / 154.0)
  789. + (double)l2_phase / 256.0 : NAN);
  790. double p1 =
  791. ((sv_status & 0x40) ? c1 +
  792. (double)p1_ca_pseudorange / 16.0 * LAMBDA_L1 : NAN);
  793. double p2 =
  794. ((sv_status & 0x20) ? c1 +
  795. (double)p2_ca_pseudorange / 16.0 * LAMBDA_L1 : NAN);
  796. GPSD_LOG(LOG_SPIN, &session->context->errout,
  797. "Navcom: >> sv status = 0x%02x (PRN %u - C/A & L1 %s - P1 %s - P2 & L2 %s)\n",
  798. sv_status, (sv_status & 0x1f),
  799. ((sv_status & 0x80) ? "valid" : "invalid"),
  800. ((sv_status & 0x40) ? "valid" : "invalid"),
  801. ((sv_status & 0x20) ? "valid" : "invalid"));
  802. GPSD_LOG(LOG_SPIN, &session->context->errout,
  803. "Navcom: >>> ch status = 0x%02x "
  804. "(Logical channel: %u - CA C/No: %u dBHz) "
  805. "sL1: %u, sL2: %u\n", ch_status, ch_status & 0x0f,
  806. ((ch_status & 0xf0) >> 4) + 35, l1_slips, l2_slips);
  807. GPSD_LOG(LOG_SPIN, &session->context->errout,
  808. "Navcom: >>> C1: %14.3f, L1: %14.3f, L2: %14.3f, P1: %14.3f, P2: %14.3f\n",
  809. c1, l1, l2, p1, p2);
  810. }
  811. #undef LAMBDA_L1
  812. return 0; /* Raw measurements not yet implemented in gpsd */
  813. }
  814. /* Pseudorange Noise Statistics */
  815. static gps_mask_t handle_0xb5(struct gps_device_t *session)
  816. {
  817. if (sizeof(double) == 8) {
  818. gps_mask_t mask = TIME_SET;
  819. char *buf = (char *)session->lexer.outbuffer + 3;
  820. uint16_t week = getleu16(buf, 3);
  821. uint32_t tow = getleu32(buf, 5);
  822. timespec_t ts_tow;
  823. #ifdef __UNUSED__
  824. double rms = getled64(buf, 9);
  825. /* Reason why it's unused is these figures do not agree
  826. * with those obtained from the PVT report (handle_0xb1).
  827. * The figures from 0xb1 do agree with the values reported
  828. * by Navcom's PC utility */
  829. /* let gpsd_error_model() handle this */
  830. //double ellips_maj = getled64(buf, 17);
  831. //double ellips_min = getled64(buf, 25);
  832. //double ellips_azm = getled64(buf, 33);
  833. double lat_sd = getled64(buf, 41);
  834. double lon_sd = getled64(buf, 49);
  835. double alt_sd = getled64(buf, 57);
  836. double hrms = sqrt(pow(lat_sd, 2) + pow(lon_sd, 2));
  837. /* Navcom doc unclear, this is likely sep? */
  838. session->newdata.sep = rms * 1.96;
  839. session->newdata.eph = hrms * 1.96;
  840. session->newdata.epv = alt_sd * 1.96;
  841. mask |= HERR_SET;
  842. #endif /* __UNUSED__ */
  843. MSTOTS(&ts_tow, tow);
  844. session->newdata.time = gpsd_gpstime_resolv(session,
  845. (unsigned short)week,
  846. ts_tow);
  847. GPSD_LOG(LOG_PROG, &session->context->errout,
  848. "Navcom: received packet type 0xb5 (Pseudorange Noise Statistics)\n");
  849. GPSD_LOG(LOG_DATA, &session->context->errout,
  850. "Navcom: sep = %f\n", session->newdata.sep);
  851. return mask;
  852. } else {
  853. /* Ignore this message block */
  854. if (!session->driver.navcom.warned) {
  855. GPSD_LOG(LOG_WARN, &session->context->errout,
  856. "Navcom: received packet type 0xb5 (Pseudorange "
  857. "Noise Statistics) ignored "
  858. " - sizeof(double) == 64 bits required\n");
  859. session->driver.navcom.warned = true;
  860. }
  861. return 0; /* Block ignored - wrong sizeof(double) */
  862. }
  863. }
  864. /* LBM DSP Status Block */
  865. static gps_mask_t handle_0xd3(struct gps_device_t *session UNUSED)
  866. {
  867. /* This block contains status information about the
  868. * unit's L-band (Inmarsat) module. There is nothing
  869. * interesting in it for our purposes so we do not deal
  870. * with it. This callback is purely to a) stop
  871. * "unrecognised packet" messages appearing in the log
  872. * and b) explain what it is for the curious */
  873. return 0; /* Nothing updated */
  874. }
  875. /* Identification Block */
  876. static gps_mask_t handle_0xae(struct gps_device_t *session)
  877. {
  878. char *engconfstr, *asicstr;
  879. unsigned char *buf = session->lexer.outbuffer + 3;
  880. size_t msg_len = (size_t) getleu16(buf, 1);
  881. uint8_t engconf = getub(buf, 3);
  882. uint8_t asic = getub(buf, 4);
  883. uint8_t swvermaj = getub(buf, 5);
  884. uint8_t swvermin = getub(buf, 6);
  885. uint16_t dcser = getleu16(buf, 7);
  886. uint8_t dcclass = getub(buf, 9);
  887. uint16_t rfcser = getleu16(buf, 10);
  888. uint8_t rfcclass = getub(buf, 12);
  889. uint8_t softtm[17] = "\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0";
  890. uint8_t bootstr[17] = "\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0";
  891. uint8_t ioptm[17] = "\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0";
  892. uint8_t iopvermaj = (uint8_t) 0x00;
  893. uint8_t iopvermin = (uint8_t) 0x00;
  894. uint8_t picver = (uint8_t) 0x00;
  895. uint8_t slsbn = (uint8_t) 0x00;
  896. uint8_t iopsbn = (uint8_t) 0x00;
  897. memcpy(softtm, &buf[13], 16);
  898. memcpy(bootstr, &buf[29], 16);
  899. if (msg_len == 0x0037) { /* No IOP */
  900. slsbn = getub(buf, 53);
  901. } else { /* IOP Present */
  902. iopvermaj = getub(buf, 53);
  903. iopvermin = getub(buf, 54);
  904. memcpy(ioptm, &buf[55], 16);
  905. picver = getub(buf, 71);
  906. slsbn = getub(buf, 72);
  907. iopsbn = getub(buf, 73);
  908. }
  909. switch (engconf) {
  910. case 0x00:
  911. engconfstr = "Unknown/Undefined";
  912. break;
  913. case 0x01:
  914. engconfstr = "NCT 2000 S";
  915. break;
  916. case 0x02:
  917. engconfstr = "NCT 2000 D";
  918. break;
  919. case 0x03:
  920. engconfstr = "Startfire Single";
  921. break;
  922. case 0x04:
  923. engconfstr = "Starfire Dual";
  924. break;
  925. case 0x05:
  926. engconfstr = "Pole Mount RTK (Internal Radio)";
  927. break;
  928. case 0x06:
  929. engconfstr = "Pole Mount GIS (LBM)";
  930. break;
  931. case 0x07:
  932. engconfstr = "Black Box RTK (Internal Radio)";
  933. break;
  934. case 0x08:
  935. engconfstr = "Black Box GIS (LBM)";
  936. break;
  937. case 0x80:
  938. engconfstr = "R100";
  939. break;
  940. case 0x81:
  941. engconfstr = "R200";
  942. break;
  943. case 0x82:
  944. engconfstr = "R210";
  945. break;
  946. case 0x83:
  947. engconfstr = "R300";
  948. break;
  949. case 0x84:
  950. engconfstr = "R310";
  951. break;
  952. default:
  953. engconfstr = "?";
  954. }
  955. switch (asic) {
  956. case 0x01:
  957. asicstr = "A-ASIC";
  958. break;
  959. case 0x02:
  960. asicstr = "B-ASIC";
  961. break;
  962. case 0x03:
  963. asicstr = "C-ASIC";
  964. break;
  965. case 0x04:
  966. asicstr = "M-ASIC";
  967. break;
  968. default:
  969. asicstr = "?";
  970. }
  971. GPSD_LOG(LOG_PROG, &session->context->errout,
  972. "Navcom: received packet type 0xae (Identification Block)\n");
  973. if (msg_len == 0x0037) {
  974. GPSD_LOG(LOG_INF, &session->context->errout, "Navcom: ID Data: "
  975. "%s %s Ver. %u.%u.%u, DC S/N: %u.%u, RF S/N: %u.%u, "
  976. "Build ID: %s, Boot software: %s\n",
  977. engconfstr, asicstr, swvermaj, swvermin, slsbn, dcser,
  978. dcclass, rfcser, rfcclass, softtm, bootstr);
  979. } else {
  980. GPSD_LOG(LOG_INF, &session->context->errout, "Navcom: ID Data: "
  981. "%s %s Ver. %u.%u.%u, DC S/N: %u.%u, RF S/N: %u.%u, "
  982. "Build ID: %s, Boot software: %s, "
  983. "IOP Ver.: %u.%u.%u, PIC: %u, IOP Build ID: %s\n",
  984. engconfstr, asicstr, swvermaj, swvermin, slsbn, dcser,
  985. dcclass, rfcser, rfcclass, softtm, bootstr, iopvermaj,
  986. iopvermin, iopsbn, picver, ioptm);
  987. }
  988. (void)snprintf(session->subtype, sizeof(session->subtype),
  989. "%s %s Ver. %u.%u.%u S/N %u.%u %u.%u",
  990. engconfstr, asicstr, swvermaj, swvermin, slsbn, dcser,
  991. dcclass, rfcser, rfcclass);
  992. return DEVICEID_SET;
  993. }
  994. /* Clock Drift and Offset */
  995. static gps_mask_t handle_0xef(struct gps_device_t *session)
  996. {
  997. unsigned char *buf = session->lexer.outbuffer + 3;
  998. //uint16_t week = getleu16(buf, 3);
  999. //uint32_t tow = getleu32(buf, 5);
  1000. int8_t osc_temp = getsb(buf, 9);
  1001. uint8_t nav_status = getub(buf, 10);
  1002. double nav_clock_offset;
  1003. float nav_clock_drift;
  1004. float osc_filter_drift_est;
  1005. char ts_buf[TIMESPEC_LEN];
  1006. int32_t time_slew = (int32_t) getles32(buf, 27);
  1007. if (sizeof(double) == 8) {
  1008. nav_clock_offset = getled64((char *)buf, 11);
  1009. } else {
  1010. nav_clock_offset = NAN;
  1011. }
  1012. if (sizeof(float) == 4) {
  1013. nav_clock_drift = getlef32((char *)buf, 19);
  1014. osc_filter_drift_est = getlef32((char *)buf, 23);
  1015. } else {
  1016. nav_clock_drift = NAN;
  1017. osc_filter_drift_est = NAN;
  1018. }
  1019. GPSD_LOG(LOG_DATA, &session->context->errout,
  1020. "Navcom: oscillator temp. = %d, nav. status = 0x%02x, "
  1021. "nav. clock offset = %f, nav. clock drift = %f, "
  1022. "osc. filter drift est. = %f, acc.time slew value = %d\n",
  1023. osc_temp, nav_status, nav_clock_offset, nav_clock_drift,
  1024. osc_filter_drift_est, time_slew);
  1025. GPSD_LOG(LOG_DATA, &session->context->errout,
  1026. "CDO 0xef: time=%s mask={TIME}\n",
  1027. timespec_str(&session->newdata.time, ts_buf, sizeof(ts_buf)));
  1028. return 0;
  1029. }
  1030. gps_mask_t navcom_parse(struct gps_device_t * session, unsigned char *buf,
  1031. size_t len)
  1032. {
  1033. unsigned char cmd_id;
  1034. unsigned int msg_len;
  1035. if (len == 0)
  1036. return 0;
  1037. cmd_id = (unsigned char)getub(buf, 3);
  1038. //payload = &buf[6];
  1039. msg_len = (unsigned int) getleu16(buf, 4);
  1040. GPSD_LOG(LOG_RAW, &session->context->errout,
  1041. "Navcom: packet type 0x%02x\n", cmd_id);
  1042. session->cycle_end_reliable = true;
  1043. switch (cmd_id) {
  1044. case 0x06:
  1045. return handle_0x06(session);
  1046. case 0x15:
  1047. return handle_0x15(session);
  1048. case 0x81:
  1049. return handle_0x81(session);
  1050. case 0x83:
  1051. return handle_0x83(session);
  1052. case 0x86:
  1053. return handle_0x86(session);
  1054. case 0xae:
  1055. return handle_0xae(session);
  1056. case 0xb0:
  1057. return handle_0xb0(session);
  1058. case 0xb1:
  1059. return handle_0xb1(session) | (CLEAR_IS | REPORT_IS);
  1060. case 0xb5:
  1061. return handle_0xb5(session);
  1062. case 0xd3:
  1063. return handle_0xd3(session);
  1064. case 0xef:
  1065. return handle_0xef(session);
  1066. default:
  1067. GPSD_LOG(LOG_PROG, &session->context->errout,
  1068. "Navcom: received packet type 0x%02x, length %d - "
  1069. "unknown or unimplemented\n",
  1070. cmd_id, msg_len);
  1071. return 0;
  1072. }
  1073. }
  1074. static gps_mask_t navcom_parse_input(struct gps_device_t *session)
  1075. {
  1076. if (session->lexer.type == NAVCOM_PACKET) {
  1077. return navcom_parse(session, session->lexer.outbuffer,
  1078. session->lexer.outbuflen);
  1079. #ifdef NMEA0183_ENABLE
  1080. } else if (session->lexer.type == NMEA_PACKET) {
  1081. return nmea_parse((char *)session->lexer.outbuffer, session);;
  1082. #endif /* NMEA0183_ENABLE */
  1083. } else
  1084. return 0;
  1085. }
  1086. #ifdef CONTROLSEND_ENABLE
  1087. static ssize_t navcom_control_send(struct gps_device_t *session,
  1088. char *buf, size_t len)
  1089. {
  1090. putbyte(session->msgbuf, 0, 0x02);
  1091. putbyte(session->msgbuf, 1, 0x99);
  1092. putbyte(session->msgbuf, 2, 0x66);
  1093. putbyte(session->msgbuf, 3, buf[0]); /* Cmd ID */
  1094. putle16(session->msgbuf, 4, len + 4); /* Length */
  1095. memcpy(session->msgbuf, buf + 6, len - 1);
  1096. putbyte(session->msgbuf, 6 + len,
  1097. checksum((unsigned char *)session->msgbuf + 3, len + 5));
  1098. putbyte(session->msgbuf, 7 + len, 0x03);
  1099. session->msgbuflen = len + 9;
  1100. return gpsd_write(session, session->msgbuf, session->msgbuflen);
  1101. }
  1102. #endif /* CONTROLSEND_ENABLE */
  1103. #ifdef RECONFIGURE_ENABLE
  1104. static bool navcom_speed(struct gps_device_t *session,
  1105. speed_t speed, char parity, int stopbits)
  1106. {
  1107. /* parity and stopbit switching aren't implemented */
  1108. if (parity != session->gpsdata.dev.parity
  1109. || stopbits != (int)session->gpsdata.dev.parity) {
  1110. return false;
  1111. } else {
  1112. uint8_t port, port_selection;
  1113. uint8_t baud;
  1114. if (session->driver.navcom.physical_port == (uint8_t) 0xFF) {
  1115. /* We still don't know which port we're connected to */
  1116. return false;
  1117. }
  1118. switch (speed) {
  1119. /* NOTE - The spec says that certain baud combinations
  1120. * on ports A and B are not allowed, those are
  1121. * 1200/115200, 2400/57600, and 2400/115200.
  1122. * To try and minimise the possibility of those
  1123. * occurring, we do not allow baud rates below
  1124. * 4800. We could also disallow 57600 and 115200
  1125. * to totally prevent this, but I do not consider
  1126. * that reasonable. Finding which baud speed the
  1127. * other port is set at would also be too much
  1128. * trouble, so we do not do it. */
  1129. case 4800:
  1130. baud = 0x04;
  1131. break;
  1132. case 9600:
  1133. baud = 0x06;
  1134. break;
  1135. case 19200:
  1136. baud = 0x08;
  1137. break;
  1138. case 38400:
  1139. baud = 0x0a;
  1140. break;
  1141. case 57600:
  1142. baud = 0x0c;
  1143. break;
  1144. case 115200:
  1145. baud = 0x0e;
  1146. break;
  1147. default:
  1148. /* Unsupported speed */
  1149. return false;
  1150. }
  1151. /* Proceed to construct our message */
  1152. port = session->driver.navcom.physical_port;
  1153. port_selection = (port ? port : (uint8_t) 0xff) | baud;
  1154. /* Send it off */
  1155. navcom_cmd_0x11(session, port_selection);
  1156. /* And cheekily return true, even though we have
  1157. * no way to know if the speed change succeeded
  1158. * until and if we receive an ACK (message 0x06),
  1159. * which will be at the new baud speed if the
  1160. * command was successful. Bottom line, the client
  1161. * should requery gpsd to see if the new speed is
  1162. * different than the old one */
  1163. return true;
  1164. }
  1165. }
  1166. #endif /* RECONFIGURE_ENABLE */
  1167. /* this is everything we export */
  1168. /* *INDENT-OFF* */
  1169. const struct gps_type_t driver_navcom =
  1170. {
  1171. .type_name = "Navcom NCT", /* full name of type */
  1172. .packet_type = NAVCOM_PACKET, /* lexer packet type */
  1173. .flags = DRIVER_STICKY, /* remember this */
  1174. .trigger = NULL, /* none */
  1175. .channels = NAVCOM_CHANNELS, /* 12 L1 + 12 L2 + 2 Inmarsat L-Band */
  1176. .probe_detect = NULL, /* no probe */
  1177. .get_packet = generic_get, /* use generic one */
  1178. .parse_packet = navcom_parse_input, /* parse message packets */
  1179. .rtcm_writer = gpsd_write, /* send RTCM data straight */
  1180. .init_query = NULL, /* non-perturbing query */
  1181. .event_hook = navcom_event_hook, /* lifetime event handler */
  1182. #ifdef RECONFIGURE_ENABLE
  1183. .speed_switcher = navcom_speed, /* we do change baud rates */
  1184. .mode_switcher = NULL, /* there is not a mode switcher */
  1185. .rate_switcher = NULL, /* no sample-rate switcher */
  1186. .min_cycle.tv_sec = 1, /* not relevant, no rate switch */
  1187. .min_cycle.tv_nsec = 0, /* not relevant, no rate switch */
  1188. #endif /* RECONFIGURE_ENABLE */
  1189. #ifdef CONTROLSEND_ENABLE
  1190. .control_send = navcom_control_send, /* how to send a control string */
  1191. #endif /* CONTROLSEND_ENABLE */
  1192. .time_offset = NULL, /* no method for NTP fudge factor */
  1193. };
  1194. /* *INDENT-ON* */
  1195. #endif /* defined(NAVCOM_ENABLE) */