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