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