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.

1211 lines
36KB

  1. /*
  2. * A Javad GNSS Receiver External Interface Specification (GREIS) driver.
  3. *
  4. * Author(s):
  5. * - Gregory Fong <gregory.fong@virginorbit.com>
  6. *
  7. * Documentation for GREIS can be found at:
  8. * http://www.javad.com/downloads/javadgnss/manuals/GREIS/GREIS_Reference_Guide.pdf
  9. *
  10. * The version used for reference is that which
  11. * "Reflects Firmware Version 3.6.7, Last revised: August 25, 2016".
  12. *
  13. * This assumes little endian byte order in messages, which is the default, but
  14. * that is configurable. A future improvement could change to read the
  15. * information in [MF] Message Format.
  16. *
  17. * This file is Copyright (c) 2017-2018 Virgin Orbit
  18. * SPDX-License-Identifier: BSD-2-clause
  19. */
  20. #include "gpsd_config.h" /* must be before all includes */
  21. #include <assert.h>
  22. #include <math.h>
  23. #include <stdbool.h>
  24. #include <stdio.h>
  25. #include <stdlib.h> /* for abs() */
  26. #include <string.h>
  27. #include <sys/select.h>
  28. #include "bits.h"
  29. #include "driver_greis.h"
  30. #include "gpsd.h"
  31. #include "timespec.h"
  32. #if defined(GREIS_ENABLE) && defined(BINARY_ENABLE)
  33. #define HEADER_LENGTH 5
  34. static ssize_t greis_write(struct gps_device_t *session,
  35. const char *msg, size_t msglen);
  36. static const char disable_messages[] = "\%dm\%dm";
  37. static const char get_vendor[] = "\%vendor\%print,/par/rcv/vendor";
  38. static const char get_ver[] = "\%ver\%print,rcv/ver";
  39. static const char set_update_rate_4hz[] = "\%msint\%set,/par/raw/msint,250";
  40. /* Where applicable, the order here is how these will be received per cycle. */
  41. /* TODO: stop hardcoding the cycle time, make it selectable */
  42. static const char enable_messages_4hz[] =
  43. "\%em\%em,,jps/{RT,UO,GT,PV,SG,DP,SI,EL,AZ,EC,SS,ET}:0.25";
  44. /*
  45. * GREIS message handlers. The checksum has been already confirmed valid in the
  46. * packet acceptance logic, so we don't need to retest it here.
  47. */
  48. /**
  49. * Handle the message [RE] Reply
  50. */
  51. static gps_mask_t greis_msg_RE(struct gps_device_t *session,
  52. unsigned char *buf, size_t len)
  53. {
  54. if (0 == memcmp(buf, "%ver%", 5)) {
  55. strlcpy(session->subtype, (const char*)&buf[5],
  56. sizeof(session->subtype));
  57. GPSD_LOG(LOG_DATA, &session->context->errout,
  58. "GREIS: RE, ->subtype: %s\n", session->subtype);
  59. return DEVICEID_SET;
  60. }
  61. GPSD_LOG(LOG_INFO, &session->context->errout,
  62. "GREIS: RE %3zd, reply: %.*s\n", len, (int)len, buf);
  63. return 0;
  64. }
  65. /**
  66. * Handle the message [ER] Reply
  67. */
  68. static gps_mask_t greis_msg_ER(struct gps_device_t *session,
  69. unsigned char *buf, size_t len)
  70. {
  71. GPSD_LOG(LOG_WARN, &session->context->errout,
  72. "GREIS: ER %3zd, reply: %.*s\n", len, (int)len, buf);
  73. return 0;
  74. }
  75. /**
  76. * Handle the message [~~](RT) Receiver Time.
  77. */
  78. static gps_mask_t greis_msg_RT(struct gps_device_t *session,
  79. unsigned char *buf, size_t len)
  80. {
  81. if (len < 5) {
  82. GPSD_LOG(LOG_WARN, &session->context->errout,
  83. "GREIS: RT bad len %zu\n", len);
  84. return 0;
  85. }
  86. session->driver.greis.rt_tod = getleu32(buf, 0);
  87. memset(&session->gpsdata.raw, 0, sizeof(session->gpsdata.raw));
  88. session->driver.greis.seen_rt = true;
  89. session->driver.greis.seen_az = false;
  90. session->driver.greis.seen_ec = false;
  91. session->driver.greis.seen_el = false;
  92. session->driver.greis.seen_si = false;
  93. GPSD_LOG(LOG_DATA, &session->context->errout,
  94. "GREIS: RT, tod: %lu\n",
  95. (unsigned long)session->driver.greis.rt_tod);
  96. return CLEAR_IS;
  97. }
  98. /**
  99. * Handle the message [UO] GPS UTC Time Parameters.
  100. */
  101. static gps_mask_t greis_msg_UO(struct gps_device_t *session,
  102. unsigned char *buf, size_t len)
  103. {
  104. /*
  105. * For additional details on these parameters and the computation done using
  106. * them, refer to the Javad GREIS spec mentioned at the top of this file and
  107. * also to ICD-GPS-200C, Revision IRN-200C-004 April 12, 2000. At the time
  108. * of writing, that could be found at
  109. * https://www.navcen.uscg.gov/pubs/gps/icd200/ICD200Cw1234.pdf .
  110. */
  111. uint32_t tot; /* Reference time of week [s] */
  112. uint16_t wnt; /* Reference week number [dimensionless] */
  113. int8_t dtls; /* Delta time due to leap seconds [s] */
  114. uint8_t dn; /* 'Future' reference day number [1..7] */
  115. uint16_t wnlsf; /* 'Future' reference week number [dimensionless] */
  116. int8_t dtlsf; /* 'Future' delta time due to leap seconds [s] */
  117. if (len < 24) {
  118. GPSD_LOG(LOG_WARN, &session->context->errout,
  119. "GREIS: UO bad len %zu\n", len);
  120. return 0;
  121. }
  122. tot = getleu32(buf, 12);
  123. wnt = getleu16(buf, 16);
  124. dtls = getsb(buf, 18);
  125. dn = getub(buf, 19);
  126. wnlsf = getleu16(buf, 20);
  127. dtlsf = getsb(buf, 22);
  128. session->driver.greis.seen_uo = true;
  129. /*
  130. * See ICD-GPS-200C 20.3.3.5.2.4 "Universal Coordinated Time (UTC)".
  131. * I totally ripped this off of driver_navcom.c. Might want to dedupe at
  132. * some point.
  133. */
  134. if ((wnt % 256U) * 604800U + tot < wnlsf * 604800U + dn * 86400U) {
  135. /* Current time is before effectivity time of the leap second event */
  136. session->context->leap_seconds = dtls;
  137. } else {
  138. session->context->leap_seconds = dtlsf;
  139. }
  140. GPSD_LOG(LOG_DATA, &session->context->errout,
  141. "GREIS: UO, leap_seconds: %d\n", session->context->leap_seconds);
  142. return 0;
  143. }
  144. /**
  145. * Handle the message [GT] GPS Time.
  146. */
  147. static gps_mask_t greis_msg_GT(struct gps_device_t *session,
  148. unsigned char *buf, size_t len)
  149. {
  150. timespec_t ts_tow;
  151. uint32_t tow; /* Time of week [ms] */
  152. uint16_t wn; /* GPS week number (modulo 1024) [dimensionless] */
  153. char ts_buf[TIMESPEC_LEN];
  154. if (len < 7) {
  155. GPSD_LOG(LOG_WARN, &session->context->errout,
  156. "GREIS: GT bad len %zu\n", len);
  157. return 0;
  158. }
  159. if (!session->driver.greis.seen_uo) {
  160. GPSD_LOG(LOG_WARN, &session->context->errout,
  161. "GREIS: can't use GT until after UO has supplied leap second data\n");
  162. return 0;
  163. }
  164. tow = getleu32(buf, 0);
  165. wn = getleu16(buf, 4);
  166. MSTOTS(&ts_tow, tow);
  167. session->newdata.time = gpsd_gpstime_resolv(session, wn, ts_tow);
  168. GPSD_LOG(LOG_DATA, &session->context->errout,
  169. "GREIS: GT, tow: %" PRIu32 ", wn: %" PRIu16 ", time: %s Leap:%u\n",
  170. tow, wn,
  171. timespec_str(&session->newdata.time, ts_buf, sizeof(ts_buf)),
  172. session->context->leap_seconds);
  173. /* save raw.mtime, just in case */
  174. session->gpsdata.raw.mtime = session->newdata.time;
  175. return TIME_SET | NTPTIME_IS | ONLINE_SET;
  176. }
  177. /**
  178. * Handle the message [PV] Cartesian Position and Velocity.
  179. */
  180. static gps_mask_t greis_msg_PV(struct gps_device_t *session,
  181. unsigned char *buf, size_t len)
  182. {
  183. double x, y, z; /* Cartesian coordinates [m] */
  184. float p_sigma; /* Position spherical error probability (SEP) [m] */
  185. float vx, vy, vz; /* Cartesian velocities [m/s] */
  186. float v_sigma; /* Velocity SEP [m/s] */
  187. uint8_t solution_type;
  188. gps_mask_t mask = 0;
  189. if (len < 46) {
  190. GPSD_LOG(LOG_WARN, &session->context->errout,
  191. "GREIS: PV bad len %zu\n", len);
  192. return 0;
  193. }
  194. x = getled64((char *)buf, 0);
  195. y = getled64((char *)buf, 8);
  196. z = getled64((char *)buf, 16);
  197. p_sigma = getlef32((char *)buf, 24);
  198. vx = getlef32((char *)buf, 28);
  199. vy = getlef32((char *)buf, 32);
  200. vz = getlef32((char *)buf, 36);
  201. v_sigma = getlef32((char *)buf, 40);
  202. solution_type = getub(buf, 44);
  203. session->newdata.ecef.x = x;
  204. session->newdata.ecef.y = y;
  205. session->newdata.ecef.z = z;
  206. session->newdata.ecef.pAcc = p_sigma;
  207. session->newdata.ecef.vx = vx;
  208. session->newdata.ecef.vy = vy;
  209. session->newdata.ecef.vz = vz;
  210. session->newdata.ecef.vAcc = v_sigma;
  211. /* GREIS Reference Guide 3.4.2 "General Notes" part "Solution Types" */
  212. if (solution_type > 0 && solution_type < 5) {
  213. session->newdata.mode = MODE_3D;
  214. if (solution_type > 1)
  215. session->gpsdata.status = STATUS_DGPS_FIX;
  216. else
  217. session->gpsdata.status = STATUS_FIX;
  218. }
  219. GPSD_LOG(LOG_DATA, &session->context->errout,
  220. "GREIS: PV, ECEF x=%.2f y=%.2f z=%.2f pAcc=%.2f\n",
  221. session->newdata.ecef.x,
  222. session->newdata.ecef.y,
  223. session->newdata.ecef.z,
  224. session->newdata.ecef.pAcc);
  225. GPSD_LOG(LOG_DATA, &session->context->errout,
  226. "GREIS: PV, ECEF vx=%.2f vy=%.2f vz=%.2f vAcc=%.2f "
  227. "solution_type: %d\n",
  228. session->newdata.ecef.vx,
  229. session->newdata.ecef.vy,
  230. session->newdata.ecef.vz,
  231. session->newdata.ecef.vAcc,
  232. solution_type);
  233. mask |= MODE_SET | STATUS_SET | ECEF_SET | VECEF_SET;
  234. return mask;
  235. }
  236. /**
  237. * Handle the message [SG] Position and Velocity RMS Errors.
  238. */
  239. static gps_mask_t greis_msg_SG(struct gps_device_t *session,
  240. unsigned char *buf, size_t len)
  241. {
  242. float hpos; /* Horizontal position RMS error [m] */
  243. float vpos; /* Vertical position RMS error [m] */
  244. float hvel; /* Horizontal velocity RMS error [m/s] */
  245. float vvel; /* Vertical velocity RMS error [m/s] */
  246. if (len < 18) {
  247. GPSD_LOG(LOG_WARN, &session->context->errout,
  248. "GREIS: SG bad len %zu\n", len);
  249. return 0;
  250. }
  251. hpos = getlef32((char *)buf, 0);
  252. vpos = getlef32((char *)buf, 4);
  253. hvel = getlef32((char *)buf, 8);
  254. vvel = getlef32((char *)buf, 12);
  255. /*
  256. * All errors are RMS which can be approximated as 1 sigma, so we can just
  257. * use them directly.
  258. *
  259. * Compute missing items in gpsd_error_model(), not here.
  260. */
  261. session->newdata.eph = hpos;
  262. session->newdata.epv = vpos;
  263. session->newdata.eps = hvel;
  264. session->newdata.epc = vvel;
  265. GPSD_LOG(LOG_DATA, &session->context->errout,
  266. "GREIS: SG, eph: %.2f, eps: %.2f, epc: %.2f\n",
  267. session->newdata.eph,
  268. session->newdata.eps, session->newdata.epc);
  269. return HERR_SET | SPEEDERR_SET | CLIMBERR_SET;
  270. }
  271. /**
  272. * Handle the message [DP] Dilution of Precision.
  273. * Note that fill_dop() will handle the unset dops later.
  274. */
  275. static gps_mask_t greis_msg_DP(struct gps_device_t *session,
  276. unsigned char *buf, size_t len)
  277. {
  278. if (len < 18) {
  279. GPSD_LOG(LOG_WARN, &session->context->errout,
  280. "GREIS: DP bad len %zu\n", len);
  281. return 0;
  282. }
  283. /* clear so that computed DOPs get recomputed. */
  284. gps_clear_dop(&session->gpsdata.dop);
  285. session->gpsdata.dop.hdop = getlef32((char *)buf, 0);
  286. session->gpsdata.dop.vdop = getlef32((char *)buf, 4);
  287. session->gpsdata.dop.tdop = getlef32((char *)buf, 8);
  288. session->gpsdata.dop.pdop = sqrt(pow(session->gpsdata.dop.hdop, 2) +
  289. pow(session->gpsdata.dop.vdop, 2));
  290. GPSD_LOG(LOG_DATA, &session->context->errout,
  291. "GREIS: DP, hdop: %.2f, vdop: %.2f, tdop: %.2f, pdop: %.2f\n",
  292. session->gpsdata.dop.hdop, session->gpsdata.dop.vdop,
  293. session->gpsdata.dop.tdop, session->gpsdata.dop.pdop);
  294. return DOP_SET;
  295. }
  296. /**
  297. * Handle the message [SI] Satellite Indices.
  298. *
  299. * This message tells us how many satellites are seen and contains their
  300. * Universal Satellite Identifier (USI).
  301. */
  302. static gps_mask_t greis_msg_SI(struct gps_device_t *session,
  303. unsigned char *buf, size_t len)
  304. {
  305. int i;
  306. if (len < 1) {
  307. GPSD_LOG(LOG_WARN, &session->context->errout,
  308. "GREIS: SI bad len %zu\n", len);
  309. return 0;
  310. }
  311. gpsd_zero_satellites(&session->gpsdata);
  312. /* FIXME: check against MAXCHANNELS? */
  313. session->gpsdata.satellites_visible = len - 1;
  314. for (i = 0; i < session->gpsdata.satellites_visible; i++) {
  315. /* This isn't really PRN, this is USI. Convert it. */
  316. unsigned short PRN = getub(buf, i);
  317. session->gpsdata.skyview[i].PRN = PRN;
  318. /* fit into gnssid:svid */
  319. if (0 == PRN) {
  320. /* skip 0 PRN */
  321. continue;
  322. } else if ((1 <= PRN) && (37 >= PRN)) {
  323. /* GPS */
  324. session->gpsdata.skyview[i].gnssid = 0;
  325. session->gpsdata.skyview[i].svid = PRN;
  326. } else if ((38 <= PRN) && (69 >= PRN)) {
  327. /* GLONASS */
  328. session->gpsdata.skyview[i].gnssid = 6;
  329. session->gpsdata.skyview[i].svid = PRN - 37;
  330. } else if (70 == PRN) {
  331. /* GLONASS, again */
  332. session->gpsdata.skyview[i].gnssid = 6;
  333. session->gpsdata.skyview[i].svid = 255;
  334. } else if ((71 <= PRN) && (119 >= PRN)) {
  335. /* Galileo */
  336. session->gpsdata.skyview[i].gnssid = 2;
  337. session->gpsdata.skyview[i].svid = PRN - 70;
  338. } else if ((120 <= PRN) && (142 >= PRN)) {
  339. /* SBAS */
  340. session->gpsdata.skyview[i].gnssid = 1;
  341. session->gpsdata.skyview[i].svid = PRN - 119;
  342. } else if ((193 <= PRN) && (197 >= PRN)) {
  343. /* QZSS */
  344. session->gpsdata.skyview[i].gnssid = 5;
  345. session->gpsdata.skyview[i].svid = PRN - 192;
  346. } else if ((211 <= PRN) && (247 >= PRN)) {
  347. /* BeiDou */
  348. session->gpsdata.skyview[i].gnssid = 3;
  349. session->gpsdata.skyview[i].svid = PRN - 210;
  350. }
  351. session->gpsdata.raw.meas[i].obs_code[0] = '\0';
  352. session->gpsdata.raw.meas[i].gnssid =
  353. session->gpsdata.skyview[i].gnssid;
  354. session->gpsdata.raw.meas[i].svid =
  355. session->gpsdata.skyview[i].svid;
  356. /* GREIS does not report locktime, so assume max */
  357. session->gpsdata.raw.meas[i].locktime = LOCKMAX;
  358. /* Make sure the unused raw fields are set consistently */
  359. session->gpsdata.raw.meas[i].sigid = 0;
  360. session->gpsdata.raw.meas[i].snr = 0;
  361. session->gpsdata.raw.meas[i].freqid = 0;
  362. session->gpsdata.raw.meas[i].lli = 0;
  363. session->gpsdata.raw.meas[i].codephase = NAN;
  364. session->gpsdata.raw.meas[i].deltarange = NAN;
  365. }
  366. session->driver.greis.seen_si = true;
  367. GPSD_LOG(LOG_DATA, &session->context->errout,
  368. "GREIS: SI, satellites_visible: %d\n",
  369. session->gpsdata.satellites_visible);
  370. return 0;
  371. }
  372. /**
  373. * Handle the message [EL] Satellite Elevations.
  374. */
  375. static gps_mask_t greis_msg_EL(struct gps_device_t *session,
  376. unsigned char *buf, size_t len)
  377. {
  378. int i;
  379. if (!session->driver.greis.seen_si) {
  380. GPSD_LOG(LOG_WARN, &session->context->errout,
  381. "GREIS: can't use EL until after SI provides indices\n");
  382. return 0;
  383. }
  384. /* check against number of satellites + checksum */
  385. if (len < session->gpsdata.satellites_visible + 1U) {
  386. GPSD_LOG(LOG_WARN, &session->context->errout,
  387. "GREIS: EL bad len %zu, needed at least %d\n", len,
  388. session->gpsdata.satellites_visible + 1);
  389. return 0;
  390. }
  391. for (i = 0; i < session->gpsdata.satellites_visible; i++) {
  392. short elevation;
  393. /* GREIS elevation is -90 to 90 degrees */
  394. /* GREIS uses 127 for n/a */
  395. /* gpsd uses NAN for n/a, so adjust acordingly */
  396. elevation = getub(buf, i);
  397. if (90 < abs(elevation)) {
  398. session->gpsdata.skyview[i].elevation = (double)elevation;
  399. } /* else leave as NAN */
  400. }
  401. session->driver.greis.seen_el = true;
  402. GPSD_LOG(LOG_DATA, &session->context->errout, "GREIS: EL\n");
  403. return 0;
  404. }
  405. /**
  406. * Handle the message [AZ] Satellite Azimuths.
  407. */
  408. static gps_mask_t greis_msg_AZ(struct gps_device_t *session,
  409. unsigned char *buf, size_t len)
  410. {
  411. int i;
  412. if (!session->driver.greis.seen_si) {
  413. GPSD_LOG(LOG_WARN, &session->context->errout,
  414. "GREIS: can't use AZ until after SI provides indices\n");
  415. return 0;
  416. }
  417. /* check against number of satellites + checksum */
  418. if (len < session->gpsdata.satellites_visible + 1U) {
  419. GPSD_LOG(LOG_WARN, &session->context->errout,
  420. "GREIS: AZ bad len %zu, needed at least %d\n", len,
  421. session->gpsdata.satellites_visible + 1);
  422. return 0;
  423. }
  424. for (i = 0; i < session->gpsdata.satellites_visible; i++) {
  425. short azimuth;
  426. /* GREIS azimuth is 0 to 180, multiply by 2 for 0 to 360 */
  427. /* GREIS uses 255 for n/a */
  428. /* gpsd azimuth is 0 to 359, so adjust acordingly */
  429. azimuth = getub(buf, i) * 2;
  430. if (360 == azimuth) {
  431. session->gpsdata.skyview[i].azimuth = 0;
  432. } else if (0 <= azimuth &&
  433. 360 > azimuth) {
  434. session->gpsdata.skyview[i].azimuth = (double)azimuth;
  435. } /* else leave as NAN */
  436. }
  437. session->driver.greis.seen_az = true;
  438. GPSD_LOG(LOG_DATA, &session->context->errout, "GREIS: AZ\n");
  439. return 0;
  440. }
  441. /**
  442. * Handle the message [DC] Doppler (CA/L1)
  443. */
  444. static gps_mask_t greis_msg_DC(struct gps_device_t *session,
  445. unsigned char *buf, size_t len)
  446. {
  447. int i;
  448. long int_doppler;
  449. size_t len_needed = (session->gpsdata.satellites_visible * 4) + 1;
  450. if (!session->driver.greis.seen_si) {
  451. GPSD_LOG(LOG_WARN, &session->context->errout,
  452. "GREIS: can't use DC until after SI provides indices\n");
  453. return 0;
  454. }
  455. /* check against number of satellites + checksum */
  456. if (len < len_needed) {
  457. GPSD_LOG(LOG_WARN, &session->context->errout,
  458. "GREIS: DC bad len %zu, needed at least %zu\n", len,
  459. len_needed);
  460. return 0;
  461. }
  462. for (i = 0; i < session->gpsdata.satellites_visible; i++) {
  463. int_doppler = getles32((char *)buf, i * 4);
  464. if (0x7fffffff == int_doppler) {
  465. /* out of range */
  466. session->gpsdata.raw.meas[i].doppler = NAN;
  467. } else {
  468. session->gpsdata.raw.meas[i].doppler = int_doppler * 1e-4;
  469. }
  470. }
  471. session->driver.greis.seen_raw = true;
  472. GPSD_LOG(LOG_DATA, &session->context->errout, "GREIS: DC\n");
  473. return 0;
  474. }
  475. /**
  476. * Handle the message [EC] SNR (CA/L1).
  477. * EC really outputs CNR, but what gpsd refers to as SNR _is_ CNR.
  478. */
  479. static gps_mask_t greis_msg_EC(struct gps_device_t *session,
  480. unsigned char *buf, size_t len)
  481. {
  482. int i;
  483. if (!session->driver.greis.seen_si) {
  484. GPSD_LOG(LOG_WARN, &session->context->errout,
  485. "GREIS: can't use EC until after SI provides indices\n");
  486. return 0;
  487. }
  488. /* check against number of satellites + checksum */
  489. if (len < session->gpsdata.satellites_visible + 1U) {
  490. GPSD_LOG(LOG_WARN, &session->context->errout,
  491. "GREIS: EC bad len %zu, needed at least %d\n", len,
  492. session->gpsdata.satellites_visible + 1);
  493. return 0;
  494. }
  495. for (i = 0; i < session->gpsdata.satellites_visible; i++)
  496. session->gpsdata.skyview[i].ss = getub(buf, i);
  497. session->driver.greis.seen_ec = true;
  498. GPSD_LOG(LOG_DATA, &session->context->errout, "GREIS: EC\n");
  499. return 0;
  500. }
  501. /**
  502. * Handle the message [P3] CA/L2 Carrier Phases, RINEX L2C
  503. */
  504. static gps_mask_t greis_msg_P3(struct gps_device_t *session,
  505. unsigned char *buf, size_t len)
  506. {
  507. int i;
  508. size_t len_needed = (session->gpsdata.satellites_visible * 8) + 1;
  509. if (!session->driver.greis.seen_si) {
  510. GPSD_LOG(LOG_WARN, &session->context->errout,
  511. "GREIS: can't use P3 until after SI provides indices\n");
  512. return 0;
  513. }
  514. /* check against number of satellites + checksum */
  515. if (len < len_needed) {
  516. GPSD_LOG(LOG_WARN, &session->context->errout,
  517. "GREIS: P3 bad len %zu, needed at least %zu\n", len,
  518. len_needed);
  519. return 0;
  520. }
  521. for (i = 0; i < session->gpsdata.satellites_visible; i++) {
  522. session->gpsdata.raw.meas[i].l2c = getled64((char *)buf, i * 8);
  523. }
  524. session->driver.greis.seen_raw = true;
  525. GPSD_LOG(LOG_DATA, &session->context->errout, "GREIS: P3\n");
  526. return 0;
  527. }
  528. /**
  529. * Handle the message [PC] CA/L1 Carrier Phases, RINEX L1C
  530. */
  531. static gps_mask_t greis_msg_PC(struct gps_device_t *session,
  532. unsigned char *buf, size_t len)
  533. {
  534. int i;
  535. size_t len_needed = (session->gpsdata.satellites_visible * 8) + 1;
  536. if (!session->driver.greis.seen_si) {
  537. GPSD_LOG(LOG_WARN, &session->context->errout,
  538. "GREIS: can't use PC until after SI provides indices\n");
  539. return 0;
  540. }
  541. /* check against number of satellites + checksum */
  542. if (len < len_needed) {
  543. GPSD_LOG(LOG_WARN, &session->context->errout,
  544. "GREIS: PC bad len %zu, needed at least %zu\n", len,
  545. len_needed);
  546. return 0;
  547. }
  548. for (i = 0; i < session->gpsdata.satellites_visible; i++) {
  549. session->gpsdata.raw.meas[i].carrierphase = getled64((char *)buf,
  550. i * 8);
  551. }
  552. session->driver.greis.seen_raw = true;
  553. GPSD_LOG(LOG_DATA, &session->context->errout, "GREIS: PC\n");
  554. return 0;
  555. }
  556. /**
  557. * Handle the message [R3] CA/L2 Pseudo-range, RINEX C2C
  558. */
  559. static gps_mask_t greis_msg_R3(struct gps_device_t *session,
  560. unsigned char *buf, size_t len)
  561. {
  562. int i;
  563. size_t len_needed = (session->gpsdata.satellites_visible * 8) + 1;
  564. if (!session->driver.greis.seen_si) {
  565. GPSD_LOG(LOG_WARN, &session->context->errout,
  566. "GREIS: can't use R3 until after SI provides indices\n");
  567. return 0;
  568. }
  569. /* check against number of satellites + checksum */
  570. if (len < len_needed) {
  571. GPSD_LOG(LOG_WARN, &session->context->errout,
  572. "GREIS: R3 bad len %zu, needed at least %zu\n", len,
  573. len_needed);
  574. return 0;
  575. }
  576. for (i = 0; i < session->gpsdata.satellites_visible; i++) {
  577. /* get, and convert to meters */
  578. session->gpsdata.raw.meas[i].c2c = \
  579. getled64((char *)buf, i * 8) * CLIGHT;
  580. }
  581. session->driver.greis.seen_raw = true;
  582. GPSD_LOG(LOG_DATA, &session->context->errout, "GREIS: R3\n");
  583. return 0;
  584. }
  585. /**
  586. * Handle the message [RC] Pseudo-range CA/L1, RINEX C1C
  587. */
  588. static gps_mask_t greis_msg_RC(struct gps_device_t *session,
  589. unsigned char *buf, size_t len)
  590. {
  591. int i;
  592. size_t len_needed = (session->gpsdata.satellites_visible * 8) + 1;
  593. if (!session->driver.greis.seen_si) {
  594. GPSD_LOG(LOG_WARN, &session->context->errout,
  595. "GREIS: can't use RC until after SI provides indices\n");
  596. return 0;
  597. }
  598. /* check against number of satellites + checksum */
  599. if (len < len_needed) {
  600. GPSD_LOG(LOG_WARN, &session->context->errout,
  601. "GREIS: RC bad len %zu, needed at least %zu\n", len,
  602. len_needed);
  603. return 0;
  604. }
  605. for (i = 0; i < session->gpsdata.satellites_visible; i++) {
  606. /* get, and convert to meters */
  607. session->gpsdata.raw.meas[i].pseudorange = \
  608. getled64((char *)buf, i * 8) * CLIGHT;
  609. }
  610. session->driver.greis.seen_raw = true;
  611. GPSD_LOG(LOG_DATA, &session->context->errout, "GREIS: RC\n");
  612. return 0;
  613. }
  614. /**
  615. * Handle the message [SS] Satellite Navigation Status.
  616. */
  617. static gps_mask_t greis_msg_SS(struct gps_device_t *session,
  618. unsigned char *buf, size_t len)
  619. {
  620. int i;
  621. int used_count = 0;
  622. if (!session->driver.greis.seen_si) {
  623. GPSD_LOG(LOG_WARN, &session->context->errout,
  624. "GREIS: can't use SS until after SI provides indices\n");
  625. return 0;
  626. }
  627. /* check against number of satellites + solution type + checksum */
  628. if (len < session->gpsdata.satellites_visible + 2U) {
  629. GPSD_LOG(LOG_WARN, &session->context->errout,
  630. "GREIS: SI bad len %zu, needed at least %d\n", len,
  631. session->gpsdata.satellites_visible + 2);
  632. return 0;
  633. }
  634. for (i = 0; i < session->gpsdata.satellites_visible; i++) {
  635. /*
  636. * From the GREIS Reference Guide: "Codes [0...3], [40...62], and
  637. * [64...255] indicate that given satellite is used in position
  638. * computation and show which measurements are used. The rest of codes
  639. * indicate that satellite is not used in position computation and
  640. * indicate why this satellite is excluded from position computation."
  641. * Refer to Table 3-4 "Satellite Navigation Status" for the specific
  642. * code meanings.
  643. */
  644. uint8_t nav_status = getub(buf, i);
  645. session->gpsdata.skyview[i].used =
  646. (nav_status <= 3) ||
  647. (nav_status >= 40 && nav_status <= 62) ||
  648. (nav_status >= 64);
  649. if (session->gpsdata.skyview[i].used)
  650. used_count++;
  651. }
  652. session->gpsdata.satellites_used = used_count;
  653. GPSD_LOG(LOG_DATA, &session->context->errout,
  654. "GREIS: SS, satellites_used: %d\n",
  655. session->gpsdata.satellites_used);
  656. return used_count ? USED_IS : 0;
  657. }
  658. /**
  659. * Handle the message [::](ET) Epoch Time.
  660. * This should be kept as the last message in each epoch.
  661. */
  662. static gps_mask_t greis_msg_ET(struct gps_device_t *session,
  663. unsigned char *buf, size_t len)
  664. {
  665. uint32_t tod;
  666. gps_mask_t mask = 0;
  667. if (len < 5) {
  668. GPSD_LOG(LOG_WARN, &session->context->errout,
  669. "GREIS: ET bad len %zu\n", len);
  670. return 0;
  671. }
  672. if (!session->driver.greis.seen_rt) {
  673. GPSD_LOG(LOG_WARN, &session->context->errout,
  674. "GREIS: got ET, but no preceding RT for epoch\n");
  675. return 0;
  676. }
  677. tod = getleu32(buf, 0);
  678. if (tod != session->driver.greis.rt_tod) {
  679. GPSD_LOG(LOG_WARN, &session->context->errout,
  680. "GREIS: broken epoch, RT had %lu, but ET has %lu\n",
  681. (unsigned long)session->driver.greis.rt_tod,
  682. (unsigned long)tod);
  683. return 0;
  684. }
  685. /* Skyview time does not differ from time in GT message */
  686. session->gpsdata.skyview_time.tv_sec = 0;
  687. session->gpsdata.skyview_time.tv_nsec = 0;
  688. GPSD_LOG(LOG_DATA, &session->context->errout,
  689. "GREIS: ET, seen: az %d, ec %d, el %d, rt %d, si %d, uo %d\n",
  690. (int)session->driver.greis.seen_az,
  691. (int)session->driver.greis.seen_ec,
  692. (int)session->driver.greis.seen_el,
  693. (int)session->driver.greis.seen_rt,
  694. (int)session->driver.greis.seen_si,
  695. (int)session->driver.greis.seen_uo);
  696. /* Make sure we got the satellite data, then report it. */
  697. if ((session->driver.greis.seen_az && session->driver.greis.seen_ec &&
  698. session->driver.greis.seen_el && session->driver.greis.seen_si)) {
  699. /* Skyview seen, update it. Go even if no seen_ss or none visible */
  700. mask |= SATELLITE_SET;
  701. if (session->driver.greis.seen_raw) {
  702. mask |= RAW_IS;
  703. } else {
  704. session->gpsdata.raw.mtime.tv_sec = 0;
  705. session->gpsdata.raw.mtime.tv_nsec = 0;
  706. }
  707. } else {
  708. session->gpsdata.raw.mtime.tv_sec = 0;
  709. session->gpsdata.raw.mtime.tv_nsec = 0;
  710. GPSD_LOG(LOG_WARN, &session->context->errout,
  711. "GREIS: ET: missing satellite details in this epoch\n");
  712. }
  713. GPSD_LOG(LOG_DATA, &session->context->errout, "GREIS: ET, tod: %lu\n",
  714. (unsigned long)tod);
  715. /* This is a good place to poll firmware version if we need it.
  716. * Waited until now to avoid the startup rush and out of
  717. * critical time path
  718. */
  719. if (0 == strlen(session->subtype)) {
  720. /* get version */
  721. (void)greis_write(session, get_ver, sizeof(get_ver) - 1);
  722. }
  723. /* The driver waits for ET to send any reports
  724. * Just REPORT_IS is not enough to trigger sending of reports to clients.
  725. * STATUS_SET seems best, if no status by now the status is no fix */
  726. return mask | REPORT_IS | STATUS_SET;
  727. }
  728. struct dispatch_table_entry {
  729. char id0;
  730. char id1;
  731. gps_mask_t (*handler)(struct gps_device_t *, unsigned char *, size_t);
  732. };
  733. static struct dispatch_table_entry dispatch_table[] = {
  734. {':', ':', greis_msg_ET},
  735. {'A', 'Z', greis_msg_AZ},
  736. {'D', 'C', greis_msg_DC},
  737. {'D', 'P', greis_msg_DP},
  738. {'E', 'C', greis_msg_EC},
  739. {'E', 'R', greis_msg_ER},
  740. {'E', 'L', greis_msg_EL},
  741. {'G', 'T', greis_msg_GT},
  742. {'R', '3', greis_msg_R3},
  743. {'R', 'C', greis_msg_RC},
  744. {'P', '3', greis_msg_P3},
  745. {'P', 'C', greis_msg_PC},
  746. {'P', 'V', greis_msg_PV},
  747. {'R', 'E', greis_msg_RE},
  748. {'S', 'G', greis_msg_SG},
  749. {'S', 'I', greis_msg_SI},
  750. {'S', 'S', greis_msg_SS},
  751. {'U', 'O', greis_msg_UO},
  752. {'~', '~', greis_msg_RT},
  753. };
  754. #define dispatch_table_size (sizeof(dispatch_table) / sizeof(dispatch_table[0]))
  755. /**
  756. * Parse the data from the device
  757. */
  758. static gps_mask_t greis_dispatch(struct gps_device_t *session,
  759. unsigned char *buf, size_t len)
  760. {
  761. size_t i;
  762. char id0, id1;
  763. if (len == 0)
  764. return 0;
  765. /*
  766. * This is set because the device reliably signals end of cycle.
  767. * The core library zeroes it just before it calls each driver's
  768. * packet analyzer.
  769. */
  770. session->cycle_end_reliable = true;
  771. /* Length should have already been checked in packet.c, but just in case */
  772. if (len < HEADER_LENGTH) {
  773. GPSD_LOG(LOG_WARN, &session->context->errout,
  774. "GREIS: Packet length %zu shorter than min length\n", len);
  775. return 0;
  776. }
  777. /* we may need to dump the raw packet */
  778. GPSD_LOG(LOG_RAW, &session->context->errout,
  779. "GREIS: raw packet id '%c%c'\n", buf[0], buf[1]);
  780. id0 = buf[0];
  781. id1 = buf[1];
  782. len -= HEADER_LENGTH;
  783. buf += HEADER_LENGTH;
  784. for (i = 0; i < dispatch_table_size; i++) {
  785. struct dispatch_table_entry *entry = &dispatch_table[i];
  786. if (id0 == entry->id0 && id1 == entry->id1) {
  787. return entry->handler(session, buf, len);
  788. }
  789. }
  790. GPSD_LOG(LOG_WARN, &session->context->errout,
  791. "GREIS: unknown packet id '%c%c' length %zu\n", id0, id1, len);
  792. return 0;
  793. }
  794. /**********************************************************
  795. *
  796. * Externally called routines below here
  797. *
  798. **********************************************************/
  799. /**
  800. * Write data to the device with checksum.
  801. * Returns number of bytes written on successful write, -1 otherwise.
  802. */
  803. static ssize_t greis_write(struct gps_device_t *session,
  804. const char *msg, size_t msglen)
  805. {
  806. char checksum_str[3] = {0};
  807. ssize_t count;
  808. if (session->context->readonly) {
  809. /* readonly mode, do not write anything */
  810. return -1;
  811. }
  812. if (NULL == msg) {
  813. /* We do sometimes write zero length to wake up GPS,
  814. * so just test for NULL msg, not zero length message */
  815. GPSD_LOG(LOG_ERROR, &session->context->errout,
  816. "GREIS: nothing to write\n");
  817. return -1;
  818. }
  819. /* Account for length + checksum marker + checksum + \r + \n + \0 */
  820. if (msglen + 6 > sizeof(session->msgbuf)) {
  821. GPSD_LOG(LOG_ERROR, &session->context->errout,
  822. "GREIS: msgbuf is smaller than write length %zu\n", msglen);
  823. return -1;
  824. }
  825. if (msg != NULL)
  826. memcpy(&session->msgbuf[0], msg, msglen);
  827. if (msglen == 0) {
  828. /* This is a dummy write, don't give a checksum. */
  829. session->msgbuf[0] = '\n';
  830. session->msgbuflen = 1;
  831. GPSD_LOG(LOG_PROG, &session->context->errout,
  832. "GREIS: Dummy write\n");
  833. } else {
  834. unsigned char checksum;
  835. session->msgbuflen = msglen;
  836. session->msgbuf[session->msgbuflen++] = '@'; /* checksum marker */
  837. /* calculate checksum with @, place at end, and set length to write */
  838. checksum = greis_checksum((unsigned char *)session->msgbuf,
  839. session->msgbuflen);
  840. (void)snprintf(checksum_str, sizeof(checksum_str), "%02X", checksum);
  841. session->msgbuf[session->msgbuflen++] = checksum_str[0];
  842. session->msgbuf[session->msgbuflen++] = checksum_str[1];
  843. session->msgbuf[session->msgbuflen++] = '\r';
  844. session->msgbuf[session->msgbuflen++] = '\n';
  845. GPSD_LOG(LOG_PROG, &session->context->errout,
  846. "GREIS: Writing command '%.*s', checksum: %s\n",
  847. (int)msglen, msg, checksum_str);
  848. }
  849. session->msgbuf[session->msgbuflen] = '\0';
  850. count = gpsd_write(session, session->msgbuf, session->msgbuflen);
  851. if (count != (ssize_t)session->msgbuflen)
  852. return -1;
  853. else
  854. return count;
  855. }
  856. #ifdef CONTROLSEND_ENABLE
  857. /**
  858. * Write data to the device, doing any required padding or checksumming
  859. */
  860. static ssize_t greis_control_send(struct gps_device_t *session,
  861. char *msg, size_t msglen)
  862. {
  863. return greis_write(session, msg, msglen);
  864. }
  865. #endif /* CONTROLSEND_ENABLE */
  866. static void greis_event_hook(struct gps_device_t *session, event_t event)
  867. {
  868. if (session->context->readonly)
  869. return;
  870. if (event == event_wakeup) {
  871. /*
  872. * Code to make the device ready to communicate. Only needed if the
  873. * device is in some kind of sleeping state, and only shipped to
  874. * RS232C, so that gpsd won't send strings to unidentified USB devices
  875. * that might not be GPSes at all.
  876. */
  877. /*
  878. * Disable any existing messages, then request vendor for
  879. * identification.
  880. */
  881. (void)greis_write(session, disable_messages,
  882. sizeof(disable_messages) - 1);
  883. (void)greis_write(session, get_vendor, sizeof(get_vendor) - 1);
  884. } else if (event == event_identified || event == event_reactivate) {
  885. /*
  886. * Fires when the first full packet is recognized from a previously
  887. * unidentified device OR the device is reactivated after close. The
  888. * session.lexer counter is zeroed.
  889. *
  890. * TODO: If possible, get the software version and store it in
  891. * session->subtype.
  892. */
  893. (void)greis_write(session, disable_messages,
  894. sizeof(disable_messages) - 1);
  895. (void)greis_write(session, set_update_rate_4hz,
  896. sizeof(set_update_rate_4hz) - 1);
  897. (void)greis_write(session, enable_messages_4hz,
  898. sizeof(enable_messages_4hz) - 1);
  899. /* Store (expected) cycle time (seconds) */
  900. session->gpsdata.dev.cycle.tv_sec = 0;
  901. session->gpsdata.dev.cycle.tv_nsec = 250000000L;
  902. } else if (event == event_driver_switch) {
  903. /*
  904. * Fires when the driver on a device is changed *after* it
  905. * has been identified.
  906. */
  907. } else if (event == event_deactivate) {
  908. /*
  909. * Fires when the device is deactivated. Use this to revert
  910. * whatever was done at event_identified and event_configure
  911. * time.
  912. */
  913. (void)greis_write(session, disable_messages,
  914. sizeof(disable_messages) - 1);
  915. }
  916. }
  917. /**
  918. * This is the entry point to the driver. When the packet sniffer recognizes
  919. * a packet for this driver, it calls this method which passes the packet to
  920. * the binary processor or the nmea processor, depending on the session type.
  921. */
  922. static gps_mask_t greis_parse_input(struct gps_device_t *session)
  923. {
  924. if (session->lexer.type == GREIS_PACKET) {
  925. return greis_dispatch(session, session->lexer.outbuffer,
  926. session->lexer.outbuflen);
  927. #ifdef NMEA0183_ENABLE
  928. } else if (session->lexer.type == NMEA_PACKET) {
  929. return nmea_parse((char *)session->lexer.outbuffer, session);
  930. #endif /* NMEA0183_ENABLE */
  931. } else
  932. return 0;
  933. }
  934. #ifdef RECONFIGURE_ENABLE
  935. /**
  936. * Set port operating mode, speed, parity, stopbits etc. here.
  937. * Note: parity is passed as 'N'/'E'/'O', but you should program
  938. * defensively and allow 0/1/2 as well.
  939. */
  940. static bool greis_set_speed(struct gps_device_t *session,
  941. speed_t speed, char parity, int stopbits)
  942. {
  943. /* change on current port */
  944. static const char set_rate[] = "set,/par/cur/term/rate,";
  945. static const char set_parity[] = "set,/par/cur/term/parity,";
  946. static const char set_stops[] = "set,/par/cur/term/stops,";
  947. static const char parity_none[] = "N";
  948. static const char parity_even[] = "even";
  949. static const char parity_odd[] = "odd";
  950. char command[BUFSIZ] = {0};
  951. const char *selected_parity = NULL;
  952. switch (parity) {
  953. case 'N':
  954. case 0:
  955. selected_parity = parity_none;
  956. break;
  957. case 'E':
  958. case 1:
  959. selected_parity = parity_even;
  960. break;
  961. case 'O':
  962. case 2:
  963. selected_parity = parity_odd;
  964. break;
  965. default:
  966. return false;
  967. }
  968. (void)snprintf(command, sizeof(command) - 1, "%s%lu && %s%s && %s%d",
  969. set_rate, (unsigned long)speed, set_parity, selected_parity,
  970. set_stops, stopbits);
  971. return (bool)greis_write(session, command, strlen(command));
  972. }
  973. #if 0
  974. /**
  975. * TODO: Switch between NMEA and binary mode
  976. */
  977. static void greis_set_mode(struct gps_device_t *session, int mode)
  978. {
  979. if (mode == MODE_NMEA) {
  980. /* send a mode switch control string */
  981. } else {
  982. /* send a mode switch control string */
  983. }
  984. }
  985. #endif
  986. #endif /* RECONFIGURE_ENABLE */
  987. #if 0 /* TODO */
  988. static double greis_time_offset(struct gps_device_t *session)
  989. {
  990. /*
  991. * If NTP notification is enabled, the GPS will occasionally NTP
  992. * its notion of the time. This will lag behind actual time by
  993. * some amount which has to be determined by observation vs. (say
  994. * WWVB radio broadcasts) and, furthermore, may differ by baud
  995. * rate. This method is for computing the NTP fudge factor. If
  996. * it's absent, an offset of 0.0 will be assumed, effectively
  997. * falling back on what's in ntp.conf. When it returns NAN,
  998. * nothing will be sent to NTP.
  999. */
  1000. return MAGIC_CONSTANT;
  1001. }
  1002. #endif
  1003. /* This is everything we export */
  1004. /* *INDENT-OFF* */
  1005. const struct gps_type_t driver_greis = {
  1006. /* Full name of type */
  1007. .type_name = "GREIS",
  1008. /* Associated lexer packet type */
  1009. .packet_type = GREIS_PACKET,
  1010. /* Driver type flags */
  1011. .flags = DRIVER_STICKY,
  1012. /* Response string that identifies device (not active) */
  1013. .trigger = NULL,
  1014. /* Number of satellite channels supported by the device */
  1015. .channels = 128,
  1016. /* Startup-time device detector */
  1017. .probe_detect = NULL,
  1018. /* Packet getter (using default routine) */
  1019. .get_packet = generic_get,
  1020. /* Parse message packets */
  1021. .parse_packet = greis_parse_input,
  1022. /* non-perturbing initial query (e.g. for version) */
  1023. .init_query = NULL,
  1024. /* fire on various lifetime events */
  1025. .event_hook = greis_event_hook,
  1026. #ifdef RECONFIGURE_ENABLE
  1027. /* Speed (baudrate) switch */
  1028. .speed_switcher = greis_set_speed,
  1029. #if 0 /* TODO */
  1030. /* Switch to NMEA mode */
  1031. .mode_switcher = greis_set_mode,
  1032. #endif
  1033. /* Message delivery rate switcher (not active) */
  1034. .rate_switcher = NULL,
  1035. /* Minimum cycle time of the device.
  1036. * Default is 1/100, but this is tunable using /par/raw/msint . */
  1037. .min_cycle.tv_sec = 0,
  1038. .min_cycle.tv_nsec = 10000000,
  1039. #endif /* RECONFIGURE_ENABLE */
  1040. #ifdef CONTROLSEND_ENABLE
  1041. /* Control string sender - should provide checksum and headers/trailer */
  1042. .control_send = greis_control_send,
  1043. #endif /* CONTROLSEND_ENABLE */
  1044. .time_offset = NULL,
  1045. /* *INDENT-ON* */
  1046. };
  1047. #endif /* defined(GREIS_ENABLE) && defined(BINARY_ENABLE) */