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.
 
 
 
 
 
 

2574 lines
88 KiB

  1. /*
  2. * UBX driver. All capabilities are common to Antaris4 and u-blox 6.
  3. * Reference manuals are at
  4. * http://www.u-blox.com/en/download/documents-a-resources/u-blox-6-gps-modules-resources.html
  5. *
  6. * updated for u-blox 8
  7. * http://www.ublox.com/images/downloads/Product_Docs/u-bloxM8_ReceiverDescriptionProtocolSpec_%28UBX-13003221%29_Public.pdf
  8. *
  9. * Week counters are not limited to 10 bits. It's unknown what
  10. * the firmware is doing to disambiguate them, if anything; it might just
  11. * be adding a fixed offset based on a hidden epoch value, in which case
  12. * unhappy things will occur on the next rollover.
  13. *
  14. * For the Antaris 4, the default leap-second offset (before getting one from
  15. * the sats, one presumes) is 0sec; for the u-blox 6 it's 15sec.
  16. *
  17. * This file is Copyright (c) 2010-2019 by the GPSD project
  18. * SPDX-License-Identifier: BSD-2-clause
  19. *
  20. */
  21. #include "gpsd_config.h" /* must be before all includes */
  22. #include <assert.h>
  23. #include <math.h>
  24. #include <stdbool.h>
  25. #include <stdio.h>
  26. #include <stdlib.h>
  27. #include <string.h>
  28. #include <unistd.h>
  29. #include "gpsd.h"
  30. #if defined(UBLOX_ENABLE) && defined(BINARY_ENABLE)
  31. #include "driver_ubx.h"
  32. #include "bits.h"
  33. #include "timespec.h"
  34. /*
  35. * A ubx packet looks like this:
  36. * leader: 0xb5 0x62
  37. * message class: 1 byte
  38. * message type: 1 byte
  39. * length of payload: 2 bytes
  40. * payload: variable length
  41. * checksum: 2 bytes
  42. *
  43. * see also the FV25 and UBX documents on reference.html
  44. */
  45. #define UBX_PREFIX_LEN 6
  46. #define UBX_CLASS_OFFSET 2
  47. #define UBX_TYPE_OFFSET 3
  48. /* because we hates magic numbers forever */
  49. #define USART1_ID 1
  50. #define USART2_ID 2
  51. #define USB_ID 3
  52. #define UBX_PROTOCOL_MASK 0x01
  53. #define NMEA_PROTOCOL_MASK 0x02
  54. #define RTCM_PROTOCOL_MASK 0x04
  55. #define UBX_CFG_LEN 20
  56. #define outProtoMask 14
  57. static gps_mask_t ubx_parse(struct gps_device_t *session, unsigned char *buf,
  58. size_t len);
  59. static gps_mask_t ubx_msg_nav_eoe(struct gps_device_t *session,
  60. unsigned char *buf, size_t data_len);
  61. static gps_mask_t ubx_msg_nav_dop(struct gps_device_t *session,
  62. unsigned char *buf, size_t data_len);
  63. static void ubx_msg_inf(struct gps_device_t *session, unsigned char *buf,
  64. size_t data_len);
  65. static gps_mask_t ubx_msg_nav_posecef(struct gps_device_t *session,
  66. unsigned char *buf, size_t data_len);
  67. static gps_mask_t ubx_msg_nav_pvt(struct gps_device_t *session,
  68. unsigned char *buf, size_t data_len);
  69. static void ubx_msg_mon_ver(struct gps_device_t *session,
  70. unsigned char *buf, size_t data_len);
  71. static gps_mask_t ubx_msg_nav_sat(struct gps_device_t *session,
  72. unsigned char *buf, size_t data_len);
  73. static gps_mask_t ubx_msg_nav_sol(struct gps_device_t *session,
  74. unsigned char *buf, size_t data_len);
  75. static gps_mask_t ubx_msg_nav_svinfo(struct gps_device_t *session,
  76. unsigned char *buf, size_t data_len);
  77. static gps_mask_t ubx_msg_nav_timegps(struct gps_device_t *session,
  78. unsigned char *buf, size_t data_len);
  79. static gps_mask_t ubx_msg_nav_velecef(struct gps_device_t *session,
  80. unsigned char *buf, size_t data_len);
  81. static void ubx_msg_sbas(struct gps_device_t *session, unsigned char *buf,
  82. size_t data_len);
  83. static gps_mask_t ubx_msg_tim_tp(struct gps_device_t *session,
  84. unsigned char *buf, size_t data_len);
  85. #ifdef RECONFIGURE_ENABLE
  86. static void ubx_mode(struct gps_device_t *session, int mode);
  87. #endif /* RECONFIGURE_ENABLE */
  88. /* make up an NMEA 4.0 (extended) PRN based on gnssId:svId,
  89. * using Appendix A from * u-blox ZED-F9P Interface Description
  90. *
  91. * Return PRN, or zero for error
  92. */
  93. static short ubx2_to_prn(int gnssId, int svId)
  94. {
  95. short nmea_PRN;
  96. if (1 > svId) {
  97. /* skip 0 svId */
  98. return 0;
  99. }
  100. switch (gnssId) {
  101. case 0:
  102. /* GPS, 1-32 maps to 1-32 */
  103. if (32 < svId) {
  104. /* skip bad svId */
  105. return 0;
  106. }
  107. nmea_PRN = svId;
  108. break;
  109. case 1:
  110. /* SBAS, 120..151, 152..158 maps to 33..64, 152..158 */
  111. if (120 > svId) {
  112. /* Huh? */
  113. return 0;
  114. } else if (151 >= svId) {
  115. nmea_PRN = svId - 87;
  116. } else if (158 >= svId) {
  117. nmea_PRN = svId;
  118. } else {
  119. /* Huh? */
  120. return 0;
  121. }
  122. break;
  123. case 2:
  124. /* Galileo, 1..36 -> 301-336 */
  125. /* Galileo, 211..246 -> 301-336 */
  126. if (36 >= svId) {
  127. nmea_PRN = svId + 300;
  128. } else if (211 > svId) {
  129. /* skip bad svId */
  130. return 0;
  131. } else if (246 >= svId) {
  132. nmea_PRN = svId + 90;
  133. } else {
  134. /* skip bad svId */
  135. return 0;
  136. }
  137. break;
  138. case 3:
  139. /* BeiDou, 1..37 -> to 401-437 */
  140. /* BeiDou, 159..163,33..64 -> to 401-437 */
  141. if (37 >= svId) {
  142. nmea_PRN = svId + 400;
  143. } else {
  144. /* skip bad svId */
  145. return 0;
  146. }
  147. break;
  148. case 4:
  149. /* IMES, 1-10 -> to 173-182, per u-blox 8/NMEA 4.0 extended */
  150. if (10 < svId) {
  151. /* skip bad svId */
  152. return 0;
  153. }
  154. nmea_PRN = svId + 172;
  155. break;
  156. case 5:
  157. /* QZSS, 1-5 maps to 193-197 */
  158. /* ZED-F9T also see 198 and 199 */
  159. if (7 < svId) {
  160. /* skip bad svId */
  161. return 0;
  162. }
  163. nmea_PRN = svId + 192;
  164. break;
  165. case 6:
  166. /* GLONASS, 1-32 maps to 65-96 */
  167. if (32 < svId) {
  168. /* skip bad svId */
  169. /* 255 == tracked, but unidentified, skip */
  170. return 0;
  171. }
  172. nmea_PRN = svId + 64;
  173. break;
  174. default:
  175. /* Huh? */
  176. return 0;
  177. }
  178. return nmea_PRN;
  179. }
  180. /* Convert a ubx PRN to an NMEA 4.0 (extended) PRN and ubx gnssid, svid
  181. *
  182. * return 0 on fail
  183. */
  184. static short ubx_to_prn(int ubx_PRN, unsigned char *gnssId,
  185. unsigned char *svId)
  186. {
  187. *gnssId = 0;
  188. *svId = 0;
  189. if (1 > ubx_PRN) {
  190. /* skip 0 PRN */
  191. return 0;
  192. } else if (32 >= ubx_PRN) {
  193. /* GPS 1..32 -> 1..32 */
  194. *gnssId = 0;
  195. *svId = ubx_PRN;
  196. } else if (64 >= ubx_PRN) {
  197. /* BeiDou, 159..163,33..64 -> 1..5,6..37 */
  198. *gnssId = 3;
  199. *svId = ubx_PRN - 27;
  200. } else if (96 >= ubx_PRN) {
  201. /* GLONASS 65..96 -> 1..32 */
  202. *gnssId = 6;
  203. *svId = ubx_PRN - 64;
  204. } else if (120 > ubx_PRN) {
  205. /* Huh? */
  206. return 0;
  207. } else if (158 >= ubx_PRN) {
  208. /* SBAS 120..158 -> 120..158 */
  209. *gnssId = 1;
  210. *svId = ubx_PRN;
  211. } else if (163 >= ubx_PRN) {
  212. /* BeiDou, 159..163 -> 1..5 */
  213. *gnssId = 3;
  214. *svId = ubx_PRN - 158;
  215. } else if (173 > ubx_PRN) {
  216. /* Huh? */
  217. return 0;
  218. } else if (182 >= ubx_PRN) {
  219. /* IMES 173..182 -> 1..5, in u-blox 8, bot u-blox 9 */
  220. *gnssId = 4;
  221. *svId = ubx_PRN - 172;
  222. } else if (193 > ubx_PRN) {
  223. /* Huh? */
  224. return 0;
  225. } else if (199 >= ubx_PRN) {
  226. /* QZSS 193..197 -> 1..5 */
  227. /* ZED-F9T also see 198 and 199 */
  228. *gnssId = 5;
  229. *svId = ubx_PRN - 192;
  230. } else if (211 > ubx_PRN) {
  231. /* Huh? */
  232. return 0;
  233. } else if (246 >= ubx_PRN) {
  234. /* Galileo 211..246 -> 1..36 */
  235. *gnssId = 2;
  236. *svId = ubx_PRN - 210;
  237. } else {
  238. /* greater than 246
  239. * GLONASS (255), unused, or other unknown */
  240. return 0;
  241. }
  242. return ubx2_to_prn(*gnssId, *svId);
  243. }
  244. /**
  245. * Receiver/Software Version
  246. * UBX-MON-VER
  247. *
  248. * sadly more info than fits in session->swtype for now.
  249. * so squish the data hard.
  250. */
  251. static void
  252. ubx_msg_mon_ver(struct gps_device_t *session, unsigned char *buf,
  253. size_t data_len)
  254. {
  255. size_t n = 0; /* extended info counter */
  256. char obuf[128]; /* temp version string buffer */
  257. char *cptr;
  258. if (40 > data_len) {
  259. GPSD_LOG(LOG_WARN, &session->context->errout,
  260. "Runt MON-VER message, payload len %zd", data_len);
  261. return;
  262. }
  263. /* save SW and HW Version as subtype */
  264. (void)snprintf(obuf, sizeof(obuf),
  265. "SW %.30s,HW %.10s",
  266. (char *)&buf[UBX_MESSAGE_DATA_OFFSET + 0],
  267. (char *)&buf[UBX_MESSAGE_DATA_OFFSET + 30]);
  268. /* save what we can */
  269. (void)strlcpy(session->subtype, obuf, sizeof(session->subtype));
  270. /* find PROTVER= */
  271. cptr = strstr(session->subtype, "PROTVER=");
  272. if (NULL != cptr) {
  273. int protver = atoi(cptr + 8);
  274. if (9 < protver) {
  275. /* protver 10, u-blox 5, is the oldest we know */
  276. session->driver.ubx.protver = protver;
  277. }
  278. }
  279. obuf[0] = '\0';
  280. /* get n number of Extended info strings. what is max n? */
  281. for ( n = 0; ; n++ ) {
  282. size_t start_of_str = UBX_MESSAGE_DATA_OFFSET + 40 + (30 * n);
  283. if ( (start_of_str + 2 ) > data_len ) {
  284. /* last one can be shorter than 30 */
  285. /* no more data */
  286. break;
  287. }
  288. (void)strlcat(obuf, ",", sizeof(obuf));
  289. (void)strlcat(obuf, (char *)&buf[start_of_str], sizeof(obuf));
  290. }
  291. /* save what we can */
  292. (void)strlcpy(session->subtype1, obuf, sizeof(session->subtype1));
  293. /* output SW and HW Version at LOG_INFO */
  294. GPSD_LOG(LOG_INF, &session->context->errout,
  295. "UBX-MON-VER: %s %s\n",
  296. session->subtype, session->subtype1);
  297. }
  298. /*
  299. * UBX-NAV-HPPOSECEF - High Precision Position Solution in ECEF
  300. */
  301. static gps_mask_t
  302. ubx_msg_nav_hpposecef(struct gps_device_t *session, unsigned char *buf,
  303. size_t data_len)
  304. {
  305. gps_mask_t mask = ECEF_SET;
  306. int version;
  307. if (28 > data_len) {
  308. GPSD_LOG(LOG_WARN, &session->context->errout,
  309. "Runt UBX-NAV-HPPOSECEF message, payload len %zd", data_len);
  310. return 0;
  311. }
  312. version = getub(buf, 0);
  313. session->driver.ubx.iTOW = getleu32(buf, 4);
  314. session->newdata.ecef.x = ((getles32(buf, 8) +
  315. (getsb(buf, 20) * 1e-2)) * 1e-2);
  316. session->newdata.ecef.y = ((getles32(buf, 12) +
  317. (getsb(buf, 21) * 1e-2)) * 1e-2);
  318. session->newdata.ecef.z = ((getles32(buf, 16) +
  319. (getsb(buf, 22) * 1e-2)) * 1e-2);
  320. session->newdata.ecef.pAcc = getleu32(buf, 24) * 1e-4;
  321. /* (long long) cast for 32-bit compat */
  322. GPSD_LOG(LOG_DATA, &session->context->errout,
  323. "UBX-NAV-HPPOSECEF: version %d iTOW=%lld ECEF x=%.4f y=%.4f z=%.4f "
  324. "pAcc=%.4f\n",
  325. version,
  326. (long long)session->driver.ubx.iTOW,
  327. session->newdata.ecef.x,
  328. session->newdata.ecef.y,
  329. session->newdata.ecef.z,
  330. session->newdata.ecef.pAcc);
  331. return mask;
  332. }
  333. /**
  334. * High Precision Geodetic Position Solution
  335. * UBX-NAV-HPPOSLLH, Class 1, ID x14
  336. *
  337. * No mode, so limited usefulness.
  338. */
  339. static gps_mask_t
  340. ubx_msg_nav_hpposllh(struct gps_device_t *session, unsigned char *buf,
  341. size_t data_len)
  342. {
  343. int version;
  344. gps_mask_t mask = 0;
  345. if (36 > data_len) {
  346. GPSD_LOG(LOG_WARN, &session->context->errout,
  347. "Runt NAV-HPPOSLLH message, payload len %zd", data_len);
  348. return mask;
  349. }
  350. mask = ONLINE_SET | HERR_SET | VERR_SET | LATLON_SET | ALTITUDE_SET;
  351. version = getub(buf, 0);
  352. session->driver.ubx.iTOW = getles32(buf, 4);
  353. session->newdata.longitude = (1e-7 * (getles32(buf, 8) +
  354. (getsb(buf, 24) * 1e-2)));
  355. session->newdata.latitude = (1e-7 * (getles32(buf, 12) + \
  356. (getsb(buf, 25) * 1e-2)));
  357. /* altitude WGS84 */
  358. session->newdata.altHAE = (1e-3 * (getles32(buf, 16) + \
  359. (getsb(buf, 26) * 1e-2)));
  360. /* altitude MSL */
  361. session->newdata.altMSL = (1e-3 * (getles32(buf, 20) + \
  362. (getsb(buf, 27) * 1e-2)));
  363. /* Let gpsd_error_model() deal with geoid_sep */
  364. /* Horizontal accuracy estimate in .1 mm, unknown est type */
  365. session->newdata.eph = getleu32(buf, 28) * 1e-4;
  366. /* Vertical accuracy estimate in .1 mm, unknown est type */
  367. session->newdata.epv = getleu32(buf, 32) * 1e-4;
  368. GPSD_LOG(LOG_DATA, &session->context->errout,
  369. "UBX-NAV-HPPOSLLH: version %d iTOW=%lld lat=%.4f lon=%.4f "
  370. "altHAE=%.4f\n",
  371. version,
  372. (long long)session->driver.ubx.iTOW,
  373. session->newdata.latitude,
  374. session->newdata.longitude,
  375. session->newdata.altHAE);
  376. return mask;
  377. }
  378. /*
  379. * Navigation Position ECEF message
  380. */
  381. static gps_mask_t
  382. ubx_msg_nav_posecef(struct gps_device_t *session, unsigned char *buf,
  383. size_t data_len)
  384. {
  385. gps_mask_t mask = ECEF_SET;
  386. if (20 > data_len) {
  387. GPSD_LOG(LOG_WARN, &session->context->errout,
  388. "Runt UBX-NAV-POSECEF message, payload len %zd", data_len);
  389. return 0;
  390. }
  391. session->driver.ubx.iTOW = getleu32(buf, 0);
  392. /* all in cm */
  393. session->newdata.ecef.x = getles32(buf, 4) * 1e-2;
  394. session->newdata.ecef.y = getles32(buf, 8) * 1e-2;
  395. session->newdata.ecef.z = getles32(buf, 12) * 1e-2;
  396. session->newdata.ecef.pAcc = getleu32(buf, 16) * 1e-2;
  397. /* (long long) cast for 32-bit compat */
  398. GPSD_LOG(LOG_DATA, &session->context->errout,
  399. "UBX-NAV-POSECEF: iTOW=%lld ECEF x=%.2f y=%.2f z=%.2f pAcc=%.2f\n",
  400. (long long)session->driver.ubx.iTOW,
  401. session->newdata.ecef.x,
  402. session->newdata.ecef.y,
  403. session->newdata.ecef.z,
  404. session->newdata.ecef.pAcc);
  405. return mask;
  406. }
  407. /**
  408. * Navigation Position Velocity Time solution message
  409. * UBX-NAV-PVT Class 1, ID 7
  410. *
  411. * Not in u-blox 5 or 6, present in u-blox 7
  412. */
  413. static gps_mask_t
  414. ubx_msg_nav_pvt(struct gps_device_t *session, unsigned char *buf,
  415. size_t data_len)
  416. {
  417. uint8_t valid;
  418. uint8_t flags;
  419. uint8_t fixType;
  420. struct tm unpacked_date;
  421. int *status = &session->gpsdata.status;
  422. int *mode = &session->newdata.mode;
  423. gps_mask_t mask = 0;
  424. char ts_buf[TIMESPEC_LEN];
  425. /* u-blox 6 and 7 are 84 bytes, u-blox 8 and 9 are 92 bytes */
  426. if (84 > data_len) {
  427. GPSD_LOG(LOG_WARN, &session->context->errout,
  428. "Runt NAV-PVT message, payload len %zd", data_len);
  429. return 0;
  430. }
  431. session->driver.ubx.iTOW = getleu32(buf, 0);
  432. valid = (unsigned int)getub(buf, 11);
  433. fixType = (unsigned char)getub(buf, 20);
  434. flags = (unsigned int)getub(buf, 21);
  435. switch (fixType) {
  436. case UBX_MODE_TMONLY:
  437. // 5 - Surveyed-in, so a precise 3D.
  438. *mode = MODE_3D;
  439. *status = STATUS_TIME;
  440. mask |= STATUS_SET | MODE_SET;
  441. break;
  442. case UBX_MODE_3D:
  443. // 3
  444. // FALLTHROUGH
  445. case UBX_MODE_GPSDR:
  446. // 4
  447. if (*mode != MODE_3D) {
  448. *mode = MODE_3D;
  449. mask |= MODE_SET;
  450. }
  451. if ((flags & UBX_NAV_PVT_FLAG_DGPS) == UBX_NAV_PVT_FLAG_DGPS) {
  452. if (*status != STATUS_DGPS_FIX) {
  453. *status = STATUS_DGPS_FIX;
  454. mask |= STATUS_SET;
  455. }
  456. } else {
  457. if (*status != STATUS_FIX) {
  458. *status = STATUS_FIX;
  459. mask |= STATUS_SET;
  460. }
  461. }
  462. mask |= LATLON_SET;
  463. break;
  464. case UBX_MODE_2D:
  465. // 2
  466. // FALLTHROUGH
  467. case UBX_MODE_DR: /* consider this too as 2D */
  468. // 1
  469. if (*mode != MODE_2D) {
  470. *mode = MODE_2D;
  471. mask |= MODE_SET;
  472. };
  473. if (*status != STATUS_FIX) {
  474. *status = STATUS_FIX;
  475. mask |= STATUS_SET;
  476. }
  477. mask |= LATLON_SET | SPEED_SET;
  478. break;
  479. case UBX_MODE_NOFIX:
  480. // 0
  481. // FALLTHROUGH
  482. default:
  483. // huh?
  484. if (*mode != MODE_NO_FIX) {
  485. *mode = MODE_NO_FIX;
  486. mask |= MODE_SET;
  487. };
  488. if (*status != STATUS_NO_FIX) {
  489. *status = STATUS_NO_FIX;
  490. mask |= STATUS_SET;
  491. }
  492. break;
  493. }
  494. if ((valid & UBX_NAV_PVT_VALID_DATE_TIME) == UBX_NAV_PVT_VALID_DATE_TIME) {
  495. unpacked_date.tm_year = (uint16_t)getleu16(buf, 4) - 1900;
  496. unpacked_date.tm_mon = (uint8_t)getub(buf, 6) - 1;
  497. unpacked_date.tm_mday = (uint8_t)getub(buf, 7);
  498. unpacked_date.tm_hour = (uint8_t)getub(buf, 8);
  499. unpacked_date.tm_min = (uint8_t)getub(buf, 9);
  500. unpacked_date.tm_sec = (uint8_t)getub(buf, 10);
  501. unpacked_date.tm_isdst = 0;
  502. unpacked_date.tm_wday = 0;
  503. unpacked_date.tm_yday = 0;
  504. session->newdata.time.tv_sec = mkgmtime(&unpacked_date);
  505. /* field 16, nano, can be negative! So normalize */
  506. session->newdata.time.tv_nsec = getles32(buf, 16);
  507. TS_NORM(&session->newdata.time);
  508. mask |= TIME_SET | NTPTIME_IS | GOODTIME_IS;
  509. }
  510. session->newdata.longitude = 1e-7 * getles32(buf, 24);
  511. session->newdata.latitude = 1e-7 * getles32(buf, 28);
  512. /* altitude WGS84 */
  513. session->newdata.altHAE = 1e-3 * getles32(buf, 32);
  514. /* altitude MSL */
  515. session->newdata.altMSL = 1e-3 * getles32(buf, 36);
  516. /* Let gpsd_error_model() deal with geoid_sep */
  517. session->newdata.speed = 1e-3 * (int32_t)getles32(buf, 60);
  518. /* u-blox calls this Heading of motion (2-D) */
  519. session->newdata.track = 1e-5 * (int32_t)getles32(buf, 64);
  520. mask |= LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET;
  521. /* u-blox does not document the basis for the following "accuracy"
  522. * estimates. Maybe CEP(50), one sigma, two sigma, CEP(99), etc. */
  523. /* Horizontal Accuracy estimate, in mm */
  524. session->newdata.eph = (double)(getles32(buf, 40) / 1000.0);
  525. /* Vertical Accuracy estimate, in mm */
  526. session->newdata.epv = (double)(getles32(buf, 44) / 1000.0);
  527. /* Speed Accuracy estimate, in mm/s */
  528. session->newdata.eps = (double)(getles32(buf, 68) / 1000.0);
  529. /* let gpsd_error_model() do the rest */
  530. mask |= HERR_SET | SPEEDERR_SET | VERR_SET;
  531. GPSD_LOG(LOG_DATA, &session->context->errout,
  532. "NAV-PVT: flags=%02x time=%s lat=%.2f lon=%.2f altHAE=%.2f "
  533. "track=%.2f speed=%.2f climb=%.2f mode=%d status=%d used=%d\n",
  534. flags,
  535. timespec_str(&session->newdata.time, ts_buf, sizeof(ts_buf)),
  536. session->newdata.latitude,
  537. session->newdata.longitude,
  538. session->newdata.altHAE,
  539. session->newdata.track,
  540. session->newdata.speed,
  541. session->newdata.climb,
  542. session->newdata.mode,
  543. session->gpsdata.status,
  544. session->gpsdata.satellites_used);
  545. if (92 <= data_len) {
  546. /* u-blox 8 and 9 extended */
  547. double magDec = NAN;
  548. double magAcc = NAN;
  549. #ifdef __UNUSED
  550. if (flags & UBX_NAV_PVT_FLAG_HDG_OK) {
  551. /* u-blox calls this Heading of vehicle (2-D)
  552. * why is it different than earlier track? */
  553. session->newdata.track = (double)(getles32(buf, 84) * 1e-5);
  554. }
  555. #endif /* __UNUSED */
  556. if (valid & UBX_NAV_PVT_VALID_MAG) {
  557. magDec = (double)(getles16(buf, 88) * 1e-2);
  558. magAcc = (double)(getleu16(buf, 90) * 1e-2);
  559. }
  560. GPSD_LOG(LOG_DATA, &session->context->errout,
  561. " headVeh %.5f magDec %.2f magAcc %.2f\n",
  562. session->newdata.track, magDec, magAcc);
  563. }
  564. return mask;
  565. }
  566. /**
  567. * High Precision Relative Positioning Information in NED frame
  568. * UBX-NAV-RELPOSNED, Class 1, ID x3c
  569. * HP GNSS only, protver 20+
  570. */
  571. static gps_mask_t
  572. ubx_msg_nav_relposned(struct gps_device_t *session, unsigned char *buf,
  573. size_t data_len)
  574. {
  575. int version;
  576. unsigned refStationId, flags;
  577. double accN, accE, accD;
  578. gps_mask_t mask = 0;
  579. if (40 > data_len) {
  580. GPSD_LOG(LOG_WARN, &session->context->errout,
  581. "Runt NAV-RELPOSNED message, payload len %zd", data_len);
  582. return mask;
  583. }
  584. mask = NED_SET;
  585. version = getub(buf, 0);
  586. refStationId = getleu16(buf, 2);
  587. session->driver.ubx.iTOW = getles32(buf, 4);
  588. session->newdata.NED.relPosN = (1e-2 * (getles32(buf, 8) +
  589. (getsb(buf, 20) * 1e-2)));
  590. session->newdata.NED.relPosE = (1e-2 * (getles32(buf, 12) +
  591. (getsb(buf, 21) * 1e-2)));
  592. session->newdata.NED.relPosD = (1e-2 * (getles32(buf, 16) +
  593. (getsb(buf, 22) * 1e-2)));
  594. accN = 1e-4 * getles32(buf, 24);
  595. accE = 1e-4 * getles32(buf, 28);
  596. accD = 1e-4 * getles32(buf, 32);
  597. flags = getleu32(buf, 36);
  598. GPSD_LOG(LOG_DATA, &session->context->errout,
  599. "UBX-NAV-RELPOSNED: version %d iTOW=%lld refStationId %u flags x%x\n"
  600. "UBX-NAV-RELPOSNED: relPos N=%.4f E=%.4f D=%.4f\n"
  601. "UBX-NAV-RELPOSNED: acc N=%.4f E=%.4f D=%.4f\n",
  602. version,
  603. (long long)session->driver.ubx.iTOW,
  604. refStationId,
  605. flags,
  606. session->newdata.NED.relPosN,
  607. session->newdata.NED.relPosE,
  608. session->newdata.NED.relPosD,
  609. accN, accE, accD);
  610. if (5 != (flags & 5)) {
  611. /* gnssFixOK or relPosValid are false, no fix */
  612. return 0;
  613. }
  614. return mask;
  615. }
  616. /**
  617. * Navigation solution message: UBX-NAV-SOL
  618. *
  619. * UBX-NAV-SOL deprecated in u-blox 6, gone in u-blox 9.
  620. * Use UBX-NAV-PVT instead
  621. */
  622. static gps_mask_t
  623. ubx_msg_nav_sol(struct gps_device_t *session, unsigned char *buf,
  624. size_t data_len)
  625. {
  626. unsigned int flags;
  627. unsigned char navmode;
  628. gps_mask_t mask;
  629. char ts_buf[TIMESPEC_LEN];
  630. if (52 > data_len) {
  631. GPSD_LOG(LOG_WARN, &session->context->errout,
  632. "Runt NAV-SOL message, payload len %zd", data_len);
  633. return 0;
  634. }
  635. session->driver.ubx.iTOW = getleu32(buf, 0);
  636. flags = (unsigned int)getub(buf, 11);
  637. mask = 0;
  638. #define DATE_VALID (UBX_SOL_VALID_WEEK | UBX_SOL_VALID_TIME)
  639. if ((flags & DATE_VALID) == DATE_VALID) {
  640. unsigned short week;
  641. timespec_t ts_tow;
  642. MSTOTS(&ts_tow, session->driver.ubx.iTOW);
  643. ts_tow.tv_nsec += (long)getles32(buf, 4);
  644. week = (unsigned short)getles16(buf, 8);
  645. session->newdata.time = gpsd_gpstime_resolv(session, week, ts_tow);
  646. mask |= TIME_SET | NTPTIME_IS | GOODTIME_IS;
  647. }
  648. #undef DATE_VALID
  649. session->newdata.ecef.x = getles32(buf, 12) / 100.0;
  650. session->newdata.ecef.y = getles32(buf, 16) / 100.0;
  651. session->newdata.ecef.z = getles32(buf, 20) / 100.0;
  652. session->newdata.ecef.pAcc = getleu32(buf, 24) / 100.0;
  653. session->newdata.ecef.vx = getles32(buf, 28) / 100.0;
  654. session->newdata.ecef.vy = getles32(buf, 32) / 100.0;
  655. session->newdata.ecef.vz = getles32(buf, 36) / 100.0;
  656. session->newdata.ecef.vAcc = getleu32(buf, 40) / 100.0;
  657. mask |= ECEF_SET | VECEF_SET;
  658. session->newdata.eps = (double)(getles32(buf, 40) / 100.0);
  659. mask |= SPEEDERR_SET;
  660. /* Better to have a single point of truth about DOPs */
  661. //session->gpsdata.dop.pdop = (double)(getleu16(buf, 44)/100.0);
  662. session->gpsdata.satellites_used = (int)getub(buf, 47);
  663. navmode = (unsigned char)getub(buf, 10);
  664. switch (navmode) {
  665. case UBX_MODE_TMONLY:
  666. /* Surveyed-in, better not have moved */
  667. session->newdata.mode = MODE_3D;
  668. session->gpsdata.status = STATUS_TIME;
  669. break;
  670. case UBX_MODE_3D:
  671. session->newdata.mode = MODE_3D;
  672. session->gpsdata.status = STATUS_FIX;
  673. break;
  674. case UBX_MODE_2D:
  675. session->newdata.mode = MODE_2D;
  676. session->gpsdata.status = STATUS_FIX;
  677. break;
  678. case UBX_MODE_DR: /* consider this too as 2D */
  679. session->newdata.mode = MODE_2D;
  680. session->gpsdata.status = STATUS_DR;
  681. break;
  682. case UBX_MODE_GPSDR: /* DR-aided GPS is valid 3D */
  683. session->newdata.mode = MODE_3D;
  684. session->gpsdata.status = STATUS_GNSSDR;
  685. break;
  686. default:
  687. session->newdata.mode = MODE_NO_FIX;
  688. session->gpsdata.status = STATUS_NO_FIX;
  689. break;
  690. }
  691. if ((flags & UBX_SOL_FLAG_DGPS) != 0)
  692. session->gpsdata.status = STATUS_DGPS_FIX;
  693. mask |= MODE_SET | STATUS_SET;
  694. GPSD_LOG(LOG_DATA, &session->context->errout,
  695. "UBX-NAV-SOL: time=%s ecef x:%.2f y:%.2f z:%.2f track=%.2f "
  696. "speed=%.2f climb=%.2f mode=%d status=%d used=%d\n",
  697. timespec_str(&session->newdata.time, ts_buf, sizeof(ts_buf)),
  698. session->newdata.ecef.x,
  699. session->newdata.ecef.y,
  700. session->newdata.ecef.z,
  701. session->newdata.track,
  702. session->newdata.speed,
  703. session->newdata.climb,
  704. session->newdata.mode,
  705. session->gpsdata.status,
  706. session->gpsdata.satellites_used);
  707. return mask;
  708. }
  709. /**
  710. * Navigation time to leap second: UBX-NAV-TIMELS
  711. *
  712. * Sets leap_notify if leap second is < 23 hours away.
  713. * Not in u-blox 5
  714. */
  715. static void ubx_msg_nav_timels(struct gps_device_t *session,
  716. unsigned char *buf, size_t data_len)
  717. {
  718. int version;
  719. unsigned int flags;
  720. int valid_curr_ls;
  721. int valid_time_to_ls_event;
  722. #define UBX_TIMELS_VALID_CURR_LS 0x01
  723. #define UBX_TIMELS_VALID_TIME_LS_EVT 0x01
  724. if (24 > data_len) {
  725. GPSD_LOG(LOG_WARN, &session->context->errout,
  726. "UBX-NAV-TIMELS: unexpected length %zd, expecting 24\n",
  727. data_len);
  728. return;
  729. }
  730. session->driver.ubx.iTOW = getles32(buf, 0);
  731. version = getsb(buf, 4);
  732. /* Only version 0 is defined so far. */
  733. flags = (unsigned int)getub(buf, 23);
  734. GPSD_LOG(LOG_PROG, &session->context->errout,
  735. "UBX-NAV-TIMELS: flags 0x%x message version %d\n",
  736. flags, version);
  737. valid_curr_ls = flags & UBX_TIMELS_VALID_CURR_LS;
  738. valid_time_to_ls_event = flags & UBX_TIMELS_VALID_TIME_LS_EVT;
  739. if (valid_curr_ls) {
  740. unsigned int src_of_curr_ls = getub(buf,8);
  741. int curr_ls = getsb(buf,9);
  742. char *src = "Unknown";
  743. static char *srcOfCurrLs[] = {
  744. "firmware",
  745. "GPS GLONASS difference",
  746. "GPS",
  747. "SBAS",
  748. "BeiDou",
  749. "Galileo",
  750. "Aided data",
  751. "Configured"
  752. };
  753. if (src_of_curr_ls < (sizeof(srcOfCurrLs) / sizeof(srcOfCurrLs[0])))
  754. src = srcOfCurrLs[src_of_curr_ls];
  755. GPSD_LOG(LOG_DATA, &session->context->errout,
  756. "UBX-NAV-TIMELS: source_of_current_leapsecond=%u:%s "
  757. "curr_ls=%d\n",
  758. src_of_curr_ls, src,curr_ls);
  759. session->context->leap_seconds = curr_ls;
  760. session->context->valid |= LEAP_SECOND_VALID;
  761. } /* Valid current leap second */
  762. if (valid_time_to_ls_event) {
  763. char *src = "Unknown";
  764. unsigned int src_of_ls_change;
  765. unsigned short dateOfLSGpsWn, dateOfLSGpsDn;
  766. int lsChange = getsb(buf, 11);
  767. int timeToLsEvent = getles32(buf, 12);
  768. static char *srcOfLsChange[] = {
  769. "No Source",
  770. "Undefined",
  771. "GPS",
  772. "SBAS",
  773. "BeiDou",
  774. "Galileo",
  775. "GLONASS",
  776. };
  777. src_of_ls_change = getub(buf,10);
  778. if (src_of_ls_change <
  779. (sizeof(srcOfLsChange) / sizeof(srcOfLsChange[0]))) {
  780. src = srcOfLsChange[src_of_ls_change];
  781. }
  782. dateOfLSGpsWn = getles16(buf,16);
  783. dateOfLSGpsDn = getles16(buf,18);
  784. GPSD_LOG(LOG_DATA, &session->context->errout,
  785. "UBX-NAV-TIMELS: source_of_leapsecond_change %u:%s "
  786. "leapSecondChage %d timeToLsEvent %d\n",
  787. src_of_ls_change,src,lsChange,timeToLsEvent);
  788. GPSD_LOG(LOG_DATA, &session->context->errout,
  789. "UBX-NAV-TIMELS: dateOfLSGpsWn=%d dateOfLSGpsDn=%d\n",
  790. dateOfLSGpsWn,dateOfLSGpsDn);
  791. if ((0 != lsChange) && (0 < timeToLsEvent) &&
  792. ((60 * 60 * 23) > timeToLsEvent)) {
  793. if (1 == lsChange) {
  794. session->context->leap_notify = LEAP_ADDSECOND;
  795. GPSD_LOG(LOG_INF, &session->context->errout,
  796. "UBX-NAV-TIMELS: Positive leap second today\n");
  797. } else if (-1 == lsChange) {
  798. session->context->leap_notify = LEAP_DELSECOND;
  799. GPSD_LOG(LOG_INF, &session->context->errout,
  800. "UBX-NAV-TIMELS: Negative leap second today\n");
  801. }
  802. } else {
  803. session->context->leap_notify = LEAP_NOWARNING;
  804. GPSD_LOG(LOG_DATA, &session->context->errout,
  805. "UBX-NAV-TIMELS: leap_notify %d, none today\n",
  806. session->context->leap_notify);
  807. }
  808. }
  809. }
  810. /**
  811. * Geodetic position solution message
  812. * UBX-NAV-POSLLH, Class 1, ID 2
  813. *
  814. * No mode, so limited usefulness
  815. */
  816. static gps_mask_t
  817. ubx_msg_nav_posllh(struct gps_device_t *session, unsigned char *buf,
  818. size_t data_len UNUSED)
  819. {
  820. gps_mask_t mask = 0;
  821. if (28 > data_len) {
  822. GPSD_LOG(LOG_WARN, &session->context->errout,
  823. "Runt NAV-POSLLH message, payload len %zd", data_len);
  824. return 0;
  825. }
  826. mask = ONLINE_SET | HERR_SET | VERR_SET | LATLON_SET | ALTITUDE_SET;
  827. session->driver.ubx.iTOW = getles32(buf, 0);
  828. session->newdata.longitude = 1e-7 * getles32(buf, 4);
  829. session->newdata.latitude = 1e-7 * getles32(buf, 8);
  830. /* altitude WGS84 */
  831. session->newdata.altHAE = 1e-3 * getles32(buf, 12);
  832. /* altitude MSL */
  833. session->newdata.altMSL = 1e-3 * getles32(buf, 16);
  834. /* Let gpsd_error_model() deal with geoid_sep */
  835. /* Horizontal accuracy estimate in mm, unknown type */
  836. session->newdata.eph = getleu32(buf, 20) * 1e-3;
  837. /* Vertical accuracy estimate in mm, unknown type */
  838. session->newdata.epv = getleu32(buf, 24) * 1e-3;
  839. GPSD_LOG(LOG_DATA, &session->context->errout,
  840. "UBX-NAV-POSLLH: iTOW=%lld lat=%.3f lon=%.3f altHAE=%.3f "
  841. "eph %.3f epv %.3f\n",
  842. (long long)session->driver.ubx.iTOW,
  843. session->newdata.latitude,
  844. session->newdata.longitude,
  845. session->newdata.altHAE,
  846. session->newdata.eph,
  847. session->newdata.epv);
  848. return mask;
  849. }
  850. /**
  851. * Dilution of precision message
  852. */
  853. static gps_mask_t
  854. ubx_msg_nav_dop(struct gps_device_t *session, unsigned char *buf,
  855. size_t data_len)
  856. {
  857. if (18 > data_len) {
  858. GPSD_LOG(LOG_WARN, &session->context->errout,
  859. "Runt RXM-SFRB message, payload len %zd", data_len);
  860. return 0;
  861. }
  862. session->driver.ubx.iTOW = getles32(buf, 0);
  863. /*
  864. * We make a deliberate choice not to clear DOPs from the
  865. * last skyview here, but rather to treat this as a supplement
  866. * to our calculations from the visibility matrix, trusting
  867. * the firmware algorithms over ours.
  868. */
  869. session->gpsdata.dop.gdop = (double)(getleu16(buf, 4) / 100.0);
  870. session->gpsdata.dop.pdop = (double)(getleu16(buf, 6) / 100.0);
  871. session->gpsdata.dop.tdop = (double)(getleu16(buf, 8) / 100.0);
  872. session->gpsdata.dop.vdop = (double)(getleu16(buf, 10) / 100.0);
  873. session->gpsdata.dop.hdop = (double)(getleu16(buf, 12) / 100.0);
  874. GPSD_LOG(LOG_DATA, &session->context->errout,
  875. "NAVDOP: gdop=%.2f pdop=%.2f "
  876. "hdop=%.2f vdop=%.2f tdop=%.2f mask={DOP}\n",
  877. session->gpsdata.dop.gdop,
  878. session->gpsdata.dop.hdop,
  879. session->gpsdata.dop.vdop,
  880. session->gpsdata.dop.pdop, session->gpsdata.dop.tdop);
  881. return DOP_SET;
  882. }
  883. /**
  884. * End of Epoch
  885. * Not in u-blox 5, 6 or 7
  886. * Present in u-blox 8 and 9
  887. */
  888. static gps_mask_t
  889. ubx_msg_nav_eoe(struct gps_device_t *session, unsigned char *buf,
  890. size_t data_len)
  891. {
  892. if (4 > data_len) {
  893. GPSD_LOG(LOG_WARN, &session->context->errout,
  894. "Runt NAV-EOE message, payload len %zd", data_len);
  895. return 0;
  896. }
  897. if (18 > session->driver.ubx.protver) {
  898. /* this GPS is at least protver 18 */
  899. session->driver.ubx.protver = 18;
  900. }
  901. session->driver.ubx.iTOW = getles32(buf, 0);
  902. GPSD_LOG(LOG_DATA, &session->context->errout, "EOE: iTOW=%lld\n",
  903. (long long)session->driver.ubx.iTOW);
  904. /* nothing to report, but the iTOW for cycle ender is good */
  905. return 0;
  906. }
  907. /**
  908. * GPS Leap Seconds - UBX-NAV-TIMEGPS
  909. */
  910. static gps_mask_t
  911. ubx_msg_nav_timegps(struct gps_device_t *session, unsigned char *buf,
  912. size_t data_len)
  913. {
  914. uint8_t valid; /* Validity Flags */
  915. gps_mask_t mask = 0;
  916. char ts_buf[TIMESPEC_LEN];
  917. if (16 > data_len) {
  918. GPSD_LOG(LOG_WARN, &session->context->errout,
  919. "Runt NAV-TIMEGPS message, payload len %zd", data_len);
  920. return 0;
  921. }
  922. session->driver.ubx.iTOW = getles32(buf, 0);
  923. valid = getub(buf, 11);
  924. // Valid leap seconds ?
  925. if ((valid & UBX_TIMEGPS_VALID_LEAP_SECOND) ==
  926. UBX_TIMEGPS_VALID_LEAP_SECOND) {
  927. session->context->leap_seconds = (int)getub(buf, 10);
  928. session->context->valid |= LEAP_SECOND_VALID;
  929. }
  930. // Valid GPS time of week and week number
  931. #define VALID_TIME (UBX_TIMEGPS_VALID_TIME | UBX_TIMEGPS_VALID_WEEK)
  932. if ((valid & VALID_TIME) == VALID_TIME) {
  933. #undef VALID_TIME
  934. uint16_t week;
  935. double tAcc; /* Time Accuracy Estimate in ns */
  936. timespec_t ts_tow;
  937. week = getles16(buf, 8);
  938. MSTOTS(&ts_tow, session->driver.ubx.iTOW);
  939. ts_tow.tv_nsec += (long)getles32(buf, 4);
  940. session->newdata.time = gpsd_gpstime_resolv(session, week, ts_tow);
  941. tAcc = (double)getleu32(buf, 12); /* tAcc in ms */
  942. session->newdata.ept = tAcc * 1e-9;
  943. mask |= (TIME_SET | NTPTIME_IS);
  944. }
  945. GPSD_LOG(LOG_DATA, &session->context->errout,
  946. "TIMEGPS: time=%s mask={TIME}\n",
  947. timespec_str(&session->newdata.time, ts_buf, sizeof(ts_buf)));
  948. return mask;
  949. }
  950. /**
  951. * GPS Satellite Info -- new style UBX-NAV-SAT
  952. * Not in u-blox 5, protocol version 15+
  953. */
  954. static gps_mask_t
  955. ubx_msg_nav_sat(struct gps_device_t *session, unsigned char *buf,
  956. size_t data_len)
  957. {
  958. unsigned int i, nchan, nsv, st, ver;
  959. if (8 > data_len) {
  960. GPSD_LOG(LOG_PROG, &session->context->errout,
  961. "Runt NAV-SAT (datalen=%zd)\n", data_len);
  962. return 0;
  963. }
  964. session->driver.ubx.iTOW = getles32(buf, 0);
  965. ver = (unsigned int)getub(buf, 4);
  966. if (1 != ver) {
  967. GPSD_LOG(LOG_WARN, &session->context->errout,
  968. "NAV-SAT message unknown version %d", ver);
  969. return 0;
  970. }
  971. nchan = (unsigned int)getub(buf, 5);
  972. if (nchan > MAXCHANNELS) {
  973. GPSD_LOG(LOG_WARN, &session->context->errout,
  974. "Runt NAV-SAT message, >%d reported visible",
  975. MAXCHANNELS);
  976. return 0;
  977. }
  978. /* two "unused" bytes at buf[6:7] */
  979. gpsd_zero_satellites(&session->gpsdata);
  980. nsv = 0;
  981. for (i = st = 0; i < nchan; i++) {
  982. unsigned int off = 8 + 12 * i;
  983. short nmea_PRN = 0;
  984. unsigned char gnssId = getub(buf, off + 0);
  985. short svId = (short)getub(buf, off + 1);
  986. unsigned char cno = getub(buf, off + 2);
  987. /* health data in flags. */
  988. uint32_t flags = getleu32(buf, off + 8);
  989. bool used = (bool)(flags & 0x08);
  990. int tmp;
  991. /* Notice NO sigid! */
  992. nmea_PRN = ubx2_to_prn(gnssId, svId);
  993. #ifdef __UNUSED
  994. /* debug */
  995. GPSD_LOG(LOG_ERROR, &session->context->errout,
  996. "NAV-SAT gnssid %d, svid %d nmea_PRN %d\n",
  997. gnssId, svId, nmea_PRN);
  998. #endif /* __UNUSED */
  999. session->gpsdata.skyview[st].gnssid = gnssId;
  1000. session->gpsdata.skyview[st].svid = svId;
  1001. session->gpsdata.skyview[st].PRN = nmea_PRN;
  1002. session->gpsdata.skyview[st].ss = (double)cno;
  1003. tmp = getsb(buf, off + 3);
  1004. if (90 >= abs(tmp)) {
  1005. session->gpsdata.skyview[st].elevation = (double)tmp;
  1006. }
  1007. tmp = getles16(buf, off + 4);
  1008. if (359 > tmp && 0 <= tmp) {
  1009. session->gpsdata.skyview[st].azimuth = (double)tmp;
  1010. }
  1011. session->gpsdata.skyview[st].used = used;
  1012. /* by some coincidence, our health flags matches u-blox's */
  1013. session->gpsdata.skyview[st].health = (flags >> 4) & 3;
  1014. /* sbas_in_use is not same as used */
  1015. if (used) {
  1016. nsv++;
  1017. session->gpsdata.skyview[st].used = true;
  1018. }
  1019. st++;
  1020. }
  1021. /* UBX does not give us these, so recompute */
  1022. session->gpsdata.dop.xdop = NAN;
  1023. session->gpsdata.dop.ydop = NAN;
  1024. session->gpsdata.skyview_time.tv_sec = 0;
  1025. session->gpsdata.skyview_time.tv_nsec = 0;
  1026. session->gpsdata.satellites_visible = (int)st;
  1027. session->gpsdata.satellites_used = (int)nsv;
  1028. GPSD_LOG(LOG_DATA, &session->context->errout,
  1029. "SAT: visible=%d used=%d mask={SATELLITE|USED}\n",
  1030. session->gpsdata.satellites_visible,
  1031. session->gpsdata.satellites_used);
  1032. return SATELLITE_SET | USED_IS;
  1033. }
  1034. /**
  1035. * GPS Satellite Info -- deprecated - UBX-NAV-SVINFO
  1036. * Not in u-blox 9, use UBX-NAV-SAT instead
  1037. */
  1038. static gps_mask_t
  1039. ubx_msg_nav_svinfo(struct gps_device_t *session, unsigned char *buf,
  1040. size_t data_len)
  1041. {
  1042. unsigned int i, nchan, nsv, st;
  1043. if (8 > data_len) {
  1044. GPSD_LOG(LOG_PROG, &session->context->errout,
  1045. "Runt NAV-SVINFO (datalen=%zd)\n", data_len);
  1046. return 0;
  1047. }
  1048. session->driver.ubx.iTOW = getles32(buf, 0);
  1049. nchan = (unsigned int)getub(buf, 4);
  1050. if (nchan > MAXCHANNELS) {
  1051. GPSD_LOG(LOG_WARN, &session->context->errout,
  1052. "Runt NAV SVINFO message, >%d reported visible",
  1053. MAXCHANNELS);
  1054. return 0;
  1055. }
  1056. gpsd_zero_satellites(&session->gpsdata);
  1057. nsv = 0;
  1058. for (i = st = 0; i < nchan; i++) {
  1059. unsigned int off = 8 + 12 * i;
  1060. short nmea_PRN;
  1061. short ubx_PRN = (short)getub(buf, off + 1);
  1062. unsigned char snr = getub(buf, off + 4);
  1063. bool used = (bool)(getub(buf, off + 2) & 0x01);
  1064. unsigned char flags = getub(buf, off + 12) & 3;
  1065. int tmp;
  1066. nmea_PRN = ubx_to_prn(ubx_PRN,
  1067. &session->gpsdata.skyview[st].gnssid,
  1068. &session->gpsdata.skyview[st].svid);
  1069. #ifdef __UNUSED
  1070. /* debug */
  1071. GPSD_LOG(LOG_ERROR, &session->context->errout,
  1072. "NAV-SVINFO ubx_prn %d gnssid %d, svid %d nmea_PRN %d\n",
  1073. ubx_PRN,
  1074. session->gpsdata.skyview[st].gnssid,
  1075. session->gpsdata.skyview[st].svid, nmea_PRN);
  1076. #endif /* __UNUSED */
  1077. if (1 > nmea_PRN) {
  1078. /* skip bad PRN */
  1079. continue;
  1080. }
  1081. session->gpsdata.skyview[st].PRN = nmea_PRN;
  1082. session->gpsdata.skyview[st].ss = (double)snr;
  1083. tmp = getsb(buf, off + 5);
  1084. if (90 >= abs(tmp)) {
  1085. session->gpsdata.skyview[st].elevation = (double)tmp;
  1086. }
  1087. tmp = (double)getles16(buf, off + 6);
  1088. if (359 > tmp && 0 <= tmp) {
  1089. session->gpsdata.skyview[st].azimuth = (double)tmp;
  1090. }
  1091. session->gpsdata.skyview[st].used = used;
  1092. if (0x10 & flags) {
  1093. session->gpsdata.skyview[st].health = SAT_HEALTH_BAD;
  1094. } else {
  1095. session->gpsdata.skyview[st].health = SAT_HEALTH_OK;
  1096. }
  1097. /* sbas_in_use is not same as used */
  1098. if (used) {
  1099. /* not really 'used', just integrity data from there */
  1100. nsv++;
  1101. session->gpsdata.skyview[st].used = true;
  1102. }
  1103. st++;
  1104. }
  1105. /* UBX does not give us these, so recompute */
  1106. session->gpsdata.dop.xdop = NAN;
  1107. session->gpsdata.dop.ydop = NAN;
  1108. session->gpsdata.skyview_time.tv_sec = 0;
  1109. session->gpsdata.skyview_time.tv_nsec = 0;
  1110. session->gpsdata.satellites_visible = (int)st;
  1111. session->gpsdata.satellites_used = (int)nsv;
  1112. GPSD_LOG(LOG_DATA, &session->context->errout,
  1113. "SVINFO: visible=%d used=%d mask={SATELLITE|USED}\n",
  1114. session->gpsdata.satellites_visible,
  1115. session->gpsdata.satellites_used);
  1116. return SATELLITE_SET | USED_IS;
  1117. }
  1118. /*
  1119. * Velocity Position ECEF message, UBX-NAV-VELECEF
  1120. */
  1121. static gps_mask_t
  1122. ubx_msg_nav_velecef(struct gps_device_t *session, unsigned char *buf,
  1123. size_t data_len)
  1124. {
  1125. gps_mask_t mask = VECEF_SET;
  1126. if (20 > data_len) {
  1127. GPSD_LOG(LOG_WARN, &session->context->errout,
  1128. "Runt NAV-VELECEF message, payload len %zd", data_len);
  1129. return 0;
  1130. }
  1131. session->driver.ubx.iTOW = getles32(buf, 0);
  1132. session->newdata.ecef.vx = getles32(buf, 4) / 100.0;
  1133. session->newdata.ecef.vy = getles32(buf, 8) / 100.0;
  1134. session->newdata.ecef.vz = getles32(buf, 12) / 100.0;
  1135. session->newdata.ecef.vAcc = getleu32(buf, 16) / 100.0;
  1136. GPSD_LOG(LOG_DATA, &session->context->errout,
  1137. "UBX-NAV-VELECEF: iTOW=%lld ECEF vx=%.2f vy=%.2f vz=%.2f vAcc=%.2f\n",
  1138. (long long)session->driver.ubx.iTOW,
  1139. session->newdata.ecef.vx,
  1140. session->newdata.ecef.vy,
  1141. session->newdata.ecef.vz,
  1142. session->newdata.ecef.vAcc);
  1143. return mask;
  1144. }
  1145. /*
  1146. * Velocity NED message, UBX-NAV-VELNED
  1147. * protocol versions 15+
  1148. */
  1149. static gps_mask_t
  1150. ubx_msg_nav_velned(struct gps_device_t *session, unsigned char *buf,
  1151. size_t data_len)
  1152. {
  1153. gps_mask_t mask = VNED_SET;
  1154. if (36 > data_len) {
  1155. GPSD_LOG(LOG_WARN, &session->context->errout,
  1156. "Runt NAV-VELNED message, payload len %zd", data_len);
  1157. return 0;
  1158. }
  1159. session->driver.ubx.iTOW = getles32(buf, 0);
  1160. session->newdata.NED.velN = getles32(buf, 4) / 100.0;
  1161. session->newdata.NED.velE = getles32(buf, 8) / 100.0;
  1162. session->newdata.NED.velD = getles32(buf, 12) / 100.0;
  1163. /* ignore speed for now */
  1164. GPSD_LOG(LOG_DATA, &session->context->errout,
  1165. "UBX-NAV-VELNED: iTOW=%lld NED velN=%.2f velE=%.2f velD=%.2f\n",
  1166. (long long)session->driver.ubx.iTOW,
  1167. session->newdata.NED.velN,
  1168. session->newdata.NED.velE,
  1169. session->newdata.NED.velD);
  1170. return mask;
  1171. }
  1172. /*
  1173. * SBAS Info UBX-NAV-SBAS
  1174. * Not in u-blox 9
  1175. * FIXME: not well decoded...
  1176. */
  1177. static void ubx_msg_sbas(struct gps_device_t *session, unsigned char *buf,
  1178. size_t data_len)
  1179. {
  1180. unsigned int i, nsv;
  1181. short ubx_PRN;
  1182. short nmea_PRN;
  1183. unsigned char gnssid = 0;
  1184. unsigned char svid = 0;
  1185. if (12 > data_len) {
  1186. GPSD_LOG(LOG_WARN, &session->context->errout,
  1187. "Runt NAV-SBAS message, payload len %zd", data_len);
  1188. return;
  1189. }
  1190. session->driver.ubx.iTOW = getles32(buf, 0);
  1191. GPSD_LOG(LOG_DATA, &session->context->errout,
  1192. "SBAS: %d %d %d %d %d\n",
  1193. (int)getub(buf, 4), (int)getub(buf, 5), (int)getub(buf, 6),
  1194. (int)getub(buf, 7), (int)getub(buf, 8));
  1195. nsv = (int)getub(buf, 8);
  1196. for (i = 0; i < nsv; i++) {
  1197. int off = 12 + 12 * i;
  1198. GPSD_LOG(LOG_DATA, &session->context->errout,
  1199. "SBAS info on SV: %d\n", (int)getub(buf, off));
  1200. }
  1201. /* really 'in_use' depends on the sats info, EGNOS is still
  1202. * in test. In WAAS areas one might also check for the type of
  1203. * corrections indicated
  1204. */
  1205. ubx_PRN = getub(buf, 4);
  1206. nmea_PRN = ubx_to_prn(ubx_PRN, &gnssid, &svid);
  1207. #ifdef __UNUSED
  1208. /* debug */
  1209. GPSD_LOG(LOG_ERROR, &session->context->errout,
  1210. "NAV-SBAS ubx_prn %d gnssid %d, svid %d nmea_PRN %d\n",
  1211. ubx_PRN, gnssid, svid, nmea_PRN);
  1212. #endif /* __UNUSED */
  1213. session->driver.ubx.sbas_in_use = nmea_PRN;
  1214. }
  1215. /*
  1216. * Multi-GNSS Raw measurement Data -- UBX-RXM-RAWX
  1217. * Not in u-blox 5, 6 or 7
  1218. */
  1219. static gps_mask_t ubx_rxm_rawx(struct gps_device_t *session,
  1220. const unsigned char *buf,
  1221. size_t data_len)
  1222. {
  1223. double rcvTow;
  1224. uint16_t week;
  1225. int8_t leapS;
  1226. uint8_t numMeas;
  1227. uint8_t recStat;
  1228. int i;
  1229. const char * obs_code;
  1230. timespec_t ts_tow;
  1231. if (16 > data_len) {
  1232. GPSD_LOG(LOG_WARN, &session->context->errout,
  1233. "Runt RXM-RAWX message, payload len %zd", data_len);
  1234. return 0;
  1235. }
  1236. /* Note: this is "approximately" GPS TOW, this is not iTOW */
  1237. rcvTow = getled64((const char *)buf, 0); /* time of week in seconds */
  1238. week = getleu16(buf, 8);
  1239. leapS = getsb(buf, 10);
  1240. numMeas = getub(buf, 11);
  1241. recStat = getub(buf, 12);
  1242. GPSD_LOG(LOG_PROG, &session->context->errout,
  1243. "UBX-RXM-RAWX: rcvTow %f week %u leapS %d numMeas %u recStat %d\n",
  1244. rcvTow, week, leapS, numMeas, recStat);
  1245. if (recStat & 1) {
  1246. /* Valid leap seconds */
  1247. session->context->leap_seconds = leapS;
  1248. session->context->valid |= LEAP_SECOND_VALID;
  1249. }
  1250. /* convert GPS weeks and "approximately" GPS TOW to UTC */
  1251. DTOTS(&ts_tow, rcvTow);
  1252. // Do not set newdata.time. set gpsdata.raw.mtime
  1253. session->gpsdata.raw.mtime = gpsd_gpstime_resolv(session, week, ts_tow);
  1254. /* zero the measurement data */
  1255. /* so we can tell which meas never got set */
  1256. memset(session->gpsdata.raw.meas, 0, sizeof(session->gpsdata.raw.meas));
  1257. for (i = 0; i < numMeas; i++) {
  1258. int off = 32 * i;
  1259. /* psuedorange in meters */
  1260. double prMes = getled64((const char *)buf, off + 16);
  1261. /* carrier phase in cycles */
  1262. double cpMes = getled64((const char *)buf, off + 24);
  1263. /* doppler in Hz, positive towards sat */
  1264. double doMes = getlef32((const char *)buf, off + 32);
  1265. uint8_t gnssId = getub(buf, off + 36);
  1266. uint8_t svId = getub(buf, off + 37);
  1267. /* reserved in u-blox 8, sigId in u-blox 9 */
  1268. uint8_t sigId = getub(buf, off + 38);
  1269. /* GLONASS frequency slot */
  1270. uint8_t freqId = getub(buf, off + 39);
  1271. /* carrier phase locktime in ms, max 64500ms */
  1272. uint16_t locktime = getleu16(buf, off + 40);
  1273. /* carrier-to-noise density ratio dB-Hz */
  1274. uint8_t cno = getub(buf, off + 42);
  1275. uint8_t prStdev = getub(buf, off + 43) & 0x0f;
  1276. uint8_t cpStdev = getub(buf, off + 44) & 0x0f;
  1277. uint8_t doStdev = getub(buf, off + 45) & 0x0f;
  1278. /* tracking stat
  1279. * bit 0 - prMes valid
  1280. * bit 1 - cpMes valid
  1281. * bit 2 - halfCycle valid
  1282. * bit 3 - halfCycle subtracted from phase
  1283. */
  1284. uint8_t trkStat = getub(buf, off + 46);
  1285. GPSD_LOG(LOG_DATA, &session->context->errout,
  1286. "%u:%u:%u freqId %u prMes %f cpMes %f doMes %f locktime %u\n"
  1287. "cno %u prStdev %u cpStdev %u doStdev %u rtkStat %u\n",
  1288. gnssId, svId, sigId, freqId, prMes, cpMes, doMes, locktime,
  1289. cno, prStdev, cpStdev, doStdev, trkStat);
  1290. session->gpsdata.raw.meas[i].gnssid = gnssId;
  1291. session->gpsdata.raw.meas[i].sigid = sigId;
  1292. /* some of these are GUESSES as the u-blox codes do not
  1293. * match RINEX codes */
  1294. switch (gnssId) {
  1295. case 0: /* GPS */
  1296. switch (sigId) {
  1297. default:
  1298. /* let PPP figure it out */
  1299. /* FALLTHROUGH */
  1300. case 0: /* L1C/A */
  1301. obs_code = "L1C";
  1302. break;
  1303. case 3: /* L2 CL */
  1304. obs_code = "L2C";
  1305. break;
  1306. case 4: /* L2 CM */
  1307. obs_code = "L2X";
  1308. break;
  1309. }
  1310. break;
  1311. case 1: /* SBAS */
  1312. /* sigId added on protVer 27, and SBAS gone in protVer 27
  1313. * so must be L1C/A */
  1314. svId -= 100; /* adjust for RINEX 3 svid */
  1315. /* SBAS can do L5I, but the code? */
  1316. switch (sigId) {
  1317. default:
  1318. /* let PPP figure it out */
  1319. /* FALLTHROUGH */
  1320. case 0: /* L1C/A */
  1321. obs_code = "L1C";
  1322. break;
  1323. }
  1324. obs_code = "L1C"; /* u-blox calls this L1C/A */
  1325. break;
  1326. case 2: /* GALILEO */
  1327. switch (sigId) {
  1328. default:
  1329. /* let PPP figure it out */
  1330. /* FALLTHROUGH */
  1331. case 0: /* */
  1332. obs_code = "L1C"; /* u-blox calls this E1OS or E1C */
  1333. break;
  1334. case 1: /* */
  1335. obs_code = "L1B"; /* u-blox calls this E1B */
  1336. break;
  1337. case 5: /* */
  1338. obs_code = "L7I"; /* u-blox calls this E5bl */
  1339. break;
  1340. case 6: /* */
  1341. obs_code = "L7Q"; /* u-blox calls this E5bQ */
  1342. break;
  1343. }
  1344. break;
  1345. case 3: /* BeiDou */
  1346. switch (sigId) {
  1347. default:
  1348. /* let PPP figure it out */
  1349. /* FALLTHROUGH */
  1350. case 0: /* */
  1351. obs_code = "L2Q"; /* u-blox calls this B1I D1 */
  1352. break;
  1353. case 1: /* */
  1354. obs_code = "L2I"; /* u-blox calls this B1I D2 */
  1355. break;
  1356. case 2: /* */
  1357. obs_code = "L7Q"; /* u-blox calls this B2I D1 */
  1358. break;
  1359. case 3: /* */
  1360. obs_code = "L7I"; /* u-blox calls this B2I D2 */
  1361. break;
  1362. }
  1363. break;
  1364. default: /* huh? */
  1365. case 4: /* IMES. really? */
  1366. obs_code = ""; /* u-blox calls this L1 */
  1367. break;
  1368. case 5: /* QZSS */
  1369. switch (sigId) {
  1370. default:
  1371. /* let PPP figure it out */
  1372. /* FALLTHROUGH */
  1373. case 0: /* */
  1374. obs_code = "L1C"; /* u-blox calls this L1C/A */
  1375. break;
  1376. case 4: /* */
  1377. obs_code = "L2S"; /* u-blox calls this L2CM */
  1378. break;
  1379. case 5: /* */
  1380. obs_code = "L2L"; /* u-blox calls this L2CL*/
  1381. break;
  1382. }
  1383. break;
  1384. case 6: /* GLONASS */
  1385. switch (sigId) {
  1386. default:
  1387. /* let PPP figure it out */
  1388. /* FALLTHROUGH */
  1389. case 0: /* */
  1390. obs_code = "L1C"; /* u-blox calls this L1OF */
  1391. break;
  1392. case 2: /* */
  1393. obs_code = "L2C"; /* u-blox calls this L2OF */
  1394. break;
  1395. }
  1396. break;
  1397. }
  1398. (void)strlcpy(session->gpsdata.raw.meas[i].obs_code, obs_code,
  1399. sizeof(session->gpsdata.raw.meas[i].obs_code));
  1400. session->gpsdata.raw.meas[i].svid = svId;
  1401. session->gpsdata.raw.meas[i].freqid = freqId;
  1402. session->gpsdata.raw.meas[i].snr = cno;
  1403. session->gpsdata.raw.meas[i].satstat = trkStat;
  1404. if (trkStat & 1) {
  1405. /* prMes valid */
  1406. session->gpsdata.raw.meas[i].pseudorange = prMes;
  1407. } else {
  1408. session->gpsdata.raw.meas[i].pseudorange = NAN;
  1409. }
  1410. if ((trkStat & 2) && (5 >= cpStdev)) {
  1411. /* cpMes valid, RTKLIB uses 5 < cpStdev */
  1412. session->gpsdata.raw.meas[i].carrierphase = cpMes;
  1413. } else {
  1414. session->gpsdata.raw.meas[i].carrierphase = NAN;
  1415. }
  1416. session->gpsdata.raw.meas[i].doppler = doMes;
  1417. session->gpsdata.raw.meas[i].codephase = NAN;
  1418. session->gpsdata.raw.meas[i].deltarange = NAN;
  1419. session->gpsdata.raw.meas[i].locktime = locktime;
  1420. if (0 == locktime) {
  1421. /* possible slip */
  1422. session->gpsdata.raw.meas[i].lli = 2;
  1423. }
  1424. }
  1425. return RAW_IS;
  1426. }
  1427. /*
  1428. * Raw Subframes - UBX-RXM-SFRB
  1429. * Not in u-blox 8 or 9
  1430. */
  1431. static gps_mask_t ubx_rxm_sfrb(struct gps_device_t *session,
  1432. unsigned char *buf, size_t data_len)
  1433. {
  1434. unsigned int i, chan, svid;
  1435. uint32_t words[10];
  1436. if (42 > data_len) {
  1437. GPSD_LOG(LOG_WARN, &session->context->errout,
  1438. "Runt RXM-SFRB message, payload len %zd", data_len);
  1439. return 0;
  1440. }
  1441. chan = (unsigned int)getub(buf, 0);
  1442. svid = (unsigned int)getub(buf, 1);
  1443. GPSD_LOG(LOG_PROG, &session->context->errout,
  1444. "UBX-RXM-SFRB: %u %u\n", chan, svid);
  1445. /* UBX does all the parity checking, but still bad data gets through */
  1446. for (i = 0; i < 10; i++) {
  1447. words[i] = (uint32_t)getleu32(buf, 4 * i + 2) & 0xffffff;
  1448. }
  1449. return gpsd_interpret_subframe(session, svid, words);
  1450. }
  1451. /* UBX-INF-* */
  1452. static void ubx_msg_inf(struct gps_device_t *session, unsigned char *buf,
  1453. size_t data_len)
  1454. {
  1455. unsigned short msgid;
  1456. static char txtbuf[MAX_PACKET_LENGTH];
  1457. /* No minimum payload length */
  1458. msgid = (unsigned short)((buf[2] << 8) | buf[3]);
  1459. if (data_len > MAX_PACKET_LENGTH - 1)
  1460. data_len = MAX_PACKET_LENGTH - 1;
  1461. (void)strlcpy(txtbuf, (char *)buf + UBX_PREFIX_LEN, sizeof(txtbuf));
  1462. txtbuf[data_len] = '\0';
  1463. switch (msgid) {
  1464. case UBX_INF_DEBUG:
  1465. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-INF-DEBUG: %s\n",
  1466. txtbuf);
  1467. break;
  1468. case UBX_INF_TEST:
  1469. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-INF-TEST: %s\n",
  1470. txtbuf);
  1471. break;
  1472. case UBX_INF_NOTICE:
  1473. GPSD_LOG(LOG_INF, &session->context->errout, "UBX-INF-NOTICE: %s\n",
  1474. txtbuf);
  1475. break;
  1476. case UBX_INF_WARNING:
  1477. GPSD_LOG(LOG_WARN, &session->context->errout, "UBX-INF-WARNING: %s\n",
  1478. txtbuf);
  1479. break;
  1480. case UBX_INF_ERROR:
  1481. GPSD_LOG(LOG_WARN, &session->context->errout, "UBX-INF-ERROR: %s\n",
  1482. txtbuf);
  1483. break;
  1484. default:
  1485. break;
  1486. }
  1487. return;
  1488. }
  1489. /**
  1490. * Time Pulse Timedata - UBX-TIM-TP
  1491. */
  1492. static gps_mask_t
  1493. ubx_msg_tim_tp(struct gps_device_t *session, unsigned char *buf,
  1494. size_t data_len)
  1495. {
  1496. gps_mask_t mask = ONLINE_SET;
  1497. uint32_t towMS;
  1498. uint32_t towSubMS;
  1499. int32_t qErr;
  1500. uint16_t week;
  1501. uint8_t flags;
  1502. uint8_t refInfo;
  1503. timespec_t ts_tow;
  1504. if (16 > data_len) {
  1505. GPSD_LOG(LOG_WARN, &session->context->errout,
  1506. "Runt TIM-TP message, payload len %zd", data_len);
  1507. return 0;
  1508. }
  1509. towMS = getleu32(buf, 0);
  1510. // towSubMS always seems zero, which will match the PPS
  1511. towSubMS = getleu32(buf, 4);
  1512. qErr = getles32(buf, 8);
  1513. week = getleu16(buf, 12);
  1514. flags = buf[14];
  1515. refInfo = buf[15];
  1516. /* are we UTC, and towSubMs is zero? */
  1517. if (3 == (flags & 0x03) &&
  1518. 0 == towSubMS) {
  1519. // leap already added!?!?
  1520. int saved_leap = session->context->leap_seconds;
  1521. // remove it!
  1522. session->context->leap_seconds = 0;
  1523. /* good, save qErr and qErr_time */
  1524. session->gpsdata.qErr = qErr;
  1525. MSTOTS(&ts_tow, towMS);
  1526. session->gpsdata.qErr_time = gpsd_gpstime_resolv(session, week, ts_tow);
  1527. // restore leap
  1528. session->context->leap_seconds = saved_leap;
  1529. #ifdef __UNUSED
  1530. {
  1531. struct gps_device_t *ppsonly;
  1532. // FIXME!! should be up a layer so other drivers can use it
  1533. // FIXME!! this qErr can only apply to one PPS!
  1534. /* propagate this in-band-time to all PPS-only devices */
  1535. for (ppsonly = devices; ppsonly < devices + MAX_DEVICES; ppsonly++)
  1536. if (ppsonly->sourcetype == source_pps)
  1537. pps_thread_qErrin(&ppsonly->pps_thread, qErr,
  1538. session->gpsdata.qErr_time);
  1539. }
  1540. #endif /* __UNUSED */
  1541. }
  1542. /* cast for 32 bit compatibility */
  1543. GPSD_LOG(LOG_DATA, &session->context->errout,
  1544. "TIM-TP: towMS %lu, towSubMS %lu, qErr %ld week %u "
  1545. "flags %#x, refInfo %#x\n",
  1546. (unsigned long)towMS, (unsigned long)towSubMS, (long)qErr,
  1547. week, flags, refInfo);
  1548. return mask;
  1549. }
  1550. gps_mask_t ubx_parse(struct gps_device_t * session, unsigned char *buf,
  1551. size_t len)
  1552. {
  1553. size_t data_len;
  1554. unsigned short msgid;
  1555. gps_mask_t mask = 0;
  1556. /* the packet at least contains a head long enough for an empty message */
  1557. if (len < UBX_PREFIX_LEN)
  1558. return 0;
  1559. session->cycle_end_reliable = true;
  1560. session->driver.ubx.iTOW = -1; /* set by decoder */
  1561. /* extract message id and length */
  1562. msgid = (buf[2] << 8) | buf[3];
  1563. data_len = (size_t) getles16(buf, 4);
  1564. switch (msgid) {
  1565. case UBX_ACK_ACK:
  1566. if (2 <= data_len) {
  1567. GPSD_LOG(LOG_DATA, &session->context->errout,
  1568. "UBX-ACK-ACK, class: %02x, id: %02x\n",
  1569. buf[UBX_MESSAGE_DATA_OFFSET],
  1570. buf[UBX_MESSAGE_DATA_OFFSET + 1]);
  1571. }
  1572. break;
  1573. case UBX_ACK_NAK:
  1574. if (2 <= data_len) {
  1575. GPSD_LOG(LOG_WARN, &session->context->errout,
  1576. "UBX-ACK-NAK, class: %02x, id: %02x\n",
  1577. buf[UBX_MESSAGE_DATA_OFFSET],
  1578. buf[UBX_MESSAGE_DATA_OFFSET + 1]);
  1579. }
  1580. break;
  1581. case UBX_CFG_PRT:
  1582. if (session->driver.ubx.port_id != buf[UBX_MESSAGE_DATA_OFFSET + 0] ) {
  1583. session->driver.ubx.port_id = buf[UBX_MESSAGE_DATA_OFFSET + 0];
  1584. GPSD_LOG(LOG_INF, &session->context->errout,
  1585. "UBX-CFG-PRT: port %d\n", session->driver.ubx.port_id);
  1586. #ifdef RECONFIGURE_ENABLE
  1587. /* Need to reinitialize since port changed */
  1588. if (session->mode == O_OPTIMIZE) {
  1589. ubx_mode(session, MODE_BINARY);
  1590. } else {
  1591. ubx_mode(session, MODE_NMEA);
  1592. }
  1593. #endif /* RECONFIGURE_ENABLE */
  1594. }
  1595. break;
  1596. case UBX_INF_DEBUG:
  1597. /* FALLTHROUGH */
  1598. case UBX_INF_ERROR:
  1599. /* FALLTHROUGH */
  1600. case UBX_INF_NOTICE:
  1601. /* FALLTHROUGH */
  1602. case UBX_INF_TEST:
  1603. /* FALLTHROUGH */
  1604. case UBX_INF_USER:
  1605. /* FALLTHROUGH */
  1606. case UBX_INF_WARNING:
  1607. ubx_msg_inf(session, buf, data_len);
  1608. break;
  1609. case UBX_MON_EXCEPT:
  1610. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-MON-EXCEPT\n");
  1611. break;
  1612. case UBX_MON_GNSS:
  1613. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-MON-GNSS\n");
  1614. break;
  1615. case UBX_MON_HW:
  1616. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-MON-HW\n");
  1617. break;
  1618. case UBX_MON_HW2:
  1619. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-MON-HW2\n");
  1620. break;
  1621. case UBX_MON_IO:
  1622. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-MON-IO\n");
  1623. break;
  1624. case UBX_MON_IPC:
  1625. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-MON-IPC\n");
  1626. break;
  1627. case UBX_MON_MSGPP:
  1628. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-MON-MSGPP\n");
  1629. break;
  1630. case UBX_MON_PATCH:
  1631. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-MON-PATCH\n");
  1632. break;
  1633. case UBX_MON_RXBUF:
  1634. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-MON-RXBUF\n");
  1635. break;
  1636. case UBX_MON_RXR:
  1637. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-MON-RXR\n");
  1638. break;
  1639. case UBX_MON_SCHED:
  1640. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-MON-SCHED\n");
  1641. break;
  1642. case UBX_MON_SMGR:
  1643. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-MON-SMGR\n");
  1644. break;
  1645. case UBX_MON_TXBUF:
  1646. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-MON-TXBUF\n");
  1647. break;
  1648. case UBX_MON_USB:
  1649. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-MON-USB\n");
  1650. break;
  1651. case UBX_MON_VER:
  1652. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-MON-VER\n");
  1653. ubx_msg_mon_ver(session, buf, data_len);
  1654. break;
  1655. case UBX_NAV_AOPSTATUS:
  1656. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-AOPSTATUS\n");
  1657. break;
  1658. case UBX_NAV_ATT:
  1659. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-ATT\n");
  1660. break;
  1661. case UBX_NAV_CLOCK:
  1662. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-CLOCK\n");
  1663. break;
  1664. case UBX_NAV_DGPS:
  1665. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-DGPS\n");
  1666. break;
  1667. case UBX_NAV_DOP:
  1668. /* DOP seems to be the last NAV sent in a cycle */
  1669. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-DOP\n");
  1670. mask = ubx_msg_nav_dop(session, &buf[UBX_PREFIX_LEN], data_len);
  1671. break;
  1672. case UBX_NAV_EKFSTATUS:
  1673. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-EKFSTATUS\n");
  1674. break;
  1675. case UBX_NAV_EOE:
  1676. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-EOE\n");
  1677. mask = ubx_msg_nav_eoe(session, &buf[UBX_PREFIX_LEN], data_len);
  1678. break;
  1679. case UBX_NAV_GEOFENCE:
  1680. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-GEOFENCE\n");
  1681. break;
  1682. case UBX_NAV_HPPOSECEF:
  1683. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-HPPOSECEF\n");
  1684. mask = ubx_msg_nav_hpposecef(session, &buf[UBX_PREFIX_LEN], data_len);
  1685. break;
  1686. case UBX_NAV_HPPOSLLH:
  1687. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-HPPOSLLH\n");
  1688. mask = ubx_msg_nav_hpposllh(session, &buf[UBX_PREFIX_LEN], data_len);
  1689. break;
  1690. case UBX_NAV_ODO:
  1691. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-ODO\n");
  1692. break;
  1693. case UBX_NAV_ORB:
  1694. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-ORB\n");
  1695. break;
  1696. case UBX_NAV_POSECEF:
  1697. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-POSECEF\n");
  1698. mask = ubx_msg_nav_posecef(session, &buf[UBX_PREFIX_LEN], data_len);
  1699. break;
  1700. case UBX_NAV_POSLLH:
  1701. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-POSLLH\n");
  1702. mask = ubx_msg_nav_posllh(session, &buf[UBX_PREFIX_LEN], data_len);
  1703. break;
  1704. case UBX_NAV_POSUTM:
  1705. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-POSUTM\n");
  1706. break;
  1707. case UBX_NAV_PVT:
  1708. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-PVT\n");
  1709. mask = ubx_msg_nav_pvt(session, &buf[UBX_PREFIX_LEN], data_len);
  1710. mask |= REPORT_IS;
  1711. break;
  1712. case UBX_NAV_RELPOSNED:
  1713. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-RELPOSNED\n");
  1714. mask = ubx_msg_nav_relposned(session, &buf[UBX_PREFIX_LEN], data_len);
  1715. break;
  1716. case UBX_NAV_RESETODO:
  1717. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-RESETODO\n");
  1718. break;
  1719. case UBX_NAV_SIG:
  1720. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-SIG\n");
  1721. break;
  1722. case UBX_NAV_SAT:
  1723. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-SAT\n");
  1724. mask = ubx_msg_nav_sat(session, &buf[UBX_PREFIX_LEN], data_len);
  1725. break;
  1726. case UBX_NAV_SBAS:
  1727. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-SBAS\n");
  1728. ubx_msg_sbas(session, &buf[UBX_PREFIX_LEN], data_len);
  1729. break;
  1730. case UBX_NAV_SOL:
  1731. /* UBX-NAV-SOL deprecated in u-blox 6, gone in u-blox 9.
  1732. * Use UBX-NAV-PVT instead */
  1733. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-SOL\n");
  1734. mask = ubx_msg_nav_sol(session, &buf[UBX_PREFIX_LEN], data_len);
  1735. mask |= REPORT_IS;
  1736. break;
  1737. case UBX_NAV_STATUS:
  1738. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-STATUS\n");
  1739. break;
  1740. case UBX_NAV_SVIN:
  1741. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-SVIN\n");
  1742. break;
  1743. case UBX_NAV_SVINFO:
  1744. /* UBX-NAV-SVINFO deprecated, use UBX-NAV-SAT instead */
  1745. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-SVINFO\n");
  1746. mask = ubx_msg_nav_svinfo(session, &buf[UBX_PREFIX_LEN], data_len);
  1747. /* this is a hack to move some initialization until after we
  1748. * get some u-blox message so we know the GPS is alive */
  1749. if ('\0' == session->subtype[0]) {
  1750. /* one time only */
  1751. (void)strlcpy(session->subtype, "Unknown", 8);
  1752. /* request SW and HW Versions */
  1753. (void)ubx_write(session, UBX_CLASS_MON, 0x04, NULL, 0);
  1754. }
  1755. break;
  1756. case UBX_NAV_TIMEBDS:
  1757. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-TIMEBDS\n");
  1758. break;
  1759. case UBX_NAV_TIMEGAL:
  1760. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-TIMEGAL\n");
  1761. break;
  1762. case UBX_NAV_TIMEGLO:
  1763. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-TIMEGLO\n");
  1764. break;
  1765. case UBX_NAV_TIMEGPS:
  1766. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-NAV-TIMEGPS\n");
  1767. mask = ubx_msg_nav_timegps(session, &buf[UBX_PREFIX_LEN], data_len);
  1768. break;
  1769. case UBX_NAV_TIMELS:
  1770. ubx_msg_nav_timels(session, &buf[UBX_PREFIX_LEN], data_len);
  1771. break;
  1772. case UBX_NAV_TIMEUTC:
  1773. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-TIMEUTC\n");
  1774. break;
  1775. case UBX_NAV_VELECEF:
  1776. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-VELECEF\n");
  1777. mask = ubx_msg_nav_velecef(session, &buf[UBX_PREFIX_LEN], data_len);
  1778. break;
  1779. case UBX_NAV_VELNED:
  1780. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-NAV-VELNED\n");
  1781. mask = ubx_msg_nav_velned(session, &buf[UBX_PREFIX_LEN], data_len);
  1782. break;
  1783. case UBX_RXM_ALM:
  1784. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-RXM-ALM\n");
  1785. break;
  1786. case UBX_RXM_EPH:
  1787. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-RXM-EPH\n");
  1788. break;
  1789. case UBX_RXM_IMES:
  1790. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-RXM-IMES\n");
  1791. break;
  1792. case UBX_RXM_MEASX:
  1793. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-RXM-MEASX\n");
  1794. break;
  1795. case UBX_RXM_PMREQ:
  1796. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-RXM-PMREQ\n");
  1797. break;
  1798. case UBX_RXM_POSREQ:
  1799. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-RXM-POSREQ\n");
  1800. break;
  1801. case UBX_RXM_RAW:
  1802. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-RXM-RAW\n");
  1803. break;
  1804. case UBX_RXM_RAWX:
  1805. mask = ubx_rxm_rawx(session, &buf[UBX_PREFIX_LEN], data_len);
  1806. break;
  1807. case UBX_RXM_RLM:
  1808. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-RXM-RLM\n");
  1809. break;
  1810. case UBX_RXM_RTCM:
  1811. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-RXM-RTCM\n");
  1812. break;
  1813. case UBX_RXM_SFRB:
  1814. mask = ubx_rxm_sfrb(session, &buf[UBX_PREFIX_LEN], data_len);
  1815. break;
  1816. case UBX_RXM_SFRBX:
  1817. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-RXM-SFRBX\n");
  1818. break;
  1819. case UBX_RXM_SVSI:
  1820. GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-RXM-SVSI\n");
  1821. break;
  1822. case UBX_TIM_DOSC:
  1823. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-TIM-DOSC\n");
  1824. break;
  1825. case UBX_TIM_FCHG:
  1826. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-TIM-FCHG\n");
  1827. break;
  1828. case UBX_TIM_HOC:
  1829. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-TIM-HOC\n");
  1830. break;
  1831. case UBX_TIM_SMEAS:
  1832. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-TIM-SMEAS\n");
  1833. break;
  1834. case UBX_TIM_SVIN:
  1835. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-TIM-SVIN\n");
  1836. break;
  1837. case UBX_TIM_TM:
  1838. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-TIM-TM\n");
  1839. break;
  1840. case UBX_TIM_TM2:
  1841. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-TIM-TM2\n");
  1842. break;
  1843. case UBX_TIM_TP:
  1844. mask = ubx_msg_tim_tp(session, &buf[UBX_PREFIX_LEN], data_len);
  1845. break;
  1846. case UBX_TIM_TOS:
  1847. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-TIM-TOS\n");
  1848. break;
  1849. case UBX_TIM_VCOCAL:
  1850. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-TIM-VCOCAL\n");
  1851. break;
  1852. case UBX_TIM_VRFY:
  1853. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX-TIM-VRFY\n");
  1854. break;
  1855. default:
  1856. GPSD_LOG(LOG_WARN, &session->context->errout,
  1857. "UBX: unknown packet id 0x%04hx (length %zd)\n",
  1858. msgid, len);
  1859. }
  1860. /* end of cycle ? */
  1861. if (session->driver.ubx.end_msgid == msgid) {
  1862. /* end of cycle, report it */
  1863. GPSD_LOG(LOG_PROG, &session->context->errout,
  1864. "UBX: cycle end %x\n", msgid);
  1865. mask |= REPORT_IS;
  1866. }
  1867. /* start of cycle ? */
  1868. if ( -1 < session->driver.ubx.iTOW) {
  1869. /* this sentence has a good time */
  1870. /* debug
  1871. GPSD_LOG(LOG_ERROR, &session->context->errout,
  1872. "UBX: time %.2f msgid %x\n",
  1873. session->newdata.time, msgid);
  1874. GPSD_LOG(LOG_ERROR, &session->context->errout,
  1875. " last_time %s last_msgid %x\n",
  1876. timespec_str(&session->driver.ubx.last_time, ts_buf,
  1877. sizeof(ts_buf)),
  1878. session->driver.ubx.last_msgid);
  1879. */
  1880. /* iTOW is to ms, can go forward or backwards */
  1881. if ((session->driver.ubx.last_iTOW != session->driver.ubx.iTOW) &&
  1882. (session->driver.ubx.end_msgid !=
  1883. session->driver.ubx.last_msgid)) {
  1884. /* time changed, new cycle ender */
  1885. session->driver.ubx.end_msgid = session->driver.ubx.last_msgid;
  1886. session->driver.ubx.last_iTOW = session->driver.ubx.iTOW;
  1887. /* debug
  1888. GPSD_LOG(LOG_ERROR, &session->context->errout,
  1889. "UBX: new ender %x, iTOW %.2f\n",
  1890. session->driver.ubx.end_msgid, iTOW);
  1891. */
  1892. }
  1893. session->driver.ubx.last_msgid = msgid;
  1894. /* FIXME: last_time never used... */
  1895. session->driver.ubx.last_time = session->newdata.time;
  1896. } else {
  1897. /* no time */
  1898. /* debug
  1899. GPSD_LOG(LOG_ERROR, &session->context->errout,
  1900. "UBX: No time, msgid %x\n", msgid);
  1901. */
  1902. }
  1903. return mask | ONLINE_SET;
  1904. }
  1905. static gps_mask_t parse_input(struct gps_device_t *session)
  1906. {
  1907. if (session->lexer.type == UBX_PACKET) {
  1908. return ubx_parse(session, session->lexer.outbuffer,
  1909. session->lexer.outbuflen);
  1910. } else
  1911. return generic_parse_input(session);
  1912. }
  1913. bool ubx_write(struct gps_device_t * session,
  1914. unsigned int msg_class, unsigned int msg_id,
  1915. unsigned char *msg, size_t data_len)
  1916. {
  1917. unsigned char CK_A, CK_B;
  1918. ssize_t count;
  1919. size_t i;
  1920. bool ok;
  1921. /* do not write if -b (readonly) option set */
  1922. if (session->context->readonly)
  1923. return true;
  1924. session->msgbuf[0] = 0xb5;
  1925. session->msgbuf[1] = 0x62;
  1926. CK_A = CK_B = 0;
  1927. session->msgbuf[2] = msg_class;
  1928. session->msgbuf[3] = msg_id;
  1929. session->msgbuf[4] = data_len & 0xff;
  1930. session->msgbuf[5] = (data_len >> 8) & 0xff;
  1931. assert(msg != NULL || data_len == 0);
  1932. if (msg != NULL)
  1933. (void)memcpy(&session->msgbuf[6], msg, data_len);
  1934. /* calculate CRC */
  1935. for (i = 2; i < 6; i++) {
  1936. CK_A += session->msgbuf[i];
  1937. CK_B += CK_A;
  1938. }
  1939. if (msg != NULL)
  1940. for (i = 0; i < data_len; i++) {
  1941. CK_A += msg[i];
  1942. CK_B += CK_A;
  1943. }
  1944. session->msgbuf[6 + data_len] = CK_A;
  1945. session->msgbuf[7 + data_len] = CK_B;
  1946. session->msgbuflen = data_len + 8;
  1947. GPSD_LOG(LOG_PROG, &session->context->errout,
  1948. "=> GPS: UBX class: %02x, id: %02x, len: %zd, crc: %02x%02x\n",
  1949. msg_class, msg_id, data_len,
  1950. CK_A, CK_B);
  1951. count = gpsd_write(session, session->msgbuf, session->msgbuflen);
  1952. ok = (count == (ssize_t) session->msgbuflen);
  1953. return (ok);
  1954. }
  1955. #ifdef CONTROLSEND_ENABLE
  1956. static ssize_t ubx_control_send(struct gps_device_t *session, char *msg,
  1957. size_t data_len)
  1958. /* not used by gpsd, it's for gpsctl and friends */
  1959. {
  1960. return ubx_write(session, (unsigned int)msg[0], (unsigned int)msg[1],
  1961. (unsigned char *)msg + 2,
  1962. (size_t)(data_len - 2)) ? ((ssize_t) (data_len + 7)) : -1;
  1963. }
  1964. #endif /* CONTROLSEND_ENABLE */
  1965. static void ubx_init_query(struct gps_device_t *session)
  1966. {
  1967. /* UBX-MON-VER: query for version information */
  1968. (void)ubx_write(session, UBX_CLASS_MON, 0x04, NULL, 0);
  1969. }
  1970. static void ubx_event_hook(struct gps_device_t *session, event_t event)
  1971. {
  1972. if (session->context->readonly)
  1973. return;
  1974. else if (event == event_identified) {
  1975. GPSD_LOG(LOG_DATA, &session->context->errout, "UBX identified\n");
  1976. /* no longer set UBX-CFG-SBAS here, u-blox 9 does not have it */
  1977. #ifdef RECONFIGURE_ENABLE
  1978. /*
  1979. * Turn off NMEA output, turn on UBX on this port.
  1980. */
  1981. if (session->mode == O_OPTIMIZE) {
  1982. ubx_mode(session, MODE_BINARY);
  1983. } else {
  1984. ubx_mode(session, MODE_NMEA);
  1985. }
  1986. #endif /* RECONFIGURE_ENABLE */
  1987. } else if (event == event_deactivate) {
  1988. /* There used to be a hotstart/reset here.
  1989. * That caused u-blox USB to re-enumerate.
  1990. * Sometimes to a new device name.
  1991. * Bad. Don't do that anymore...
  1992. */
  1993. }
  1994. }
  1995. #ifdef RECONFIGURE_ENABLE
  1996. static void ubx_cfg_prt(struct gps_device_t *session,
  1997. speed_t speed, const char parity, const int stopbits,
  1998. const int mode)
  1999. /* generate and send a configuration block */
  2000. {
  2001. unsigned long usart_mode = 0;
  2002. unsigned char buf[UBX_CFG_LEN];
  2003. memset(buf, '\0', UBX_CFG_LEN);
  2004. /*
  2005. * When this is called from gpsd, the initial probe for UBX should
  2006. * have picked up the device's port number from the CFG_PRT response.
  2007. */
  2008. /* FIXME! Bad test, port_id == 0 is valid too. DDC (I2X) = port 0 */
  2009. if (session->driver.ubx.port_id != 0) {
  2010. buf[0] = session->driver.ubx.port_id;
  2011. }
  2012. /*
  2013. * This default can be hit if we haven't sent a CFG_PRT query yet,
  2014. * which can happen in gpsmon because it doesn't autoprobe.
  2015. *
  2016. * What we'd like to do here is dispatch to USART1_ID or
  2017. * USB_ID intelligently based on whether this is a USB or RS232
  2018. * source. Unfortunately the GR601-W screws that up by being
  2019. * a USB device with port_id 1. So we bite the bullet and
  2020. * default to port 1.
  2021. *
  2022. * Without further logic, this means gpsmon wouldn't be able to
  2023. * change the speed on the EVK 6H's USB port. But! To pick off
  2024. * the EVK 6H on Linux as a special case, we notice that its
  2025. * USB device name is /dev/ACMx - it presents as a USB modem.
  2026. *
  2027. * This logic will fail on any USB u-blox device that presents
  2028. * as an ordinary USB serial device (/dev/USB*) and actually
  2029. * has port ID 3 the way it "ought" to.
  2030. */
  2031. else if (strstr(session->gpsdata.dev.path, "/ACM") != NULL) {
  2032. /* using the built in USB port */
  2033. session->driver.ubx.port_id = buf[0] = USB_ID;
  2034. } else {
  2035. /* A guess. Could be UART2, or SPI, or DDC port */
  2036. session->driver.ubx.port_id = buf[0] = USART1_ID;
  2037. }
  2038. putle32(buf, 8, speed);
  2039. /*
  2040. * u-blox tech support explains the default contents of the mode
  2041. * field as follows:
  2042. *
  2043. * D0 08 00 00 mode (LSB first)
  2044. *
  2045. * re-ordering bytes: 000008D0
  2046. * dividing into fields: 000000000000000000 00 100 0 11 0 1 0000
  2047. * nStopbits = 00 = 1
  2048. * parity = 100 = none
  2049. * charLen = 11 = 8-bit
  2050. * reserved1 = 1
  2051. *
  2052. * The protocol reference further gives the following subfield values:
  2053. * 01 = 1.5 stop bits (?)
  2054. * 10 = 2 stopbits
  2055. * 000 = even parity
  2056. * 001 = odd parity
  2057. * 10x = no parity
  2058. * 10 = 7 bits
  2059. *
  2060. * Some UBX reference code amplifies this with:
  2061. *
  2062. * prtcfg.mode = (1<<4) | // compatibility with ANTARIS 4
  2063. * (1<<7) | // charLen = 11 = 8 bit
  2064. * (1<<6) | // charLen = 11 = 8 bit
  2065. * (1<<11); // parity = 10x = none
  2066. */
  2067. usart_mode |= (1<<4); /* reserved1 Antaris 4 compatibility bit */
  2068. usart_mode |= (1<<7); /* high bit of charLen */
  2069. /* u-blox 5+ binary only supports 8N1 */
  2070. switch (parity) {
  2071. case (int)'E':
  2072. case 2:
  2073. usart_mode |= (1<<7); /* 7E */
  2074. break;
  2075. case (int)'O':
  2076. case 1:
  2077. usart_mode |= (1<<9) | (1<<7); /* 7O */
  2078. break;
  2079. case (int)'N':
  2080. case 0:
  2081. default:
  2082. usart_mode |= (1<<11) | (3<<6); /* 8N */
  2083. break;
  2084. }
  2085. if (stopbits == 2)
  2086. usart_mode |= (1<<13);
  2087. putle32(buf, 4, usart_mode);
  2088. /* enable all input protocols by default */
  2089. /* FIXME! RTCM3 needs to be set too */
  2090. buf[12] = NMEA_PROTOCOL_MASK | UBX_PROTOCOL_MASK | RTCM_PROTOCOL_MASK;
  2091. /* FIXME? RTCM/RTCM3 needs to be set too? */
  2092. buf[outProtoMask] = (mode == MODE_NMEA
  2093. ? NMEA_PROTOCOL_MASK : UBX_PROTOCOL_MASK);
  2094. (void)ubx_write(session, 0x06u, 0x00, buf, sizeof(buf));
  2095. GPSD_LOG(LOG_DATA, &session->context->errout,
  2096. "UBX ubx_cfg_prt mode:%d, port:%d\n", mode, buf[0]);
  2097. /* selectively enable output protocols */
  2098. if (mode == MODE_NMEA) {
  2099. /*
  2100. * We have to club the GR601-W over the head to make it stop emitting
  2101. * UBX after we've told it to start. Turning off the UBX protocol
  2102. * mask, by itself, seems to be ineffective.
  2103. */
  2104. unsigned char msg[3];
  2105. msg[0] = 0x01; /* class */
  2106. msg[1] = 0x04; /* msg id = UBX-NAV-DOP */
  2107. msg[2] = 0x00; /* rate */
  2108. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2109. /* UBX-NAV-SOL deprecated in u-blox 6, gone in u-blox 9.
  2110. * UBX-NAV-PVT for later models. Turn both off */
  2111. msg[0] = 0x01; /* class */
  2112. msg[1] = 0x06; /* msg id = NAV-SOL */
  2113. msg[2] = 0x00; /* rate */
  2114. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2115. msg[0] = 0x01; /* class */
  2116. msg[1] = 0x07; /* msg id = NAV-PVT */
  2117. msg[2] = 0x00; /* rate */
  2118. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2119. msg[0] = 0x01; /* class */
  2120. msg[1] = 0x20; /* msg id = UBX-NAV-TIMEGPS */
  2121. msg[2] = 0x00; /* rate */
  2122. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2123. /* NAV-SVINFO became NAV-SAT */
  2124. msg[0] = 0x01; /* class */
  2125. msg[1] = 0x30; /* msg id = NAV-SVINFO */
  2126. msg[2] = 0x00; /* rate */
  2127. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2128. msg[0] = 0x01; /* class */
  2129. msg[1] = 0x35; /* msg id = NAV-SAT */
  2130. msg[2] = 0x00; /* rate */
  2131. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2132. msg[0] = 0x01; /* class */
  2133. msg[1] = 0x32; /* msg id = NAV-SBAS */
  2134. msg[2] = 0x00; /* rate */
  2135. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2136. /* UBX-NAV-EOE */
  2137. msg[0] = 0x01; /* class */
  2138. msg[1] = 0x61; /* msg id = NAV-EOE */
  2139. msg[2] = 0x00; /* rate */
  2140. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2141. /* try to improve the sentence mix. in particular by enabling ZDA */
  2142. msg[0] = 0xf0; /* class */
  2143. msg[1] = 0x09; /* msg id = GBS */
  2144. msg[2] = 0x01; /* rate */
  2145. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2146. msg[0] = 0xf0; /* class */
  2147. msg[1] = 0x00; /* msg id = GGA */
  2148. msg[2] = 0x01; /* rate */
  2149. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2150. msg[0] = 0xf0; /* class */
  2151. msg[1] = 0x02; /* msg id = GSA */
  2152. msg[2] = 0x01; /* rate */
  2153. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2154. msg[0] = 0xf0; /* class */
  2155. msg[1] = 0x07; /* msg id = GST */
  2156. msg[2] = 0x01; /* rate */
  2157. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2158. msg[0] = 0xf0; /* class */
  2159. msg[1] = 0x03; /* msg id = GSV */
  2160. msg[2] = 0x01; /* rate */
  2161. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2162. msg[0] = 0xf0; /* class */
  2163. msg[1] = 0x04; /* msg id = RMC */
  2164. msg[2] = 0x01; /* rate */
  2165. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2166. msg[0] = 0xf0; /* class */
  2167. msg[1] = 0x05; /* msg id = VTG */
  2168. msg[2] = 0x01; /* rate */
  2169. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2170. msg[0] = 0xf0; /* class */
  2171. msg[1] = 0x08; /* msg id = ZDA */
  2172. msg[2] = 0x01; /* rate */
  2173. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2174. } else { /* MODE_BINARY */
  2175. /*
  2176. * Just enabling the UBX protocol for output is not enough to
  2177. * actually get UBX output; the sentence mix is initially empty.
  2178. * Fix that...
  2179. */
  2180. /* FIXME: possibly sending too many messages without waiting
  2181. * for u-blox ACK, over running its input buffer.
  2182. *
  2183. * for example, the UBX-MON-VER fails here, but works in other
  2184. * contexts
  2185. */
  2186. unsigned char msg[3] = {0, 0, 0};
  2187. /* request SW and HW Versions */
  2188. (void)ubx_write(session, UBX_CLASS_MON, 0x04, msg, 0);
  2189. msg[0] = 0x01; /* class */
  2190. msg[1] = 0x04; /* msg id = UBX-NAV-DOP */
  2191. msg[2] = 0x01; /* rate */
  2192. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2193. /* UBX-NAV-SOL deprecated in u-blox 6, gone in u-blox 9.
  2194. * Use UBX-NAV-PVT after u-blox 7 */
  2195. if (10 > session->driver.ubx.protver) {
  2196. /* unknown version, enable both */
  2197. msg[0] = 0x01; /* class */
  2198. msg[1] = 0x06; /* msg id = NAV-SOL */
  2199. msg[2] = 0x01; /* rate */
  2200. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2201. msg[0] = 0x01; /* class */
  2202. msg[1] = 0x07; /* msg id = NAV-PVT */
  2203. msg[2] = 0x01; /* rate */
  2204. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2205. } else if (15 > session->driver.ubx.protver) {
  2206. /* before u-blox 8, just NAV-SOL */
  2207. /* do not do both to avoid NACKs */
  2208. msg[0] = 0x01; /* class */
  2209. msg[1] = 0x06; /* msg id = NAV-SOL */
  2210. msg[2] = 0x01; /* rate */
  2211. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2212. } else {
  2213. /* u-blox 8 or later */
  2214. msg[0] = 0x01; /* class */
  2215. msg[1] = 0x07; /* msg id = NAV-PVT */
  2216. msg[2] = 0x01; /* rate */
  2217. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2218. }
  2219. /* UBX-NAV-TIMEGPS is a great cycle ender */
  2220. msg[0] = 0x01; /* class */
  2221. msg[1] = 0x20; /* msg id = UBX-NAV-TIMEGPS */
  2222. msg[2] = 0x01; /* rate */
  2223. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2224. /* UBX-NAV-SVINFO deprecated in u-blox 8, gone in u-blox 9.
  2225. * Use UBX-NAV-SAT after u-blox 7 */
  2226. if (10 > session->driver.ubx.protver) {
  2227. /* unknown version, enable both */
  2228. msg[0] = 0x01; /* class */
  2229. msg[1] = 0x30; /* msg id = NAV-SVINFO */
  2230. msg[2] = 0x0a; /* rate */
  2231. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2232. msg[0] = 0x01; /* class */
  2233. msg[1] = 0x35; /* msg id = NAV-SAT */
  2234. msg[2] = 0x0a; /* rate */
  2235. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2236. } else if (15 > session->driver.ubx.protver) {
  2237. /* before u-blox 8, just NAV-SVINFO */
  2238. /* do not do both to avoid NACKs */
  2239. msg[0] = 0x01; /* class */
  2240. msg[1] = 0x30; /* msg id = NAV-SVINFO */
  2241. msg[2] = 0x0a; /* rate */
  2242. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2243. } else {
  2244. /* u-blox 8 or later */
  2245. msg[0] = 0x01; /* class */
  2246. msg[1] = 0x35; /* msg id = NAV-SAT */
  2247. msg[2] = 0x0a; /* rate */
  2248. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2249. }
  2250. if (24 > session->driver.ubx.protver) {
  2251. /* Gone after u-blox 8 */
  2252. msg[0] = 0x01; /* class */
  2253. msg[1] = 0x32; /* msg id = NAV-SBAS */
  2254. msg[2] = 0x0a; /* rate */
  2255. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2256. }
  2257. msg[0] = 0x01; /* class */
  2258. msg[1] = 0x01; /* msg id = UBX-NAV-POSECEF */
  2259. msg[2] = 0x01; /* rate */
  2260. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2261. msg[0] = 0x01; /* class */
  2262. msg[1] = 0x11; /* msg id = UBX-NAV-VELECEF */
  2263. msg[2] = 0x01; /* rate */
  2264. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2265. msg[0] = 0x01; /* class */
  2266. msg[1] = 0x26; /* msg id = UBX-NAV-TIMELS */
  2267. msg[2] = 0xff; /* about every 4 minutes if nav rate is 1Hz */
  2268. (void)ubx_write(session, 0x06, 0x01, msg, 3);
  2269. if (18 <= session->driver.ubx.protver) {
  2270. /* first in u-blox 8 */
  2271. /* UBX-NAV-EOE makes a good cycle ender */
  2272. msg[0] = 0x01; /* class */
  2273. msg[1] = 0x61; /* msg id = NAV-EOE */
  2274. msg[2] = 0x00; /* rate */
  2275. (void)ubx_write(session, 0x06u, 0x01, msg, 3);
  2276. }
  2277. }
  2278. }
  2279. static void ubx_mode(struct gps_device_t *session, int mode)
  2280. {
  2281. ubx_cfg_prt(session,
  2282. gpsd_get_speed(session),
  2283. gpsd_get_parity(session),
  2284. gpsd_get_stopbits(session),
  2285. mode);
  2286. }
  2287. static bool ubx_speed(struct gps_device_t *session,
  2288. speed_t speed, char parity, int stopbits)
  2289. {
  2290. ubx_cfg_prt(session,
  2291. speed,
  2292. parity,
  2293. stopbits,
  2294. (session->lexer.type == UBX_PACKET) ? MODE_BINARY : MODE_NMEA);
  2295. return true;
  2296. }
  2297. static bool ubx_rate(struct gps_device_t *session, double cycletime)
  2298. /* change the sample rate of the GPS */
  2299. {
  2300. unsigned short s;
  2301. unsigned char msg[6] = {
  2302. 0x00, 0x00, /* U2: Measurement rate (ms) */
  2303. 0x00, 0x01, /* U2: Navigation rate (cycles) */
  2304. 0x00, 0x00, /* U2: Alignment to reference time: 0 = UTC, !0 = GPS */
  2305. };
  2306. /* clamp to cycle times that i know work on my receiver */
  2307. if (cycletime > 1000.0)
  2308. cycletime = 1000.0;
  2309. if (cycletime < 200.0)
  2310. cycletime = 200.0;
  2311. GPSD_LOG(LOG_DATA, &session->context->errout,
  2312. "UBX rate change, report every %f secs\n", cycletime);
  2313. s = (unsigned short)cycletime;
  2314. msg[0] = (unsigned char)(s >> 8);
  2315. msg[1] = (unsigned char)(s & 0xff);
  2316. return ubx_write(session, 0x06, 0x08, msg, 6); /* CFG-RATE */
  2317. }
  2318. #endif /* RECONFIGURE_ENABLE */
  2319. /* This is everything we export */
  2320. /* *INDENT-OFF* */
  2321. const struct gps_type_t driver_ubx = {
  2322. .type_name = "u-blox", /* Full name of type */
  2323. .packet_type = UBX_PACKET, /* associated lexer packet type */
  2324. .flags = DRIVER_STICKY, /* remember this */
  2325. .trigger = NULL,
  2326. /* Number of satellite channels supported by the device */
  2327. .channels = 50,
  2328. .probe_detect = NULL, /* Startup-time device detector */
  2329. /* Packet getter (using default routine) */
  2330. .get_packet = generic_get,
  2331. .parse_packet = parse_input, /* Parse message packets */
  2332. /* RTCM handler (using default routine) */
  2333. .rtcm_writer = gpsd_write,
  2334. .init_query = ubx_init_query, /* non-perturbing initial query */
  2335. .event_hook = ubx_event_hook, /* Fire on various lifetime events */
  2336. #ifdef RECONFIGURE_ENABLE
  2337. .speed_switcher = ubx_speed, /* Speed (baudrate) switch */
  2338. .mode_switcher = ubx_mode, /* Mode switcher */
  2339. .rate_switcher = ubx_rate, /* Message delivery rate switcher */
  2340. .min_cycle.tv_sec = 0, /* not relevant, no rate switch */
  2341. .min_cycle.tv_nsec = 250000000, /* Maximum 4Hz sample rate */
  2342. #endif /* RECONFIGURE_ENABLE */
  2343. #ifdef CONTROLSEND_ENABLE
  2344. .control_send = ubx_control_send,/* how to send a control string */
  2345. #endif /* CONTROLSEND_ENABLE */
  2346. .time_offset = NULL, /* no method for NTP fudge factor */
  2347. };
  2348. /* *INDENT-ON* */
  2349. #endif /* defined(UBLOX_ENABLE) && defined(BINARY_ENABLE) */
  2350. // vim: set expandtab shiftwidth=4