Coverage Report

Created: 2026-09-01 06:30

next uncovered line (L), next uncovered region (R), next uncovered branch (B)
/src/gpsd/gpsd-3.27.6~dev/drivers/driver_ubx.c
Line
Count
Source
1
/*
2
 * UBX driver.  For u-blox binary, also includes Antaris4 binary
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 by the GPSD project
18
 * SPDX-License-Identifier: BSD-2-clause
19
 *
20
 */
21
22
#include "../include/gpsd_config.h"  // must be before all includes
23
24
#include <math.h>
25
#include <stdbool.h>
26
#include <stdio.h>
27
#include <stdlib.h>                // for abs()
28
#include <string.h>
29
#include <unistd.h>
30
31
32
#include "../include/compiler.h"   // for FALLTHROUGH
33
#include "../include/gpsd.h"
34
#include "../include/driver_ubx.h"
35
36
#include "../include/bits.h"       // For UINT2INT()
37
#include "../include/timespec.h"
38
39
// UBX-NAV-PVT, UBX-NAV-PVAT flag bits
40
#define UBX_NAV_PVT_FLAG_GPS_FIX_OK 0x01
41
2.00k
#define UBX_NAV_PVT_FLAG_DGPS       0x02
42
#define UBX_NAV_PVT_FLAG_ROLL_OK    0x08
43
#define UBX_NAV_PVT_FLAG_PITCH_OK   0x10
44
504
#define UBX_NAV_PVT_FLAG_HDG_OK     0x20
45
850
#define UBX_NAV_PVT_FLAG_RTK_FLT    0x40
46
1.25k
#define UBX_NAV_PVT_FLAG_RTK_FIX    0x80
47
48
/*
49
 * Some high-precision messages provide data where the main part is a
50
 * signed 32-bit integer (same as the standard-precision versions),
51
 * and there's an 8-bit signed field providing an addend scaled to
52
 * 1/100th of the main value.  This macro provides a fetch for such
53
 * values, scaled to match the extension (i.e., 100X the main-value scale).
54
 * Since the fields are nonconsective, the offsets are provided separately.
55
 * The result is a signed 64-bit integer.
56
 *
57
 * The second macro incorporates scaling the result by a specified double.
58
 */
59
#define getles32x100s8(buf, off, offx) \
60
964
    ((int64_t)(getles32((buf), (off)) * 100LL + getsb((buf), (offx))))
61
#define getles32x100s8d(buf, off, offx, scale) \
62
964
    (getles32x100s8((buf), (off), (offx)) * (double)(scale))
63
64
/*
65
 * A ubx packet looks like this:
66
 * leader: 0xb5 0x62
67
 * message class: 1 byte
68
 * message type: 1 byte
69
 * length of payload: 2 bytes
70
 * payload: variable length
71
 * checksum: 2 bytes
72
 *
73
 * see also the FV25 and UBX documents on reference.html
74
 */
75
32.0k
#define UBX_PREFIX_LEN          6
76
#define UBX_CLASS_OFFSET        2
77
#define UBX_TYPE_OFFSET         3
78
79
// because we hates magic numbers forever
80
85
#define USART1_ID               1
81
#define USART2_ID               2
82
0
#define USB_ID                  3
83
310
#define UBX_PROTOCOL_MASK       0x01
84
310
#define NMEA_PROTOCOL_MASK      0x02
85
155
#define RTCM_PROTOCOL_MASK      0x04
86
310
#define RTCM3_PROTOCOL_MASK     0x20    // protVer 20+
87
155
#define UBX_CFG_LEN             20
88
155
#define outProtoMask            14
89
90
// UBX Satellite/Dignal Numbering
91
static const struct vlist_t vgnss_sig_ids[] = {
92
    {0x0000, "GPS L1 C/A"},
93
    {0x0003, "GPS L2 CL"},
94
    {0x0004, "GPS L2 CM"},
95
    {0x0006, "GPS L5 I"},
96
    {0x0007, "GPS L5 Q"},
97
    {0x0100, "SBAS L1 C/A"},
98
    {0x0200, "GAL E1 C"},
99
    {0x0201, "GAL E1 B"},
100
    {0x0203, "GAL E5 aI"},
101
    {0x0204, "GAL E5 aQ"},
102
    {0x0205, "GAL E5 bI"},
103
    {0x0206, "GAL E5 bQ"},
104
    {0x0208, "GAL E6 B"},
105
    {0x0209, "GAL E6 C"},
106
    {0x020A, "GAL E6 A"},
107
    {0x0300, "BDS B1I D1"},
108
    {0x0301, "BDS B1I D2"},
109
    {0x0302, "BDS B2I D1"},
110
    {0x0303, "BDS B2I D2"},
111
    {0x0304, "BDS B3I D1"},
112
    {0x0305, "BDS B1 Cp"},
113
    {0x0306, "BDS B1 Cd"},
114
    {0x0307, "BDS B2 ap"},
115
    {0x0308, "BDS B2 ad"},
116
    {0x030A, "BDS B3I D2"},
117
    {0x0500, "QZSS L1 C/A"},
118
    {0x0501, "QZSS L1 S"},
119
    {0x0504, "QZSS L2 CM"},
120
    {0x0505, "QZSS L2 CL"},
121
    {0x0508, "QZSS L5 I"},
122
    {0x0509, "QZSS L5 Q"},
123
    {0x050C, "QZSS L1 C/B"},
124
    {0x0600, "GLO L1 OF"},
125
    {0x0602, "GLO L2 OF"},
126
    {0x0700, "NavIc L5 A"},
127
    {0, NULL},
128
};
129
130
// UBX-ACK-* ids
131
static const struct vlist_t vack_ids[] = {
132
    {UBX_ACK_ACK, "ACK-ACK"},
133
    {UBX_ACK_NAK, "ACK-NAK"},
134
    {0, NULL},
135
};
136
137
// UBX-INF-* inf_ids
138
static const struct vlist_t vinf_ids[] = {
139
    {UBX_INF_DEBUG, "INF-DEBUG"},
140
    {UBX_INF_TEST, "INF-TEST"},
141
    {UBX_INF_NOTICE, "INF-NOTICE"},
142
    {UBX_INF_WARNING, "INF-WARNING"},
143
    {UBX_INF_ERROR, " INF-ERROR"},
144
    {0, NULL},
145
};
146
147
// UBX-MON-COMMS protIds
148
static const struct vlist_t vprotIds[] = {
149
    {0, "UBX"},
150
    {1, "NMEA"},
151
    {2, "RTCM2"},
152
    {5, "RTCM3"},
153
    {255, "None"},
154
    {0, NULL},
155
};
156
157
// UBX-MON-COMMS txErrors
158
static const struct flist_t vmon_comms_txerrors[] = {
159
    {1, 1, "mem"},
160
    {2, 2, "alloc"},
161
    {0, 0, NULL},
162
};
163
164
// UBX-MON-TXBUF errors
165
static const struct flist_t vmon_txbuf_errors[] = {
166
    {0x40, 0x40, "mem"},
167
    {0x80, 0x80, "alloc"},
168
    {0, 0, NULL},
169
};
170
171
// UBX-MON-HW flags
172
static const struct flist_t vmon_hw_flags[] = {
173
    {1, 1, "RTC Calibrated"},
174
    {2, 2, "Safeboot Active"},
175
    {0x04, 0x0c, "Jam OK"},
176
    {0x08, 0x0c, "Jam Warn"},
177
    {0x0c, 0x0c, "Jam Critical"},
178
    {0x10, 0x10, "xtal Absent"},
179
    {0, 0, NULL},
180
};
181
182
// UBX-MON-HW aPower
183
static const struct vlist_t vaPower[] = {
184
    {0, "Off"},
185
    {1, "On"},
186
    {2, "Unk"},
187
    {0, NULL},
188
};
189
190
// UBX-MON-HW aStatus
191
static const struct vlist_t vaStatus[] = {
192
    {0, "Init"},
193
    {1, "Unk"},
194
    {2, "OK"},
195
    {3, "Short"},
196
    {4, "Open"},
197
    {0, NULL},
198
};
199
200
// UBX-MON-RF blockId
201
static const struct vlist_t vmon_rf_blockId[] = {
202
    {0, "L1 Unk"},
203
    {1, "L2 or L5"},
204
    {0, NULL},
205
};
206
207
// UBX-MON-RF flags
208
static const struct vlist_t vmon_rf_flags[] = {
209
    {0, "Jam Unk"},
210
    {1, "Jam OK"},
211
    {2, "Jam Warn"},
212
    {3, "Jam Crit"},
213
    {0, NULL},
214
};
215
216
// Names for portID values in:
217
//  UBX-CFG-PRT, UBX-MON-IO, UBX-MON-RXBUF, UBX-MON-TXBUF, target
218
static const struct vlist_t vtarget[] = {
219
    {0, "DDC"},             // The license free name for I2C
220
    {1, "UART1"},
221
    {2, "UART2"},
222
    {3, "USB"},
223
    {4, "SPI"},
224
    {0x100, "UART1"},       // MON-COMMS
225
    {0x200, "UART2"},       // MON-COMMS
226
    {0x300, "USB"},         // MON-COMMS
227
    {0x400, "SPI"},         // MON-COMMS
228
    {0, NULL},
229
};
230
231
// UBX-HNR-PVT, UBX-NAV-SOL gpsFix, UBX-NAV-PVT fixType
232
static const struct vlist_t vpvt_fixType[] = {
233
    {0, "None"},
234
    {1, "DR"},
235
    {2, "2D"},
236
    {3, "3D"},
237
    {4, "GNSSDR"},
238
    {5, "Time"},
239
    {0, NULL},
240
};
241
242
// UBX-HNR-PVT flags
243
static const struct flist_t fhnr_pvt_flags[] = {
244
    {1, 1, "gnssFixOK"},
245
    {2, 2, "diffSoln"},
246
    {4, 4, "WKNSET"},
247
    {8, 8, "TOWSET"},
248
    {0x20, 0x20, "headVehValid"},
249
    {0, 0, NULL},
250
};
251
252
// UBX-NAV-PVT flags
253
static const struct flist_t fnav_pvt_flags[] = {
254
    {1, 1, "gnssFixOK"},
255
    {2, 2, "diffSoln"},
256
    {8, 8, "vehRollValid"},
257
    // {0, 0x10, "psmState"},      // ??
258
    {0x10, 0x10, "vehPitchValid"},
259
    {0x10, 0x10, "vehHeadingValid"},  // aka headVelValid
260
    {0x40, 0xc0, "CarrSolnFLT"},
261
    {0x80, 0xc0, "CarrSolnFIX"},
262
    {0, 0, NULL},
263
};
264
265
// UBX-NAV-PVT flags2
266
static const struct flist_t fpvt_flags2[] = {
267
    {0x20, 0x20, "confirmedAvai"},   // protver 19+
268
    {0x40, 0x40, "confirmedDate"},
269
    {0x80, 0x80, "confirmedTime"},
270
    {0, 0, NULL},
271
};
272
273
// UBX-NAV-PVT flags3
274
static const struct flist_t fpvt_flags3[] = {
275
    {0x20, 0x20, "invalLlh"},
276
    {0, 0, NULL},
277
};
278
279
// UBX-HNR-PVT, UBX-NAV-PVT valid
280
static const struct flist_t fpvt_valid[] = {
281
    {1, 1, "validDate"},
282
    {2, 2, "validTime"},
283
    {4, 4, "fullyResolved"},
284
    {8, 8, "validMag"},
285
    {0, 0, NULL},
286
};
287
288
// UBX-NAV-PVT, dgps_age
289
static const int pvt_dgps_age[] = {
290
    -1, 1, 2, 5, 10,
291
    15, 20, 30, 45, 60,
292
    90, 120, 240};
293
294
// UBX-NAV-SAT flags
295
static const struct flist_t fsat_flags[] = {
296
    // bits 0, 1, and 2 == qualityInd
297
    {8, 8, "Used"},
298
    {0x10, 0x30, "healthy"},
299
    {0x20, 0x30, "unhealthy"},
300
    {0x40, 0x40, "diffCorr"},
301
    {0x800, 0x800, "ephAvail"},
302
    {0x1000, 0x1000, "almAvail"},
303
    {0x2000, 0x2000, "anoAvail"},
304
    {0x4000, 0x4000, "aopAvail"},
305
    {0x10000, 0x10000, "sbasCorrUsed"},
306
    {0x20000, 0x20000, "rtcmCorrUsed"},
307
    {0x40000, 0x40000, "slasCorrUsed"},
308
    {0x80000, 0x80000, "spartnCorrUsed"},
309
    {0x100000L, 0x100000L, "prCorrUsed"},
310
    {0x200000L, 0x200000L, "crCorrUsed"},
311
    {0x400000L, 0x400000L, "doCorrUsed"},
312
    {0x800000L, 0x800000L, "cbasCorrUsed"},
313
    {0, 0, NULL},
314
};
315
316
// UBX-NAV-SIG corrSource
317
static const struct vlist_t vsig_corrsource[] = {
318
    {0, "None"},
319
    {1, "SBAS"},
320
    {2, "rBDS"},
321
    {3, "RTCM2"},
322
    {4, "RTCM3 OSR"},
323
    {5, "RTCM3 SSR"},
324
    {6, "QZSS SLAS"},
325
    {7, "SPARTN"},
326
    {8, "CLAS"},
327
    {0, NULL},
328
};
329
330
// UBX-NAV-SIG ionoModel
331
static const struct vlist_t vsig_ionomodel[] = {
332
    {0, "None"},
333
    {1, "Klobuchar GPS"},
334
    {2, "SBAS"},
335
    {8, "Dual F Delay"},
336
    {0, NULL},
337
};
338
339
// UBX-NAV-SIG sigFlags
340
static const struct flist_t fsig_sigFlags[] = {
341
    {1, 3, "healthy"},
342
    {2, 3, "unhealthy"},
343
    {4, 4, "prSmoothed"},
344
    {8, 8, "prUsed"},
345
    {0x10, 0x10, "crUsed"},
346
    {0x20, 0x20, "doUsed"},
347
    {0x40, 0x40, "prCorrUsed"},
348
    {0x80, 0x80, "crCorrUsed"},
349
    {0x100, 0x100, "doCorrUsed"},
350
    {0x200, 0x200, "Authenticated"},  // u-blox M9 SPG, GALILEO
351
    {0, 0, NULL},
352
};
353
354
// UBX-NAV-SVIN active
355
// UBX-TIM-SVIN active
356
static const struct vlist_t vsvin_active[] = {
357
    {0, "Inactive"},
358
    {1, "Active"},
359
    {0, NULL},
360
};
361
362
// UBX-NAV-SVIN valid
363
// UBX-TIM-SVIN valid
364
static const struct vlist_t vsvin_valid[] = {
365
    {0, "Invalid"},
366
    {1, "Valid"},
367
    {0, NULL},
368
};
369
370
// UBX-NAV-SVINFO flags
371
static const struct flist_t fsvinfo_flags[] = {
372
    {1, 1, "svUsed"},
373
    {2, 2, "diffCorr"},
374
    {4, 4, "orbitAvail"},
375
    {8, 8, "orbitEph"},
376
    {0x10, 0x10, "unhealthy"},
377
    {0x20, 0x20, "orbitAlm"},
378
    {0x40, 0x40, "orbitAop"},
379
    {0x80, 0x80, "smoothed"},
380
    {0, 0, NULL},
381
};
382
383
// UBX-NAV-SVINFO globalFlags
384
static const struct vlist_t vglobalFlags[] = {
385
    {0, "Antaris 4"},
386
    {1, "u-blox 5"},
387
    {2, "u-blox 6"},
388
    {3, "u-blox 7"},
389
    {4, "u-blox 8"},
390
    {0, NULL},
391
};
392
393
// UBX-NAV-SAT, UBX-NAV-SVINFO qualtyiInd
394
static const struct vlist_t vquality[] = {
395
    {0, "None"},
396
    {1, "Searching"},
397
    {2, "Acquired"},
398
    {3, "Unusable"},
399
    {4, "Code locked"},
400
    {5, "Carrier locked"},
401
    {6, "Carrier locked"},
402
    {7, "Carrier locked"},
403
    {0, NULL},
404
};
405
406
// UBX-NAV-TIMEGPS valid
407
static const struct flist_t vtimegps_valid[] = {
408
    {1, 1, "towValid"},
409
    {2, 2, "weekValid"},
410
    {4, 4, "leapSValid"},
411
    {0, 0, NULL},
412
};
413
414
// UBX-NAV-TIMELS srcOfCurrLs
415
static const struct vlist_t vsrcOfCurrLs[] = {
416
    {0, "firmware"},
417
    {1, "GPS GLONASS difference"},
418
    {2, "GPS"},
419
    {3, "SBAS"},
420
    {4, "BeiDou"},
421
    {5, "Galileo"},
422
    {6, "Aided data"},
423
    {7, "Configured"},
424
    {0, NULL},
425
};
426
427
// UBX-NAV-TIMELS srcOfLsChange
428
static const struct vlist_t vsrcOfLsChange[] = {
429
    {0, "No Source"},
430
    {1, "Undefined"},
431
    {2, "GPS"},
432
    {3, "SBAS"},
433
    {4, "BeiDou"},
434
    {5, "Galileo"},
435
    {6, "GLONASS"},
436
    {0, NULL},
437
};
438
439
// UBX-NAV-TIMELS valid
440
static const struct flist_t vtimels_valid[] = {
441
    {1, 1, "validCurrLs"},
442
    {2, 2, "validTimeToLsEvent"},
443
    {0, 0, NULL},
444
};
445
446
// start ubx message configuration stuff
447
448
/* UBX-NAV-SOL deprecated in u-blox 6, gone in u-blox 9.
449
 * Use UBX-NAV-PVT after u-blox 7 (protver 15+)
450
 * u-blox 6 w/ GLONASS, protver 14 have NAV-PVT
451
 * UBX-NAV-SOL has same data from NAV-POSECEF and NAV-VELECEF.
452
 * Need NAV-SOL for fix type and fix flags.
453
 * skip NAV-POSLLH as we compute lat/lon/alt/geoid from ECEF.
454
 *
455
 * UBX-NAV-SVINFO deprecated in u-blox 8, gone in u-blox 9.
456
 * Use UBX-NAV-SAT after u-blox 7
457
 *
458
 * UBX-NAV-EOE makes a good cycle ender */
459
460
// nmea to turn off
461
static const unsigned char nmea_off[] = {
462
    0x00,          // msg id  = GGA
463
    0x01,          // msg id  = GLL
464
    0x02,          // msg id  = GSA
465
    0x03,          // msg id  = GSV
466
    0x04,          // msg id  = RMC
467
    0x05,          // msg id  = VTG
468
    0x07,          // msg id  = GST
469
    0x08,          // msg id  = ZDA
470
    0x09,          // msg id  = GBS
471
};
472
473
// UBX-NAV that ww want on, for all protver
474
static const unsigned char ubx_nav_on[] = {
475
    0x04,          // UBX-NAV-DOP
476
    0x20,          // UBX-NAV-TIMEGPS
477
    // UBX-NAV-CLOCK, nice cycle ender if no NAV-EOE (protVer 18)
478
    0x22,
479
};
480
481
// UBX-NAV for protver < 15, not present in protVer >= 27
482
static const unsigned char ubx_14_nav_on[] = {
483
    0x06,              // msg id = NAV-SOL
484
    0x30,              // msg id = NAV-SVINFO
485
};
486
487
// UBX for protver >= 15
488
static const unsigned char ubx_15_nav_on[] = {
489
    // Need NAV-POSECEF, NAV-VELECEF and NAV-PVT to replace NAV-SOL
490
    0x01,              // msg id = NAV-POSECEF
491
    0x07,              // msg id = NAV-PVT
492
    0x11,              // msg id = NAV-VELECEF
493
    0x35,              // msg id = NAV-SAT
494
    0x43,              // msg id = NAV-SIG
495
};
496
497
// end ubx message configuration stuff
498
499
static gps_mask_t ubx_msg_inf(struct gps_device_t *session, unsigned char *buf,
500
                              size_t data_len);
501
static gps_mask_t ubx_msg_log_batch(struct gps_device_t *session,
502
                                    unsigned char *buf, size_t data_len);
503
static gps_mask_t ubx_msg_log_info(struct gps_device_t *session,
504
                                   unsigned char *buf, size_t data_len);
505
static gps_mask_t ubx_msg_log_retrievepos(struct gps_device_t *session,
506
                                          unsigned char *buf, size_t data_len);
507
static gps_mask_t ubx_msg_log_retrieveposextra(struct gps_device_t *session,
508
                                               unsigned char *buf,
509
                                               size_t data_len);
510
static gps_mask_t ubx_msg_log_retrievestring(struct gps_device_t *session,
511
                                             unsigned char *buf,
512
                                             size_t data_len);
513
static gps_mask_t ubx_msg_nav_dop(struct gps_device_t *session,
514
                                  unsigned char *buf, size_t data_len);
515
static gps_mask_t ubx_msg_nav_eoe(struct gps_device_t *session,
516
                                  unsigned char *buf, size_t data_len);
517
static gps_mask_t ubx_msg_nav_posecef(struct gps_device_t *session,
518
                                      unsigned char *buf, size_t data_len);
519
static gps_mask_t ubx_msg_nav_pvt(struct gps_device_t *session,
520
                                  unsigned char *buf, size_t data_len);
521
static gps_mask_t ubx_msg_nav_sat(struct gps_device_t *session,
522
                                  unsigned char *buf, size_t data_len);
523
static gps_mask_t ubx_msg_nav_sbas(struct gps_device_t *session,
524
                                   unsigned char *buf, size_t data_len);
525
static gps_mask_t ubx_msg_nav_sol(struct gps_device_t *session,
526
                                  unsigned char *buf, size_t data_len);
527
static gps_mask_t ubx_msg_nav_svinfo(struct gps_device_t *session,
528
                                     unsigned char *buf, size_t data_len);
529
static gps_mask_t ubx_msg_nav_timegps(struct gps_device_t *session,
530
                                      unsigned char *buf, size_t data_len);
531
static gps_mask_t ubx_msg_sec_uniqid(struct gps_device_t *session,
532
                                  unsigned char *buf, size_t data_len);
533
static gps_mask_t ubx_msg_nav_velecef(struct gps_device_t *session,
534
                                      unsigned char *buf, size_t data_len);
535
static gps_mask_t ubx_msg_tim_tp(struct gps_device_t *session,
536
                                 unsigned char *buf, size_t data_len);
537
static void ubx_mode(struct gps_device_t *session, int mode);
538
539
typedef struct {
540
    const char *fw_string;
541
    const float protver;
542
} fw_protver_map_entry_t;
543
544
/* based on u-blox document no. GPS.G7-SW-12001-B1 (15 June 2018)
545
 * capture decimal parts of protVer info even when session->protver currently
546
 * is integer (which _might_ change in the future, so avoid having to revisit
547
 * the info at that time).
548
 * This list is substantially incomplete and over specific. */
549
static const fw_protver_map_entry_t fw_protver_map[] = {
550
    {"2.10", 8.10},           // antaris 4, version 8 is a guess
551
    {"2.11", 8.11},           // antaris 4, version 8 is a guess
552
    {"3.04", 9.00},           // antaris 4, version 9 is a guess
553
    {"4.00", 10.00},          // antaris 4, and u-blox 5
554
    {"4.01", 10.01},          // antaris 4, and u-blox 5
555
    {"5.00", 11.00},          // u-blox 5 and antaris 4
556
    {"6.00", 12.00},          // u-blox 5 and 6
557
    {"6.02", 12.02},          // u-blox 5 and 6
558
    {"6.02", 12.03},          // u-blox 5 and 6
559
    {"7.01", 13.01},          // u-blox 7
560
    {"7.03", 13.03},          // u-blox 6 and 7
561
    {"1.00", 14.00},          // u-blox 6 w/ GLONASS, and 7
562
    // protVer >14 should carry explicit protVer in MON-VER extension
563
    {NULL, 0.0},
564
};
565
566
// Fix GPS week since we know UTC time
567
static void fix_week(struct gps_device_t * session)
568
687
{
569
    // we hope leap seconds is correct...
570
687
    time_t secs_since_gps_epoch =
571
687
        (session->newdata.time.tv_sec + session->context->leap_seconds -
572
687
         GPS_EPOCH);
573
574
    // compute GPS week
575
687
    session->context->gps_week = secs_since_gps_epoch / 604800;
576
687
    session->context->rollovers = (int)(secs_since_gps_epoch / GPS_ROLLOVER);
577
687
}
578
579
/*
580
 * Model  Fw          Protver
581
 * M8     2,01        15.00
582
 * M9     HPG 1.13    27.12
583
 * M10    SPG 5.00    34.00
584
 * F20P   HPG 2.02    50.10
585
 */
586
587
/* send a ubx message.
588
 * calculate checksums, etc.
589
 */
590
bool ubx_write(struct gps_device_t * session,
591
               unsigned int msg_class, unsigned int msg_id,
592
               const unsigned char *msg, size_t data_len)
593
7.11k
{
594
7.11k
    unsigned char CK_A, CK_B;
595
7.11k
    ssize_t count;
596
7.11k
    size_t i;
597
7.11k
    bool ok;
598
599
    // do not write if -b (readonly) option set
600
    // "passive" handled earlier
601
7.11k
    if (session->context->readonly) {
602
0
        return true;
603
0
    }
604
605
7.11k
    session->msgbuf[0] = 0xb5;
606
7.11k
    session->msgbuf[1] = 0x62;
607
608
7.11k
    CK_A = CK_B = 0;
609
7.11k
    session->msgbuf[2] = msg_class;
610
7.11k
    session->msgbuf[3] = msg_id;
611
7.11k
    session->msgbuf[4] = data_len & 0xff;
612
7.11k
    session->msgbuf[5] = (data_len >> 8) & 0xff;
613
614
7.11k
    if ((sizeof(session->msgbuf) - 8) <= data_len) {
615
0
        GPSD_LOG(LOG_ERROR, &session->context->errout,
616
0
                 "=> GPS: UBX class: %02x, id: %02x, len: %zd TOO LONG!\n",
617
0
                 msg_class, msg_id, data_len);
618
0
    }
619
7.11k
    if (NULL != msg &&
620
4.60k
        0 < data_len) {
621
4.60k
        (void)memcpy(&session->msgbuf[6], msg, data_len);
622
4.60k
    }
623
624
    // calculate CRC
625
52.0k
    for (i = 2; i < (6 + data_len); i++) {
626
44.9k
        CK_A += session->msgbuf[i];
627
44.9k
        CK_B += CK_A;
628
44.9k
    }
629
630
7.11k
    session->msgbuf[6 + data_len] = CK_A;
631
7.11k
    session->msgbuf[7 + data_len] = CK_B;
632
7.11k
    session->msgbuflen = data_len + 8;
633
634
7.11k
    GPSD_LOG(LOG_PROG, &session->context->errout,
635
7.11k
             "=> GPS: UBX class: %02x, id: %02x, len: %zd, crc: %02x%02x\n",
636
7.11k
             msg_class, msg_id, data_len,
637
7.11k
             CK_A, CK_B);
638
7.11k
    count = gpsd_write(session, session->msgbuf, session->msgbuflen);
639
7.11k
    ok = (count == (ssize_t) session->msgbuflen);
640
7.11k
    return (ok);
641
7.11k
}
642
643
/* aPower2ant_power()
644
 * convert UBX antenna power flag to gpsd ant_power flag.
645
 * Used by UBX-MON-HW, and UBX-MON-RF.
646
 *
647
 */
648
static int aPower2ant_power(unsigned aPower)
649
244
{
650
244
    int ant_power;
651
652
244
    switch(aPower) {
653
61
    case 0:
654
        // Power off
655
61
        ant_power = ANT_PWR_OFF;
656
61
        break;
657
5
    case 1:
658
        // Power on
659
5
        ant_power = ANT_PWR_ON;
660
5
        break;
661
0
    case 2:
662
        // Power state unknown
663
0
        FALLTHROUGH
664
178
    default:
665
        // Unknown values
666
178
        ant_power = ANT_PWR_UNK;
667
178
       break;
668
244
    }
669
244
    return ant_power;
670
244
}
671
672
/* antStat2ant_stat()
673
 * convert UBX antenna status flag to gpsd ant_stat flag.
674
 * Used by UBX-MON-HW, and UBX-MON-RF.
675
 *
676
 */
677
static int antStat2ant_status(unsigned antStat)
678
244
{
679
244
    int ant_stat;
680
681
244
    switch (antStat) {
682
67
    case 2:
683
67
        ant_stat = ANT_OK;
684
67
        break;
685
34
    case 3:
686
34
        ant_stat = ANT_SHORT;
687
34
        break;
688
0
    case 4:
689
0
        ant_stat = ANT_OPEN;
690
0
        break;
691
61
    case 0:
692
        // Init
693
61
        FALLTHROUGH
694
61
    case 1:
695
        // Unknown
696
61
        FALLTHROUGH
697
143
    default:
698
        // Dunno...
699
143
        ant_stat = ANT_UNK;
700
143
        break;
701
244
    }
702
244
    return ant_stat;
703
244
}
704
705
/* Convert a ubx PRN (single svid) to an NMEA 4.0 (extended)
706
 * PRN and ubx gnssid, svid
707
 *
708
 * This does NOT match NMEA 4.10 and 4.11 where all PRN are 1-99,
709
 * except IMES, QZSS, and some SBAS.
710
 *
711
 * return 0 on fail
712
 */
713
static short ubx_to_prn(int ubx_PRN, gnssid_t *gnssId,
714
                        unsigned char *svId)
715
68.9k
{
716
68.9k
    *gnssId = 0;
717
68.9k
    *svId = 0;
718
719
    // IRNSS??
720
68.9k
    if (1 > ubx_PRN) {
721
        // skip 0 PRN
722
60.5k
        return 0;
723
60.5k
    } else if (32 >= ubx_PRN) {
724
        // GPS 1..32 -> 1..32
725
972
        *gnssId = GNSSID_GPS;
726
972
        *svId = ubx_PRN;
727
7.43k
    } else if (64 >= ubx_PRN) {
728
        /* BeiDou, 159..163,33..64 -> 1..5,6..37
729
         * Wikipedia, March 2025, says BDS PRNs go up to 62
730
         * Where/how do they map??
731
         * https://en.wikipedia.org/wiki/List_of_BeiDou_satellites */
732
1.06k
        *gnssId = GNSSID_BD;
733
1.06k
        *svId = ubx_PRN - 27;
734
6.36k
    } else if (96 >= ubx_PRN) {
735
        // GLONASS 65..96 -> 1..32
736
1.15k
        *gnssId = GNSSID_GLO;
737
1.15k
        *svId = ubx_PRN - 64;
738
5.21k
    } else if (120 > ubx_PRN) {
739
        // Huh?
740
609
        return 0;
741
4.60k
    } else if (158 >= ubx_PRN) {
742
        // SBAS 120..158 -> 120..158
743
646
        *gnssId = GNSSID_SBAS;
744
646
        *svId = ubx_PRN;
745
3.95k
    } else if (163 >= ubx_PRN) {
746
        // BeiDou, 159..163 -> 1..5
747
365
        *gnssId = GNSSID_BD;
748
365
        *svId = ubx_PRN - 158;
749
3.59k
    } else if (173 > ubx_PRN) {
750
        // Huh?
751
429
        return GNSSID_GPS;
752
3.16k
    } else if (182 >= ubx_PRN) {
753
        // IMES 173..182 -> 1..5, in u-blox 8, bot u-blox 9
754
740
        *gnssId = GNSSID_IMES;
755
740
        *svId = ubx_PRN - 172;
756
2.42k
    } else if (193 > ubx_PRN) {
757
        // Huh?
758
327
        return 0;
759
2.09k
    } else if (199 >= ubx_PRN) {
760
        // QZSS 193..197 -> 1..5
761
        // ZED-F9T also see 198 and 199
762
466
        *gnssId = GNSSID_QZSS;
763
466
        *svId = ubx_PRN - 192;
764
1.63k
    } else if (211 > ubx_PRN) {
765
        // Huh?
766
293
        return 0;
767
1.33k
    } else if (246 >= ubx_PRN) {
768
        // Galileo 211..246 -> 1..36
769
467
        *gnssId = GNSSID_GAL;
770
467
        *svId = ubx_PRN - 210;
771
871
    } else {
772
        // greater than 246, GLONASS (255), unused, or other unknown
773
871
        return 0;
774
871
    }
775
5.87k
    return ubx2_to_prn(*gnssId, *svId);
776
68.9k
}
777
778
// UBX-ACK-ACK, UBX-ACK-NAK
779
static gps_mask_t ubx_msg_ack(struct gps_device_t *session,
780
                              unsigned char *buf, size_t data_len)
781
135
{
782
135
    unsigned msgid = getbes16(buf, 2);
783
784
135
    if (2 > data_len) {
785
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
786
0
                 "UBX: %s-: runt payload len %zd",
787
0
                 val2str(msgid, vack_ids), data_len);
788
0
        return 0;
789
0
    }
790
135
    GPSD_LOG(LOG_PROG, &session->context->errout,
791
135
             "UBX: %s: class: %02x, id: %02x\n",
792
135
             val2str(msgid, vack_ids),
793
135
             buf[UBX_PREFIX_LEN],
794
135
             buf[UBX_PREFIX_LEN + 1]);
795
135
    return 0;
796
135
}
797
798
// UBX-CFG-DOSC
799
static gps_mask_t ubx_msg_cfg_dosc(struct gps_device_t *session,
800
                                   unsigned char *buf, size_t data_len)
801
132
{
802
132
    unsigned version, numOsc, reserved1;
803
804
132
    if (4 > data_len) {
805
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
806
0
                 "UBX: CFG-DOSC, runt payload len %zd", data_len);
807
0
        return 0;
808
0
    }
809
132
    version = getub(buf, 0);
810
811
132
    if (0 != version) {
812
66
        GPSD_LOG(LOG_WARN, &session->context->errout,
813
66
                 "UBX: CFG-DOSC, unknown version %u\n", version);
814
66
        return 0;
815
66
    }
816
817
66
    numOsc = getub(buf, 1);
818
66
    if (2 < numOsc) {
819
66
        GPSD_LOG(LOG_WARN, &session->context->errout,
820
66
                 "UBX: CFG-DOSC, invalid numOsc %u\n", numOsc);
821
66
        return 0;
822
66
    }
823
0
    reserved1 = getleu16(buf, 2);
824
825
0
    GPSD_LOG(LOG_PROG, &session->context->errout,
826
0
             "UBX: CFG-DOSC: version %u numOsc %u reserved1 x%x \n",
827
0
             version, numOsc, reserved1);
828
829
0
    return 0;
830
66
}
831
832
// UBX-CFG-ESRC
833
static gps_mask_t ubx_msg_cfg_esrc(struct gps_device_t *session,
834
                                   unsigned char *buf, size_t data_len)
835
0
{
836
0
    unsigned version, numSources, reserved1;
837
838
0
    if (4 > data_len) {
839
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
840
0
                 "UBX: CFG-ESRC, runt payload len %zd", data_len);
841
0
        return 0;
842
0
    }
843
0
    version = getub(buf, 0);
844
845
0
    if (0 != version) {
846
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
847
0
                 "UBX: CFG-DOSC, unknown version %u\n", version);
848
0
        return 0;
849
0
    }
850
851
0
    numSources = getub(buf, 1);
852
0
    if (2 < numSources) {
853
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
854
0
                 "UBX: CFG-ESRC, invalid numSources %u\n", numSources);
855
0
        return 0;
856
0
    }
857
0
    reserved1 = getleu16(buf, 2);
858
859
0
    GPSD_LOG(LOG_PROG, &session->context->errout,
860
0
             "UBX: CFG-ESRC: version %u numSources %u reserved1 x%x \n",
861
0
             version, numSources, reserved1);
862
863
0
    return 0;
864
0
}
865
866
// UBX-CFG-RATE
867
// Deprecated in u-blox 10
868
static gps_mask_t ubx_msg_cfg_rate(struct gps_device_t *session,
869
                                   unsigned char *buf, size_t data_len)
870
0
{
871
0
    uint16_t measRate, navRate, timeRef;
872
873
0
    if (6 > data_len) {
874
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
875
0
                 "UBX: CFG-RATE, runt payload len %zd", data_len);
876
0
        return 0;
877
0
    }
878
879
0
    measRate = getleu16(buf, 0);  // Measurement rate (ms)
880
0
    navRate = getleu16(buf, 2);   // Navigation rate (cycles)
881
0
    timeRef = getleu16(buf, 4);   // Time system, e.g. UTC, GPS, ...
882
883
0
    GPSD_LOG(LOG_PROG, &session->context->errout,
884
0
             "UBX: CFG-RATE: measRate %ums, navRate %u cycle(s), timeRef %u\n",
885
0
             (unsigned)measRate, (unsigned)navRate,
886
0
             (unsigned)timeRef);
887
888
    // Update our notion of what the device's measurement rate is
889
0
    MSTOTS(&session->gpsdata.dev.cycle, measRate);
890
891
0
    return 0;
892
0
}
893
894
/* UBX-CFG-VALGET
895
 * Present in protVer 24 and up
896
 */
897
static gps_mask_t ubx_msg_cfg_valget(struct gps_device_t *session,
898
                                   unsigned char *buf, size_t data_len)
899
0
{
900
0
    unsigned version, layer, position;
901
902
0
    if (4 > data_len) {
903
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
904
0
                 "UBX: CFG-VALGET, runt payload len %zd", data_len);
905
0
        return 0;
906
0
    }
907
908
0
    version = getub(buf, 0);        // version
909
910
0
    if (1 != version) {
911
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
912
0
                 "UBX: CFG-VALGET, unknown version %u\n", version);
913
0
        return 0;
914
0
    }
915
916
0
    layer = getub(buf, 1);           // layer
917
0
    position = getleu16(buf, 2);     // position
918
919
0
    GPSD_LOG(LOG_PROG, &session->context->errout,
920
0
             "UBX: CFG-VALGET: version %u layer %u position %u\n",
921
0
             version, layer, position);
922
923
    // FIXME: get the key/value pairs.
924
925
0
    return 0;
926
0
}
927
928
/* UBX-ESF-ALG
929
 *
930
 * UBX-ESF-ALG, and UBX-ESF-INS are synchronous to the GNSS epoch.
931
 * They need to be combined and reported together with the rest of
932
 * the epoch.
933
 */
934
static gps_mask_t ubx_msg_esf_alg(struct gps_device_t *session,
935
                                  unsigned char *buf, size_t data_len)
936
176
{
937
176
    unsigned version, flags, error, reserved1;
938
176
    unsigned long yaw;
939
176
    int pitch, roll;
940
176
    static gps_mask_t mask = 0;
941
942
176
    if (16 > data_len) {
943
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
944
0
                 "UBX: ESF-ALG: runt payload len %zd", data_len);
945
0
        return mask;
946
0
    }
947
948
    // UBX-ESF-ALG is aligned with the GNSS epoch.
949
176
    session->driver.ubx.iTOW = getleu32(buf, 0);
950
951
176
    version = getub(buf, 4);
952
176
    flags = getub(buf, 5);
953
176
    error = getub(buf, 6);
954
176
    reserved1 = getub(buf, 7);
955
176
    yaw = getleu32(buf, 8);
956
176
    pitch = getles16(buf, 12);
957
176
    roll = getles16(buf, 14);
958
959
176
    if (0 == (2 & error)) {
960
        // no yawAlgError
961
66
        session->gpsdata.attitude.yaw = 0.01 * yaw;
962
66
        mask |= ATTITUDE_SET;
963
66
    }
964
176
    if (0 == (5 & error)) {
965
        // no tiltAlgError or angleError
966
66
        session->gpsdata.attitude.roll = 0.01 * roll;
967
66
        session->gpsdata.attitude.pitch = 0.01 * pitch;
968
66
        mask |= ATTITUDE_SET;
969
66
    }
970
971
176
    if (0 != mask) {
972
176
        timespec_t ts_tow;
973
        // got good data, set the measurement time
974
176
        MSTOTS(&ts_tow, session->driver.ubx.iTOW);
975
176
        session->gpsdata.attitude.mtime =
976
176
            gpsd_gpstime_resolv(session, session->context->gps_week, ts_tow);
977
176
    }
978
176
    GPSD_LOG(LOG_PROG, &session->context->errout,
979
176
             "UBX: ESF-ALG: iTOW %lld version %u flags x%x error x%x"
980
176
             " reserved1 x%x yaw %ld pitch %u roll %u\n",
981
176
            (long long)session->driver.ubx.iTOW, version, flags, error,
982
176
            reserved1, yaw, pitch, roll);
983
984
176
    return mask;
985
176
}
986
987
/* UBX-ESF-INS
988
 *
989
 * protVer 19 and up.  ADR and UDR only
990
 *
991
 * UBX-ESF-ALG, and UBX-ESF-INS are synchronous to the GNSS epoch.
992
 * They need to be combined and reported together with the rest of
993
 * the epoch.
994
 */
995
static gps_mask_t ubx_msg_esf_ins(struct gps_device_t *session,
996
                                  unsigned char *buf, size_t data_len)
997
341
{
998
341
    unsigned long long bitfield0, reserved1;
999
341
    long xAngRate, yAngRate, zAngRate;
1000
341
    long xAccel, yAccel, zAccel;
1001
341
    static gps_mask_t mask = 0;
1002
1003
341
    if (16 > data_len) {
1004
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
1005
0
                 "UBX: ESF-INS: runt payload len %zd", data_len);
1006
0
        return mask;
1007
0
    }
1008
1009
341
    bitfield0 = getleu32(buf, 0);
1010
341
    reserved1 = getleu32(buf, 4);
1011
    // UBX-ESF-INS is aligned with the GNSS epoch.
1012
341
    session->driver.ubx.iTOW = getleu32(buf, 8);
1013
341
    xAngRate = getles32(buf, 12);
1014
341
    yAngRate = getles32(buf, 16);
1015
341
    zAngRate = getles32(buf, 20);
1016
341
    xAccel = getles32(buf, 24);
1017
341
    yAccel = getles32(buf, 28);
1018
341
    zAccel = getles32(buf, 32);
1019
1020
341
    if (0x100 == (0x100 & bitfield0)) {
1021
        // xAngRateValid
1022
66
        session->gpsdata.attitude.gyro_x = 0.001 * xAngRate;  // deg/s
1023
66
        mask |= ATTITUDE_SET;
1024
66
    }
1025
341
    if (0x200 == (0x200 & bitfield0)) {
1026
        // yAngRateValid
1027
133
        session->gpsdata.attitude.gyro_y = 0.001 * yAngRate;  // deg/s
1028
133
        mask |= ATTITUDE_SET;
1029
133
    }
1030
341
    if (0x400 == (0x400 & bitfield0)) {
1031
        // zAngRateValid
1032
67
        session->gpsdata.attitude.gyro_z = 0.001 * zAngRate;  // deg/s
1033
67
        mask |= ATTITUDE_SET;
1034
67
    }
1035
341
    if (0x800 == (0x800 & bitfield0)) {
1036
        // xAccelValid
1037
132
        session->gpsdata.attitude.acc_x = 0.01 * xAccel;  // m/s^2
1038
132
        mask |= ATTITUDE_SET;
1039
132
    }
1040
341
    if (0x1000 == (0x1000 & bitfield0)) {
1041
        // yAccelValid
1042
67
        session->gpsdata.attitude.acc_y = 0.01 * yAccel;  // m/s^2
1043
67
        mask |= ATTITUDE_SET;
1044
67
    }
1045
341
    if (0x2000 == (0x2000 & bitfield0)) {
1046
        // zAccelValid
1047
133
        session->gpsdata.attitude.acc_z = 0.01 * zAccel;  // m/s^2
1048
133
        mask |= ATTITUDE_SET;
1049
133
    }
1050
1051
341
    if (0 != mask) {
1052
341
        timespec_t ts_tow;
1053
        // got good data, set the measurement time
1054
341
        MSTOTS(&ts_tow, session->driver.ubx.iTOW);
1055
341
        session->gpsdata.attitude.mtime =
1056
341
            gpsd_gpstime_resolv(session, session->context->gps_week, ts_tow);
1057
341
    }
1058
341
    GPSD_LOG(LOG_PROG, &session->context->errout,
1059
341
             "UBX: ESF-INS: bitfield0 %llu, reserved1 %llu iTOW %lld"
1060
341
             " xAngRate %ld yAngRate %ld zAngRate %ld"
1061
341
             " xAccel %ld yAccel %ld zAccel %ld\n",
1062
341
            bitfield0, reserved1,
1063
341
            (long long)session->driver.ubx.iTOW,
1064
341
            xAngRate, yAngRate, zAngRate, xAccel, yAccel, zAccel);
1065
1066
341
    return mask;
1067
341
}
1068
1069
/* UBX-ESF-MEAS
1070
 *
1071
 * protVer 15 and up.  ADR only
1072
 * protVer 19 and up.  ADR and UDR only
1073
 *
1074
 * asynchronous to the GNSS epoch, and at a higher rate.
1075
 * Needs to be reported immediately.
1076
 *
1077
 */
1078
static gps_mask_t ubx_msg_esf_meas(struct gps_device_t *session,
1079
                                   unsigned char *buf, size_t data_len)
1080
132
{
1081
132
    unsigned flags, id, numMeas, expected_len;
1082
132
    gps_mask_t mask = 0;
1083
132
    unsigned i;
1084
    // where to store the IMU data.
1085
132
    struct attitude_t *datap = &session->gpsdata.imu[0];
1086
1087
132
    if (8 > data_len) {
1088
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
1089
0
                 "UBX: ESF-MEAS: runt payload len %zd", data_len);
1090
0
        return mask;
1091
0
    }
1092
    // do not accumulate IMU data
1093
132
    gps_clear_att(datap);
1094
132
    (void)strlcpy(datap->msg, "UBX-ESF-MEAS", sizeof(datap->msg));
1095
1096
132
    datap->timeTag = getleu32(buf, 0);
1097
132
    flags = getleu16(buf, 4);
1098
132
    numMeas = (flags >> 11) & 0x01f;
1099
132
    id = getleu16(buf, 6);
1100
132
    expected_len = 8 + (4 * numMeas);
1101
132
    if (0x08 & flags) {
1102
66
        expected_len += 4;
1103
66
    }
1104
132
    if (expected_len != data_len) {
1105
132
        GPSD_LOG(LOG_WARN, &session->context->errout,
1106
132
                 "UBX: ESF-MEAS: bad length.  Got %zd, expected %u",
1107
132
                 data_len, expected_len);
1108
132
        return 0;
1109
132
    }
1110
1111
0
    GPSD_LOG(LOG_PROG, &session->context->errout,
1112
0
             "UBX: ESF-MEAS: timeTag %lu flags x%x (numMeas %u) id %u\n",
1113
0
             datap->timeTag, flags, numMeas, id);
1114
1115
0
    for (i = 0; i < numMeas; i++) {
1116
0
        unsigned long data, dataField;
1117
0
        long dataF;
1118
0
        unsigned char dataType;
1119
1120
0
        data = getleu32(buf, 8 + (i * 4));
1121
0
        dataType = (unsigned char)(data >> 24) & 0x3f;
1122
0
        dataField = data & BITMASK(24);
1123
0
        switch (dataType) {
1124
0
        case 5:            // gyro z angular rate, deg/s^2
1125
0
            dataF = UINT2INT(dataField, 24);
1126
0
            datap->gyro_z = dataF / 4096.0;
1127
0
            mask |= IMU_SET;
1128
0
            break;
1129
0
        case 12:           // gyro temp, deg C
1130
0
            dataF = UINT2INT(dataField, 24);
1131
0
            datap->gyro_temp = dataF / 100.0;
1132
0
            mask |= IMU_SET;
1133
0
            break;
1134
0
        case 13:           // gyro y angular rate, deg/s^2
1135
0
            dataF = UINT2INT(dataField, 24);
1136
0
            datap->gyro_y = dataF / 4096.0;
1137
0
            mask |= IMU_SET;
1138
0
            break;
1139
0
        case 14:           // gyro x angular rate, deg/s^2
1140
0
            dataF = UINT2INT(dataField, 24);
1141
0
            datap->gyro_x = dataF / 4096.0;
1142
0
            mask |= IMU_SET;
1143
0
            break;
1144
0
        case 16:            // accel x, m/s^2
1145
0
            dataF = UINT2INT(dataField, 24);
1146
0
            datap->acc_x = dataF / 1024.0;
1147
0
            mask |= IMU_SET;
1148
0
            break;
1149
0
        case 17:           // accel y, m/s^2
1150
0
            dataF = UINT2INT(dataField, 24);
1151
0
            datap->acc_y = dataF / 1024.0;
1152
0
            mask |= IMU_SET;
1153
0
            break;
1154
0
        case 18:           // accel z, m/s^2
1155
0
            dataF = UINT2INT(dataField, 24);
1156
0
            datap->acc_z = dataF / 1024.0;
1157
0
            mask |= IMU_SET;
1158
0
            break;
1159
        // case 6:            // front-left wheel ticks
1160
        // case 7:            // front-right wheel ticks
1161
        // case 8:            // rear-left wheel ticks
1162
        // case 9:            // rear-right wheel ticks
1163
        // case 10:           // speed tick
1164
        // case 11:           // speed, m/s
1165
0
        default:
1166
            // ignore all else
1167
0
            dataF = dataField;
1168
0
            break;
1169
0
        }
1170
1171
0
        GPSD_LOG(LOG_PROG + 1, &session->context->errout,
1172
0
                 "UBX: ESF-MEAS: dataType %2u dataField %9ld\n",
1173
0
                 dataType, dataF);
1174
0
    }
1175
1176
0
    return mask;
1177
0
}
1178
1179
/* UBX-ESF-RAW
1180
 *
1181
 * protVer 15 and up.  ADR only
1182
 * protVer 19 and up.  ADR and UDR only
1183
 *
1184
 * asynchronous to the GNSS epoch, and a a higher rate.
1185
 * Needs to be reported immediately.
1186
 *
1187
 */
1188
static gps_mask_t ubx_msg_esf_raw(struct gps_device_t *session,
1189
                                  unsigned char *buf, size_t data_len)
1190
18
{
1191
18
    unsigned long reserved1, last_sTtag = 0;
1192
18
    unsigned i;
1193
18
    uint16_t blocks;
1194
18
    gps_mask_t mask = 0;
1195
18
    struct attitude_t *datap = NULL;
1196
18
    int max_imu, cur_imu = -1;
1197
18
    max_imu = sizeof(session->gpsdata.imu) / sizeof(struct attitude_t);
1198
1199
18
    if (4 > data_len) {
1200
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
1201
0
                 "UBX: ESF-RAW:runt payload len %zd", data_len);
1202
0
        return mask;
1203
0
    }
1204
1205
18
    reserved1 = getleu32(buf, 0);  // reserved1
1206
18
    if (0 != ((data_len - 4) % 8)) {
1207
18
        GPSD_LOG(LOG_WARN, &session->context->errout,
1208
18
                 "UBX: ESF-RAW: weird payload len %zd", data_len);
1209
18
        return mask;
1210
18
    }
1211
0
    blocks = (data_len - 4) / 8;
1212
1213
0
    GPSD_LOG(LOG_PROG, &session->context->errout,
1214
0
             "UBX: ESF-RAW: reserved1 x%lx, blocks %u\n",
1215
0
             reserved1, blocks);
1216
1217
    // loop over all blocks, use the next imu[] when time changes.
1218
0
    for (i = 0; i < blocks; i++) {
1219
0
        unsigned long data, dataField, sTtag;
1220
0
        long dataF;
1221
0
        unsigned char dataType;
1222
1223
0
        sTtag = getleu32(buf, 8 + (i * 8));
1224
0
        if ((-1 == cur_imu) ||
1225
0
            (last_sTtag != sTtag)) {
1226
0
            cur_imu++;
1227
0
            if (max_imu <= cur_imu) {
1228
0
                GPSD_LOG(LOG_WARN, &session->context->errout,
1229
0
                         "UBX: ESF-RAW: too many imu max %d block %u\n",
1230
0
                         max_imu, i);
1231
0
                break;
1232
0
            }
1233
0
            last_sTtag = sTtag;
1234
0
            datap = &session->gpsdata.imu[cur_imu];
1235
            // do not accumulate IMU data
1236
0
            gps_clear_att(datap);
1237
0
            (void)strlcpy(datap->msg, "UBX-ESF-RAW", sizeof(datap->msg));
1238
0
        }
1239
0
        if (NULL == datap) {
1240
            // paranoia
1241
0
            continue;
1242
0
        }
1243
1244
0
        data = getleu32(buf, 4 + (i * 8));
1245
0
        dataType = (unsigned char)(data >> 24) & 0x3f;
1246
0
        dataField = data & BITMASK(24);
1247
0
        datap->timeTag = sTtag;
1248
0
        switch (dataType) {
1249
0
        case 5:            // gyro z angular rate, deg/s^2
1250
0
            dataF = UINT2INT(dataField, 24);
1251
0
            datap->gyro_z = dataF / 4096.0;
1252
0
            mask |= IMU_SET;
1253
0
            break;
1254
0
        case 12:           // gyro temp, deg C
1255
0
            dataF = UINT2INT(dataField, 24);
1256
0
            datap->gyro_temp = dataF / 100.0;
1257
0
            mask |= IMU_SET;
1258
0
            break;
1259
0
        case 13:           // gyro y angular rate, deg/s^2
1260
0
            dataF = UINT2INT(dataField, 24);
1261
0
            datap->gyro_y = dataF / 4096.0;
1262
0
            mask |= IMU_SET;
1263
0
            break;
1264
0
        case 14:           // gyro x angular rate, deg/s^2
1265
0
            dataF = UINT2INT(dataField, 24);
1266
0
            datap->gyro_x = dataF / 4096.0;
1267
0
            mask |= IMU_SET;
1268
0
            break;
1269
0
        case 16:            // accel x, m/s^2
1270
0
            dataF = UINT2INT(dataField, 24);
1271
0
            datap->acc_x = dataF / 1024.0;
1272
0
            mask |= IMU_SET;
1273
0
            break;
1274
0
        case 17:           // accel y, m/s^2
1275
0
            dataF = UINT2INT(dataField, 24);
1276
0
            datap->acc_y = dataF / 1024.0;
1277
0
            mask |= IMU_SET;
1278
0
            break;
1279
0
        case 18:           // accel z, m/s^2
1280
0
            dataF = UINT2INT(dataField, 24);
1281
0
            datap->acc_z = dataF / 1024.0;
1282
0
            mask |= IMU_SET;
1283
0
            break;
1284
        // case 6:            // front-left wheel ticks
1285
        // case 7:            // front-right wheel ticks
1286
        // case 8:            // rear-left wheel ticks
1287
        // case 9:            // rear-right wheel ticks
1288
        // case 10:           // speed tick
1289
        // case 11:           // speed, m/s
1290
0
        default:
1291
            // ignore all else
1292
0
            dataF = dataField;
1293
0
            break;
1294
0
        }
1295
1296
0
        GPSD_LOG(LOG_PROG + 1, &session->context->errout,
1297
0
                 "UBX: ESF-RAW: dataType %2u dataField %9ld sTtag %lu\n",
1298
0
                 dataType, dataF, datap->timeTag);
1299
0
    }
1300
0
    return mask;
1301
0
}
1302
1303
// UBX-ESF-STATUS
1304
static gps_mask_t ubx_msg_esf_status(struct gps_device_t *session,
1305
                                     unsigned char *buf, size_t data_len)
1306
206
{
1307
206
    unsigned version, fusionMode, numSens, expected_len;
1308
206
    static gps_mask_t mask = 0;
1309
1310
206
    if (16 > data_len) {
1311
66
        GPSD_LOG(LOG_WARN, &session->context->errout,
1312
66
                 "UBX: ESF-STATUS:runt payload len %zd", data_len);
1313
66
        return mask;
1314
66
    }
1315
1316
140
    session->driver.ubx.iTOW = getleu32(buf, 0);
1317
140
    version = getub(buf, 4);
1318
140
    fusionMode = getub(buf, 12);
1319
140
    numSens = getub(buf, 15);
1320
140
    expected_len = 16 + (4 * numSens);
1321
1322
140
    if (expected_len != data_len) {
1323
71
        GPSD_LOG(LOG_WARN, &session->context->errout,
1324
71
                 "UBX: ESF-STATUS: bad length.  Expected %u got %zd",
1325
71
                 expected_len, data_len);
1326
71
        return mask;
1327
71
    }
1328
1329
69
    GPSD_LOG(LOG_PROG, &session->context->errout,
1330
69
             "UBX: ESF-STATUS: iTOW %lld version %u fusionMode %u numSens %u\n",
1331
69
            (long long)session->driver.ubx.iTOW, version, fusionMode, numSens);
1332
1333
69
    return mask;
1334
140
}
1335
1336
/**
1337
 * HNR Attitude solution
1338
 * UBX-HNR-ATT Class x28, ID 1
1339
 *
1340
 * Not before u-blox 8, protVer 19.2 and up.
1341
 * only on ADR, and UDR
1342
 */
1343
static gps_mask_t ubx_msg_hnr_att(struct gps_device_t *session,
1344
                                  unsigned char *buf, size_t data_len)
1345
68
{
1346
68
    uint8_t version;
1347
68
    int64_t iTOW;
1348
68
    timespec_t ts_tow;
1349
68
    gps_mask_t mask = 0;
1350
1351
68
    if (32 > data_len) {
1352
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
1353
0
                 "UBX: HNR-ATT: runt payload len %zd", data_len);
1354
0
        return 0;
1355
0
    }
1356
1357
    // don't set session->driver.ubx.iTOW, HNR is off-cycle
1358
68
    iTOW = getleu32(buf, 0);
1359
68
    MSTOTS(&ts_tow, iTOW);
1360
68
    session->gpsdata.attitude.mtime =
1361
68
        gpsd_gpstime_resolv(session, session->context->gps_week, ts_tow);
1362
1363
68
    version  = (unsigned int)getub(buf, 4);
1364
1365
68
    session->gpsdata.attitude.roll = 1e-5 * getles32(buf, 8);
1366
68
    session->gpsdata.attitude.pitch = 1e-5 * getles32(buf, 12);
1367
    // seems to be true heading
1368
68
    session->gpsdata.attitude.heading = 1e-5 * getles32(buf, 16);
1369
68
    mask |= ATTITUDE_SET;
1370
1371
68
    GPSD_LOG(LOG_PROG, &session->context->errout,
1372
68
         "UBX: HNR-ATT: iTOW %lld version %u roll %.5f pitch %.5f "
1373
68
         "heading %.5f\n",
1374
68
         (long long)iTOW,
1375
68
         version,
1376
68
         session->gpsdata.attitude.roll,
1377
68
         session->gpsdata.attitude.pitch,
1378
68
         session->gpsdata.attitude.heading);
1379
1380
68
    return mask;
1381
68
}
1382
1383
/**
1384
 * HNR Vehicle dynamics information
1385
 * UBX-HNR-INS Class x28, ID 2
1386
 *
1387
 * Not before u-blox 8, protVer 19.1 and up.
1388
 * only on ADR, and UDR
1389
 */
1390
static gps_mask_t ubx_msg_hnr_ins(struct gps_device_t *session,
1391
                                  unsigned char *buf, size_t data_len)
1392
0
{
1393
0
    uint8_t version;
1394
0
    uint32_t bitfield0;
1395
0
    gps_mask_t mask = 0;
1396
0
    int64_t iTOW;
1397
1398
0
    if (36 > data_len) {
1399
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
1400
0
                 "UBX: HNR-INS: runt payload len %zd", data_len);
1401
0
        return 0;
1402
0
    }
1403
1404
0
    version  = (unsigned int)getub(buf, 0);
1405
1406
0
    bitfield0 = getleu32(buf, 0);
1407
    // don't set session->driver.ubx.iTOW, HNR is off-cycle
1408
0
    iTOW = getleu32(buf, 8);
1409
1410
0
    if (0x100 == (0x100 & bitfield0)) {
1411
        // xAngRateValid
1412
0
        session->gpsdata.attitude.gyro_x = 0.001 * getles32(buf, 12);  // deg/s
1413
0
        mask |= ATTITUDE_SET;
1414
0
    }
1415
0
    if (0x200 == (0x200 & bitfield0)) {
1416
        // yAngRateValid
1417
0
        session->gpsdata.attitude.gyro_y = 0.001 * getles32(buf, 16);  // deg/s
1418
0
        mask |= ATTITUDE_SET;
1419
0
    }
1420
0
    if (0x400 == (0x400 & bitfield0)) {
1421
        // zAngRateValid
1422
0
        session->gpsdata.attitude.gyro_z = 0.001 * getles32(buf, 20);  // deg/s
1423
0
        mask |= ATTITUDE_SET;
1424
0
    }
1425
0
    if (0x800 == (0x800 & bitfield0)) {
1426
        // xAccelValid
1427
0
        session->gpsdata.attitude.acc_x = 0.01 * getles32(buf, 24);  // m/s^2
1428
0
        mask |= ATTITUDE_SET;
1429
0
    }
1430
0
    if (0x1000 == (0x1000 & bitfield0)) {
1431
        // yAccelValid
1432
0
        session->gpsdata.attitude.acc_y = 0.01 * getles32(buf, 28);  // m/s^2
1433
0
        mask |= ATTITUDE_SET;
1434
0
    }
1435
0
    if (0x2000 == (0x2000 & bitfield0)) {
1436
        // zAccelValid
1437
0
        session->gpsdata.attitude.acc_z = 0.01 * getles32(buf, 32);  // m/s^2
1438
0
        mask |= ATTITUDE_SET;
1439
0
    }
1440
1441
0
    if (0 != mask) {
1442
0
        timespec_t ts_tow;
1443
        // got good data, set the measurement time
1444
0
        MSTOTS(&ts_tow, iTOW);
1445
0
        session->gpsdata.attitude.mtime =
1446
0
            gpsd_gpstime_resolv(session, session->context->gps_week, ts_tow);
1447
0
    }
1448
1449
0
    GPSD_LOG(LOG_PROG, &session->context->errout,
1450
0
         "UBX: HNR-INS: iTOW %lld version %u bitfield0 x%x "
1451
0
         "gyro_x %.3f gyro_y %.3f gyro_z %.3f "
1452
0
         "acc_x %.3f acc_y %.3f acc_z %.3f\n",
1453
0
         (long long)iTOW,
1454
0
         version, bitfield0,
1455
0
         session->gpsdata.attitude.gyro_x,
1456
0
         session->gpsdata.attitude.gyro_y,
1457
0
         session->gpsdata.attitude.gyro_z,
1458
0
         session->gpsdata.attitude.acc_x,
1459
0
         session->gpsdata.attitude.acc_y,
1460
0
         session->gpsdata.attitude.acc_z);
1461
1462
0
    return mask;
1463
0
}
1464
1465
/**
1466
 * High rate output of PVT solution
1467
 * UBX-HNR-PVT Class x28, ID 2
1468
 *
1469
 * Present in:
1470
 *    protVer 19 and up
1471
 *    only on ADR, and UDR
1472
 */
1473
static gps_mask_t ubx_msg_hnr_pvt(struct gps_device_t *session,
1474
                                  unsigned char *buf, size_t data_len)
1475
0
{
1476
0
    char buf2[80];
1477
0
    char buf3[80];
1478
0
    char ts_buf[TIMESPEC_LEN];
1479
0
    gps_mask_t mask = 0;
1480
0
    int64_t iTOW;
1481
0
    int *mode = &session->newdata.mode;
1482
0
    int *status = &session->newdata.status;
1483
0
    struct tm unpacked_date = {0};
1484
0
    unsigned flags;
1485
0
    unsigned gpsFix;    // same as NAV-PVT typeFix
1486
0
    unsigned valid;
1487
1488
0
    if (72 > data_len) {
1489
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
1490
0
                 "UBX: HNR-PVT: runt payload len %zd", data_len);
1491
0
        return 0;
1492
0
    }
1493
1494
    // don't set session->driver.ubx.iTOW, HNR is off-cycle
1495
0
    iTOW = getleu32(buf, 0);
1496
    // valid same as UBX-NAV-PVT valid
1497
0
    valid = (unsigned int)getub(buf, 11);
1498
    // gpsFix same as UBX-NAV-PVT fixType
1499
0
    gpsFix = (unsigned char)getub(buf, 16);
1500
    // flags NOT same as UBX-NAV-PVT flags
1501
0
    flags = (unsigned int)getub(buf, 17);
1502
1503
0
    switch (gpsFix) {
1504
0
    case UBX_MODE_TMONLY:
1505
        // 5 - Surveyed-in, so a precise 3D.
1506
0
        *mode = MODE_3D;
1507
0
        *status = STATUS_TIME;
1508
0
        mask |= LATLON_SET | SPEED_SET | MODE_SET | STATUS_SET;
1509
0
        break;
1510
1511
0
    case UBX_MODE_3D:
1512
        // 3
1513
0
        *mode = MODE_3D;
1514
0
        *status = STATUS_GPS;
1515
0
        mask |= LATLON_SET | SPEED_SET | MODE_SET | STATUS_SET;
1516
0
        break;
1517
1518
0
    case UBX_MODE_GPSDR:
1519
        // 4
1520
0
        *mode = MODE_3D;
1521
0
        *status = STATUS_GNSSDR;
1522
0
        mask |= LATLON_SET | SPEED_SET | MODE_SET | STATUS_SET;
1523
0
        break;
1524
1525
0
    case UBX_MODE_2D:
1526
        // 2
1527
0
        *mode = MODE_2D;
1528
0
        *status = STATUS_GPS;
1529
0
        mask |= LATLON_SET | SPEED_SET | MODE_SET | STATUS_SET;
1530
0
        break;
1531
1532
0
    case UBX_MODE_DR:           // consider this too as 2D
1533
        // 1
1534
        // should be 3D?
1535
0
        *mode = MODE_2D;
1536
0
        *status = STATUS_DR;
1537
0
        mask |= LATLON_SET | SPEED_SET | MODE_SET | STATUS_SET;
1538
0
        break;
1539
1540
0
    case UBX_MODE_NOFIX:
1541
        // 0
1542
0
        FALLTHROUGH
1543
0
    default:
1544
        // huh?
1545
0
        *mode = MODE_NO_FIX;
1546
0
        *status = STATUS_UNK;
1547
0
        mask |= MODE_SET | STATUS_SET;
1548
0
        break;
1549
0
    }
1550
1551
0
    if (UBX_NAV_PVT_FLAG_DGPS == (flags & UBX_NAV_PVT_FLAG_DGPS)) {
1552
        // RTK flags not in u-blox 8
1553
0
        if (STATUS_TIME == *status) {
1554
            // leave it
1555
0
        } else if (UBX_NAV_PVT_FLAG_RTK_FIX ==
1556
0
                   (flags & UBX_NAV_PVT_FLAG_RTK_FIX)) {
1557
0
            *status = STATUS_RTK_FIX;
1558
0
        } else if (UBX_NAV_PVT_FLAG_RTK_FLT ==
1559
0
                   (flags & UBX_NAV_PVT_FLAG_RTK_FLT)) {
1560
0
            *status = STATUS_RTK_FLT;
1561
0
        } else {
1562
0
            *status = STATUS_DGPS;
1563
0
        }
1564
0
        mask |= STATUS_SET;
1565
0
    }
1566
1567
0
    if (UBX_NAV_PVT_VALID_DATE_TIME == (valid & UBX_NAV_PVT_VALID_DATE_TIME)) {
1568
0
        unpacked_date.tm_year = (uint16_t)getleu16(buf, 4) - 1900;
1569
0
        unpacked_date.tm_mon = (uint8_t)getub(buf, 6) - 1;
1570
0
        unpacked_date.tm_mday = (uint8_t)getub(buf, 7);
1571
0
        unpacked_date.tm_hour = (uint8_t)getub(buf, 8);
1572
0
        unpacked_date.tm_min = (uint8_t)getub(buf, 9);
1573
0
        unpacked_date.tm_sec = (uint8_t)getub(buf, 10);
1574
0
        unpacked_date.tm_isdst = 0;
1575
0
        unpacked_date.tm_wday = 0;
1576
0
        unpacked_date.tm_yday = 0;
1577
0
        session->newdata.time.tv_sec = mkgmtime(&unpacked_date);
1578
        // field 9, nano, can be negative! So normalize
1579
0
        session->newdata.time.tv_nsec = getles32(buf, 12);
1580
0
        TS_NORM(&session->newdata.time);
1581
        // compute GPS week
1582
0
        fix_week(session);
1583
1584
0
        mask |= TIME_SET | NTPTIME_IS | GOODTIME_IS;
1585
0
    }
1586
1587
0
    session->newdata.longitude = 1e-7 * getles32(buf, 20);
1588
0
    session->newdata.latitude = 1e-7 * getles32(buf, 24);
1589
    // altitude WGS84
1590
0
    session->newdata.altHAE = (double)1e-3 * getles32(buf, 28);
1591
    // altitude MSL, double to prevent promotion to (long double)
1592
0
    session->newdata.altMSL = (double)1e-3 * getles32(buf, 32);
1593
    // Let gpsd_error_model() deal with geoid_sep
1594
1595
    // gSpeed (2D)
1596
0
    session->newdata.speed = 1e-3 * (int32_t)getles32(buf, 36);
1597
    // offset 40,  Speed (3D) do what with it?
1598
    // u-blox calls this headMot (Heading of motion 2-D)
1599
0
    session->newdata.track = 1e-5 * (int32_t)getles32(buf, 44);
1600
    // offset 48, headVeh (Heading of Vehicle 2-D)
1601
0
    mask |= LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET;
1602
1603
    /* u-blox does not document the basis for the following "accuracy"
1604
     * estimates.  Maybe CEP(50), one sigma, two sigma, CEP(99), etc. */
1605
1606
    // Horizontal Accuracy estimate, in mm
1607
0
    session->newdata.eph = (double)(getles32(buf, 52) / 1000.0);
1608
    // Vertical Accuracy estimate, in mm
1609
0
    session->newdata.epv = (double)(getles32(buf, 56) / 1000.0);
1610
    // Speed Accuracy estimate, in mm/s
1611
0
    session->newdata.eps = (double)(getles32(buf, 60) / 1000.0);
1612
    // headAcc (Heading Accuracy)
1613
0
    session->newdata.epd = (double)getles32(buf, 64) * 1e-5;
1614
    // let gpsd_error_model() do the rest
1615
1616
    // 4 final bytes reserved
1617
1618
0
    mask |= HERR_SET | SPEEDERR_SET | VERR_SET;
1619
    // HNR-PVT interleaves with the normal cycle, so cycle end is a mess
1620
0
    mask |= REPORT_IS;
1621
1622
0
    GPSD_LOG(LOG_PROG, &session->context->errout,
1623
0
         "UBX: HNR-PVT: iTOW %lld flags %02x time %s lat %.2f lon %.2f "
1624
0
         "altHAE %.2f track %.2f speed %.2f climb %.2f mode %d status %d "
1625
0
         "used %d\n",
1626
0
         (long long)iTOW, flags,
1627
0
         timespec_str(&session->newdata.time, ts_buf, sizeof(ts_buf)),
1628
0
         session->newdata.latitude,
1629
0
         session->newdata.longitude,
1630
0
         session->newdata.altHAE,
1631
0
         session->newdata.track,
1632
0
         session->newdata.speed,
1633
0
         session->newdata.climb,
1634
0
         session->newdata.mode,
1635
0
         session->newdata.status,
1636
0
         session->gpsdata.satellites_used);
1637
0
    GPSD_LOG(LOG_IO, &session->context->errout,
1638
0
             "UBX: HNR-PVT: gpsFix:%s flags:%s valid:%s\n",
1639
0
             val2str(gpsFix, vpvt_fixType),
1640
0
             flags2str(flags, fhnr_pvt_flags, buf2, sizeof(buf2)),
1641
0
             flags2str(valid, fpvt_valid, buf3, sizeof(buf3)));
1642
1643
0
    return mask;
1644
0
}
1645
1646
/* UBX-INF-*
1647
 *
1648
 * Present in:
1649
 *   protVer 13 (6-series)
1650
 *   to
1651
 *   protVer 34 (10-series)
1652
 */
1653
static gps_mask_t ubx_msg_inf(struct gps_device_t *session,
1654
                              unsigned char *buf, size_t data_len)
1655
296
{
1656
296
    unsigned msgid = getbes16(buf, 2);
1657
1658
    // No minimum payload length
1659
1660
296
    if (data_len > MAX_PACKET_LENGTH - 1) {
1661
0
        data_len = MAX_PACKET_LENGTH - 1;
1662
0
    }
1663
1664
296
    GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: %s: %.*s\n",
1665
296
             val2str(msgid, vinf_ids),
1666
296
             (int)data_len, (char *)buf + UBX_PREFIX_LEN);
1667
296
    return 0;
1668
296
}
1669
1670
/**
1671
 * UBX-LOG-BATCH entry only part of UBX protocol
1672
 * Used for GPS standalone operation (internal batch retrieval)
1673
 */
1674
static gps_mask_t ubx_msg_log_batch(struct gps_device_t *session,
1675
                                    unsigned char *buf UNUSED, size_t data_len)
1676
34
{
1677
34
    struct tm unpacked_date = {0};
1678
34
    unsigned char contentValid, timeValid, flags, psmState;
1679
34
    bool gnssFixOK, diffSoln;
1680
34
    char ts_buf[TIMESPEC_LEN];
1681
34
    gps_mask_t mask = 0;
1682
1683
34
    gps_clear_log(&session->gpsdata.log);
1684
    // u-blox 8 100 bytes payload
1685
34
    if (100 > data_len) {
1686
34
        GPSD_LOG(LOG_WARN, &session->context->errout,
1687
34
                 "UBX: LOG-BATCH: runt len %zd", data_len);
1688
34
        return 0;
1689
34
    }
1690
0
    timeValid = getub(buf, 15);
1691
0
    if (3 != (timeValid & 3)) {
1692
        // No time, pointless...
1693
0
        return 0;
1694
0
    }
1695
1696
0
    unpacked_date.tm_year = getleu16(buf, 8) - 1900;
1697
0
    unpacked_date.tm_mon = getub(buf, 10) - 1;
1698
0
    unpacked_date.tm_mday = getub(buf, 11);
1699
0
    unpacked_date.tm_hour = getub(buf, 12);
1700
0
    unpacked_date.tm_min = getub(buf, 13);
1701
0
    unpacked_date.tm_sec = getub(buf, 14);
1702
1703
0
    contentValid = getub(buf, 1);
1704
0
    session->gpsdata.log.index_cnt = getleu16(buf, 2);
1705
1706
0
    session->gpsdata.log.then.tv_sec = mkgmtime(&unpacked_date);
1707
0
    session->gpsdata.log.then.tv_nsec = getles32(buf, 20);
1708
0
    TS_NORM(&session->gpsdata.log.then);
1709
1710
0
    session->gpsdata.log.fixType = getub(buf, 24);
1711
0
    flags = getub(   buf, 25);
1712
0
    gnssFixOK = flags & 1;
1713
0
    diffSoln = flags & 2;
1714
0
    psmState = ((flags >> 2) & 7);
1715
1716
    // flags2 undocumented
1717
    // flags2 = getub(   buf, 26);
1718
1719
0
    if ((gnssFixOK &&
1720
0
         2 <= session->gpsdata.log.fixType)) {
1721
        // good 2D fix
1722
0
        session->gpsdata.log.lon = 1.0e-7 * getles32(buf, 28);
1723
0
        session->gpsdata.log.lat = 1.0e-7 * getles32(buf, 32);
1724
0
        session->gpsdata.log.gSpeed = 1.0e-3 * getles32(buf, 64);
1725
        // seems to be true heading
1726
0
        session->gpsdata.log.heading = 1.0e-5 * getles32(buf, 68);
1727
0
        if (diffSoln) {
1728
0
            session->gpsdata.log.status = STATUS_DGPS;
1729
0
        } else {
1730
0
            session->gpsdata.log.status = STATUS_GPS;
1731
0
        }
1732
0
        if (3 <= session->gpsdata.log.fixType) {
1733
            // good 3D fix
1734
0
            session->gpsdata.log.altHAE = 1.0e-3 * getles32(buf, 36);
1735
0
        }
1736
0
    }
1737
0
    session->gpsdata.log.hAcc = 1.0e-3 * getleu32(buf, 44);
1738
1739
0
    GPSD_LOG(LOG_INF, &session->context->errout,
1740
0
            "UBX: LOG-BATCH: time=%s index_cnt=%u fixType=%u lon=%.7f lat=%.7f"
1741
0
             " gSpeed=%.3f heading=%.5f altHae=%.3f psmState=%u hAcc=%.3f\n",
1742
0
             timespec_str(&session->gpsdata.log.then, ts_buf, sizeof(ts_buf)),
1743
0
             session->gpsdata.log.index_cnt, session->gpsdata.log.fixType,
1744
0
             session->gpsdata.log.lon, session->gpsdata.log.lat,
1745
0
             session->gpsdata.log.gSpeed, session->gpsdata.log.heading,
1746
0
             session->gpsdata.log.altHAE, psmState,
1747
0
             session->gpsdata.log.hAcc);
1748
1749
1750
0
    if (1 == (contentValid & 1)) {
1751
        // extraPVT valid
1752
        //  iTOW = getleu32(buf, 4);
1753
0
        session->gpsdata.log.tAcc = (double)getleu32(buf, 16);
1754
0
        session->gpsdata.log.numSV = getub(buf, 27);
1755
0
        session->gpsdata.log.altMSL = 1.0e-3 * getles32(buf, 40);
1756
0
        session->gpsdata.log.vAcc = 1.0e-3 * getleu32(buf, 48);
1757
0
        session->gpsdata.log.velN = 1.0e-3 * getles32(buf, 52);
1758
0
        session->gpsdata.log.velE = 1.0e-3 * getles32(buf, 56);
1759
0
        session->gpsdata.log.velD = 1.0e-3 * getles32(buf, 60);
1760
0
        session->gpsdata.log.sAcc = 1.0e-3 * getleu32(buf, 72);
1761
0
        session->gpsdata.log.headAcc = 1.0e-5 * getleu32(buf, 76);
1762
0
        session->gpsdata.log.pDOP = 1.0e-2 * getleu32(buf, 80);
1763
0
        GPSD_LOG(LOG_INF, &session->context->errout,
1764
0
                "UBX: LOG-BATCH extraPVT: time=%s index_cnt=%d"
1765
0
                 " tAcc=%.2f numSV=%d altMSL=%.3f hAcc=%.2f vAcc=%.3f"
1766
0
                 " velN=%.3f velE=%.3f velD=%.3f sAcc=%.3f headAcc=%.5f"
1767
0
                 " pDOP=%.5f\n",
1768
0
                 timespec_str(&session->gpsdata.log.then, ts_buf,
1769
0
                              sizeof(ts_buf)),
1770
0
                 session->gpsdata.log.index_cnt,
1771
0
                 session->gpsdata.log.tAcc, session->gpsdata.log.numSV,
1772
0
                 session->gpsdata.log.altMSL, session->gpsdata.log.hAcc,
1773
0
                 session->gpsdata.log.vAcc, session->gpsdata.log.velN,
1774
0
                 session->gpsdata.log.velE, session->gpsdata.log.velD,
1775
0
                 session->gpsdata.log.sAcc, session->gpsdata.log.headAcc,
1776
0
                 session->gpsdata.log.pDOP);
1777
0
    }
1778
1779
0
    if (2 == (contentValid & 2)) {
1780
0
        session->gpsdata.log.distance = getleu32(buf, 84);
1781
0
        session->gpsdata.log.totalDistance = getleu32(buf, 88);
1782
0
        session->gpsdata.log.distanceStd = getleu32(buf, 92);
1783
0
        GPSD_LOG(LOG_INF, &session->context->errout,
1784
0
                 "UBX: LOG-BATCH extraOdo: time=%s index_cnt=%d distance=%.0f"
1785
0
                 " totalDistance=%.0f distanceStd=%.0f\n",
1786
0
                 timespec_str(&session->gpsdata.log.then, ts_buf,
1787
0
                              sizeof(ts_buf)),
1788
0
                 session->gpsdata.log.index_cnt, session->gpsdata.log.distance,
1789
0
                 session->gpsdata.log.totalDistance,
1790
0
                 session->gpsdata.log.distanceStd);
1791
0
    }
1792
1793
0
    mask |= LOG_SET;
1794
0
    return mask;
1795
0
}
1796
1797
/**
1798
 * UBX-LOG-INFO info of log status
1799
 * u-blox 7,8,9.  protVer 14 to 29
1800
 * WIP: Initial decode, log only.
1801
 *
1802
 */
1803
static gps_mask_t ubx_msg_log_info(struct gps_device_t *session,
1804
                                   unsigned char *buf UNUSED, size_t data_len)
1805
0
{
1806
0
    struct tm oldest_date = {0}, newest_date = {0};
1807
0
    timespec_t oldest = {0, 0};
1808
0
    timespec_t newest = {0, 0};
1809
0
    unsigned char version, status;
1810
0
    unsigned long filestoreCapacity;
1811
0
    unsigned long currentMaxLogSize;
1812
0
    unsigned long currentLogSize;
1813
0
    unsigned long entryCount;
1814
0
    char ts_buf[TIMESPEC_LEN];
1815
0
    char ts_buf1[TIMESPEC_LEN];
1816
0
    gps_mask_t mask = 0;
1817
1818
0
    gps_clear_log(&session->gpsdata.log);
1819
    // u-blox 7/8/9 48 bytes payload
1820
0
    if (48 > data_len) {
1821
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
1822
0
                 "UBX: LOG-INFO: runt len %zd", data_len);
1823
0
        return 0;
1824
0
    }
1825
    // u-blox 7/8/9 version 1
1826
0
    version = getub(buf, 0);
1827
0
    filestoreCapacity = getleu32(buf, 4);
1828
0
    currentMaxLogSize = getleu32(buf, 16);
1829
0
    currentLogSize = getleu32(buf, 20);
1830
0
    entryCount = getleu32(buf, 24);
1831
0
    status = getub(buf, 44);
1832
1833
0
    oldest_date.tm_year = getleu16(buf, 28);
1834
0
    if (0 != oldest_date.tm_year) {
1835
0
        oldest_date.tm_year -= 1900;
1836
0
        oldest_date.tm_mon = getub(buf, 30) - 1;
1837
0
        oldest_date.tm_mday = getub(buf, 31);
1838
0
        oldest_date.tm_hour = getub(buf, 32);
1839
0
        oldest_date.tm_min = getub(buf, 33);
1840
0
        oldest_date.tm_sec = getub(buf, 34);
1841
0
        oldest.tv_sec = mkgmtime(&oldest_date);
1842
0
        oldest.tv_nsec = 0;
1843
0
        TS_NORM(&oldest);
1844
0
    }
1845
1846
0
    newest_date.tm_year = getleu16(buf, 36);
1847
0
    if (0 != newest_date.tm_year) {
1848
0
        newest_date.tm_year -= 1900;
1849
0
        newest_date.tm_mon = getub(buf, 38) - 1;
1850
0
        newest_date.tm_mday = getub(buf, 39);
1851
0
        newest_date.tm_hour = getub(buf, 40);
1852
0
        newest_date.tm_min = getub(buf, 41);
1853
0
        newest_date.tm_sec = getub(buf, 42);
1854
0
        newest.tv_sec = mkgmtime(&newest_date);
1855
0
        newest.tv_nsec = 0;
1856
0
        TS_NORM(&newest);
1857
0
    }
1858
1859
0
    GPSD_LOG(LOG_INF, &session->context->errout,
1860
0
             "UBX: LOG-INFO: version=%u status=x%x Cap=%lu MaxSize=%lu "
1861
0
             "Size=%lu cnt=%lu oldest=%s newest=%s\n",
1862
0
             version, status,
1863
0
             filestoreCapacity,
1864
0
             currentMaxLogSize,
1865
0
             currentLogSize,
1866
0
             entryCount,
1867
0
             timespec_str(&oldest, ts_buf, sizeof(ts_buf)),
1868
0
             timespec_str(&newest, ts_buf1, sizeof(ts_buf1)));
1869
1870
    // mask |= LOG_SET;
1871
0
    return mask;
1872
0
}
1873
1874
/*
1875
 * UBX-LOG-RETRIEVEPOS (Indexed PVT entry)
1876
 * Used for GPS standalone operation and host saved logs
1877
 * u-blox 7,8,9.  protVer 14 to 29
1878
 */
1879
static gps_mask_t ubx_msg_log_retrievepos(struct gps_device_t *session,
1880
                                          unsigned char *buf UNUSED,
1881
                                          size_t data_len)
1882
0
{
1883
0
    struct tm unpacked_date = {0};
1884
0
    unsigned char fixType;
1885
0
    gps_mask_t mask = 0;
1886
1887
0
    gps_clear_log(&session->gpsdata.log);
1888
    // u-blox 40 bytes payload
1889
0
    if (40 > data_len) {
1890
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
1891
0
                 "UBX: LOG-RETRIEVEPOS: runt len %zd", data_len);
1892
0
        return 0;
1893
0
    }
1894
0
    unpacked_date.tm_year = getleu16(buf, 30);
1895
0
    if (1900 > unpacked_date.tm_year) {
1896
        // useless, no date
1897
0
        return 0;
1898
0
    }
1899
0
    unpacked_date.tm_year -= 1900;
1900
0
    unpacked_date.tm_mon = getub(buf, 32) - 1;
1901
0
    unpacked_date.tm_mday = getub(buf, 33);
1902
0
    unpacked_date.tm_hour = getub(buf, 34);
1903
0
    unpacked_date.tm_min = getub(buf, 35);
1904
0
    unpacked_date.tm_sec = getub(buf, 36);
1905
0
    session->gpsdata.log.then.tv_sec = mkgmtime(&unpacked_date);
1906
1907
0
    session->gpsdata.log.index_cnt = getleu32(buf, 0);
1908
0
    session->gpsdata.log.lon = getleu32(buf, 4) * 1.0e-7;
1909
0
    session->gpsdata.log.lat = getleu32(buf, 8) * 1.0e-7;
1910
0
    session->gpsdata.log.altMSL = getleu32(buf, 12) * 1.0e-3;
1911
    // hAcc CEP() unspecified...
1912
0
    session->gpsdata.log.hAcc = getleu32(buf, 16) * 1.0e-3;
1913
0
    session->gpsdata.log.gSpeed = getleu32(buf, 20) * 1.0e-3;
1914
    // seems to be true heading
1915
0
    session->gpsdata.log.heading = getleu32(buf, 24) * 1.0e-5;
1916
0
    fixType = getub(buf, 29);
1917
0
    session->gpsdata.log.numSV = getub(buf, 38);
1918
1919
0
    switch (fixType) {
1920
0
    case 1:
1921
        // doc is unclear: 2D or 3D?
1922
0
        session->gpsdata.log.fixType = MODE_3D;
1923
0
        session->gpsdata.log.status = STATUS_DR;
1924
0
        break;
1925
0
    case 2:
1926
0
        session->gpsdata.log.fixType = MODE_2D;
1927
0
        session->gpsdata.log.status = STATUS_GPS;
1928
0
        break;
1929
0
    case 3:
1930
0
        session->gpsdata.log.fixType = MODE_3D;
1931
0
        session->gpsdata.log.status = STATUS_GPS;
1932
0
        break;
1933
0
    case 4:
1934
        // doc is unclear: 2D or 3D?
1935
0
        session->gpsdata.log.fixType = MODE_3D;
1936
0
        session->gpsdata.log.status = STATUS_GNSSDR;
1937
0
        break;
1938
1939
0
    case 0:
1940
0
        FALLTHROUGH
1941
0
    default:
1942
        // huh?
1943
0
        session->gpsdata.log.fixType = MODE_NO_FIX;
1944
0
        session->gpsdata.log.status = STATUS_UNK;
1945
0
        break;
1946
0
    }
1947
1948
    // (long long) because of time_t
1949
0
    GPSD_LOG(LOG_INF, &session->context->errout,
1950
0
             "UBX: LOG-RETRIEVEPOS: time=%lld entryIndex=%d"
1951
0
             " lon=%.7f lat=%.7f altMSL=%.3f hAcc=%.3f"
1952
0
             " gspeed=%.3f heading=%.5f fixType=%d numSV=%d\n",
1953
0
             (long long)session->gpsdata.log.then.tv_sec,
1954
0
             session->gpsdata.log.index_cnt, session->gpsdata.log.lon,
1955
0
             session->gpsdata.log.lat, session->gpsdata.log.altMSL,
1956
0
             session->gpsdata.log.hAcc, session->gpsdata.log.gSpeed,
1957
0
             session->gpsdata.log.heading, session->gpsdata.log.fixType,
1958
0
             session->gpsdata.log.numSV);
1959
1960
1961
0
    mask |= LOG_SET;
1962
0
    return mask;
1963
0
}
1964
1965
/*
1966
 * UBX-LOG-RETRIEVEPOSEXTRA (Indexed Odometry entry)
1967
 * Used for GPS standalone operation and host saved logs
1968
 * u-blox 7,8,9.  protVer 14 to 29
1969
 */
1970
static gps_mask_t ubx_msg_log_retrieveposextra(struct gps_device_t *session,
1971
                                               unsigned char *buf UNUSED,
1972
                                               size_t data_len)
1973
0
{
1974
0
    struct tm unpacked_date = {0};
1975
0
    gps_mask_t mask = 0;
1976
1977
0
    gps_clear_log(&session->gpsdata.log);
1978
    // u-blox 32 bytes payload
1979
0
    if (32 > data_len) {
1980
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
1981
0
                 "UBX: LOG-RETRIEVEPOSEXTRA: runt len %zd", data_len);
1982
0
        return 0;
1983
0
    }
1984
1985
0
    unpacked_date.tm_year = getleu16(buf, 6);
1986
0
    if (1900 > unpacked_date.tm_year) {
1987
        // useless, no date
1988
0
        return 0;
1989
0
    }
1990
0
    unpacked_date.tm_year -= 1900;
1991
0
    unpacked_date.tm_mon = getub(buf, 8) - 1;
1992
0
    unpacked_date.tm_mday = getub(buf, 9);
1993
0
    unpacked_date.tm_hour = getub(buf, 10);
1994
0
    unpacked_date.tm_min = getub(buf, 11);
1995
0
    unpacked_date.tm_sec = getub(buf, 12);
1996
1997
0
    session->gpsdata.log.then.tv_sec = mkgmtime(&unpacked_date);
1998
0
    session->gpsdata.log.index_cnt = getleu32(buf, 0);
1999
    // distance units undocumented!  Assume meters, as in UBX-LOG-BATCH
2000
0
    session->gpsdata.log.distance = (double)getleu32(buf, 16);
2001
2002
    // (long long) because of time_t
2003
0
    GPSD_LOG(LOG_INF, &session->context->errout,
2004
0
             "UBX: LOG-RETRIEVEPOSEXTRA:"
2005
0
             " time=%lld entryindex=%u distance=%.0f\n",
2006
0
             (long long)session->gpsdata.log.then.tv_sec,
2007
0
             session->gpsdata.log.index_cnt, session->gpsdata.log.distance);
2008
2009
0
    mask |= LOG_SET;
2010
0
    return mask;
2011
0
}
2012
2013
/*
2014
 * UBX-LOG-RETRIEVESTRING
2015
 * Used for GPS standalone operation and host saved logs
2016
 * u-blox 7,8,9.  protVer 14 to 29
2017
 */
2018
static gps_mask_t ubx_msg_log_retrievestring(struct gps_device_t *session,
2019
                                             unsigned char *buf UNUSED,
2020
                                             size_t data_len)
2021
0
{
2022
0
    struct tm unpacked_date = {0};
2023
0
    unsigned int byteCount;
2024
0
    gps_mask_t mask = 0;
2025
2026
0
    gps_clear_log(&session->gpsdata.log);
2027
    // u-blox 16+ bytes payload
2028
0
    if (16 > data_len) {
2029
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
2030
0
                 "UBX: LOG-RETRIEVESTRING: runt len %zd", data_len);
2031
0
        return 0;
2032
0
    }
2033
2034
0
    unpacked_date.tm_year = getleu16(buf, 6);
2035
0
    if (1900 > unpacked_date.tm_year) {
2036
        // useless, no date
2037
0
        return 0;
2038
0
    }
2039
0
    unpacked_date.tm_year -= 1900;
2040
0
    unpacked_date.tm_mon = getub(buf, 8) - 1;
2041
0
    unpacked_date.tm_mday = getub(buf, 9);
2042
0
    unpacked_date.tm_hour = getub(buf, 10);
2043
0
    unpacked_date.tm_min = getub(buf, 11);
2044
0
    unpacked_date.tm_sec = getub(buf, 12);
2045
2046
0
    session->gpsdata.log.then.tv_sec = mkgmtime(&unpacked_date);
2047
0
    session->gpsdata.log.index_cnt = getleu32(buf, 0);
2048
0
    byteCount = getleu16(buf, 14);
2049
2050
    // string could be 0 to 256 bytes, plus NUL
2051
0
    (void)strlcpy(session->gpsdata.log.string, (const char*)&buf[16],
2052
0
                  sizeof(session->gpsdata.log.string));
2053
    // (long long) because of time_t
2054
0
    GPSD_LOG(LOG_INF, &session->context->errout,
2055
0
             "UBX: LOG-RETRIEVESTRING:"
2056
0
             " time=%lld entryindex=%u byteCount=%u string=%s\n",
2057
0
             (long long)session->gpsdata.log.then.tv_sec,
2058
0
             session->gpsdata.log.index_cnt,
2059
0
             byteCount, session->gpsdata.log.string);
2060
2061
0
    mask |= LOG_SET;
2062
0
    return mask;
2063
0
}
2064
2065
/* UBX-MON-COMMS
2066
 * Replacement for MON-RXBUF and MON-TXBUF
2067
 */
2068
static gps_mask_t ubx_msg_mon_comms(struct gps_device_t *session,
2069
                                    unsigned char *buf, size_t data_len)
2070
0
{
2071
0
    gps_mask_t mask = 0;
2072
0
    char buf2[80];
2073
0
    unsigned i;
2074
0
    unsigned version;
2075
0
    unsigned nPorts;
2076
0
    unsigned txErrors;
2077
0
    unsigned protIds[4];
2078
2079
0
    if (8 > data_len) {
2080
        // 8 + (nPorts * 40)
2081
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
2082
0
                 "UBX: MON-COMMS: runt payload len %zd\n", data_len);
2083
0
        return 0;
2084
0
    }
2085
0
    version = getub(buf, 0);
2086
0
    if (0 != version) {
2087
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
2088
0
                 "UBX: MON-COMMS unkwnown version %u\n", version);
2089
0
        return 0;
2090
0
    }
2091
0
    nPorts = getub(buf, 1);
2092
0
    if ((8 + (nPorts * 40)) > data_len) {
2093
        // 8 + (nPorts * 40)
2094
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
2095
0
                 "UBX: MON-COMMS unkwnown runt %zd\n", data_len);
2096
0
        return 0;
2097
0
    }
2098
0
    txErrors = getub(buf, 2);
2099
0
    for (i = 0; i < 4; i++) {
2100
0
        protIds[i] = getub(buf, 3 + i);
2101
0
    }
2102
2103
0
    GPSD_LOG(LOG_PROG, &session->context->errout,
2104
0
             "UBX: MON-COMMS: version %u, nPorts %u txErrors x%x  "
2105
0
             "protIds %u %u %u %u\n",
2106
0
             version, nPorts, txErrors, protIds[0], protIds[1], protIds[2],
2107
0
             protIds[3]);
2108
0
    GPSD_LOG(LOG_IO, &session->context->errout,
2109
0
             "UBX: MON-COMMS: txErrors:%s protIds %s %s %s %s\n",
2110
0
             flags2str(txErrors, vmon_comms_txerrors, buf2, sizeof(buf2)),
2111
0
             val2str(protIds[0], vprotIds),
2112
0
             val2str(protIds[1], vprotIds),
2113
0
             val2str(protIds[2], vprotIds),
2114
0
             val2str(protIds[3], vprotIds));
2115
2116
0
    for (i = 0; i < nPorts; i++) {
2117
0
        unsigned portId = getleu16(buf, 8 + (i * 40));
2118
0
        unsigned txPending = getleu16(buf, 10 + (i * 40));
2119
0
        unsigned long txBytes = getleu32(buf, 12 + (i * 40));
2120
0
        unsigned txUsage = getub(buf, 16 + (i * 40));
2121
0
        unsigned txPeakUsage = getub(buf, 17 + (i * 40));
2122
0
        unsigned rxPending = getleu16(buf, 18 + (i * 40));
2123
0
        unsigned long rxBytes = getleu32(buf, 20 + (i * 40));
2124
0
        unsigned rxUsage = getub(buf, 24 + (i * 40));
2125
0
        unsigned rxPeakUsage = getub(buf, 25 + (i * 40));
2126
0
        unsigned overrunErrs = getleu16(buf, 26 + (i * 40));
2127
0
        unsigned long msgs = getleu32(buf, 28 + (i * 40));
2128
0
        unsigned long skipped = getleu32(buf, 44 + (i * 40));
2129
2130
0
        GPSD_LOG(LOG_IO, &session->context->errout,
2131
0
                 "UBX: MON-COMMS: portId:%s\n",
2132
0
                 val2str(portId, vtarget));
2133
0
        GPSD_LOG(LOG_PROG, &session->context->errout,
2134
0
                 "UBX: MON-COMMS: portId x%x, "
2135
0
                 "txPending %u txBytes %lu txUsage %u%% txPeakUsage %u%% "
2136
0
                 "rxPending %u rxBytes %lu rxUsage %u%% rxPeakUsage %u%% "
2137
0
                 "overrunErrs %u msgs %lu skipped %lu\n",
2138
0
                 portId,
2139
0
                 txPending, txBytes, txUsage, txPeakUsage,
2140
0
                 rxPending, rxBytes, rxUsage, rxPeakUsage,
2141
0
                 overrunErrs, msgs, skipped);
2142
0
    }
2143
0
    return mask;
2144
0
}
2145
2146
/* UBX-MON-HW
2147
 * 68 bytes in protVer 12 ( 6-series)
2148
 *    Present from Antaris (4-series)
2149
 * 60 bytes in 8-series and 9-series
2150
 *    Deprecated in protVer 32. M9 and 10-series, use MON-HW and MON-RF
2151
 * 56 bytes in protVer 34 (10-series)
2152
 *    Deprecated. and undocumented,  on M10, use MON-HW and MON-RF
2153
 *
2154
 * Oddly, UBX-MON-HW is output after NAV-EOE.  So too lare for the one
2155
 * TPV for that epoch, and too early for the next epoch.
2156
 */
2157
static gps_mask_t ubx_msg_mon_hw(struct gps_device_t *session,
2158
                                 unsigned char *buf, size_t data_len)
2159
327
{
2160
327
    char buf2[80];
2161
327
    unsigned int noisePerMs;
2162
327
    unsigned int agcCnt;
2163
327
    unsigned int aStatus;
2164
327
    unsigned int aPower;
2165
327
    unsigned int flags;
2166
327
    unsigned int jamInd;
2167
327
    gps_mask_t mask = 0;
2168
2169
327
    if (60 > data_len) {
2170
        // Doc says 68, but 8-series can have 60
2171
83
        GPSD_LOG(LOG_WARN, &session->context->errout,
2172
83
                 "UBX: MON-HW: runt payload len %zd\n", data_len);
2173
83
        return 0;
2174
83
    }
2175
2176
244
    noisePerMs = getleu16(buf, 16);
2177
244
    agcCnt = getleu16(buf, 18);         // 0 to 8191
2178
244
    aStatus = getub(buf, 20);
2179
244
    aPower = getub(buf, 21);
2180
    /* flags:
2181
     * 5 only has rtcCalib
2182
     * 6 (6.03) adds safeBoot
2183
     * 6 (7.03) adds jammingState
2184
     * 9 adds xtalAbsent
2185
     */
2186
244
    flags = getub(buf, 22);
2187
    // VP, 17 bytes on protVer 15+
2188
    // VP, 25 bytes on u-blox 6
2189
    // jamInd, on 5 this is reserved
2190
244
    if (68 == data_len) {
2191
0
        jamInd = getub(buf, 53);
2192
244
    } else if (60 == data_len) {
2193
0
        jamInd = getub(buf, 45);
2194
244
    } else {
2195
        // probably 56 == data_len, undocuemted in M10
2196
244
        jamInd = 0;   // WTF?
2197
244
    }
2198
244
    session->newdata.jam = jamInd;
2199
244
    session->newdata.ant_stat = antStat2ant_status(aStatus);
2200
244
    session->newdata.ant_power = aPower2ant_power(aPower);
2201
2202
244
    if (0 < jamInd ||
2203
244
        ANT_OK <= session->newdata.ant_stat ||
2204
167
        ANT_PWR_UNK != session->newdata.ant_power) {
2205
167
        mask |= REPORT_IS;           // force a new, extra, TPV.
2206
167
    }
2207
2208
244
    GPSD_LOG(LOG_PROG, &session->context->errout,
2209
244
             "UBX: MON-HW: noisePerMs %u, agcCmt %u aStatus %u aPower %u "
2210
244
             "flags x%x jamInd %u\n",
2211
244
             noisePerMs, agcCnt, aStatus, aPower, flags, jamInd);
2212
244
    GPSD_LOG(LOG_IO, &session->context->errout,
2213
244
             "UBX: MON-HW:aStatus:%s aPower:%s flags:%s\n",
2214
244
             val2str(aStatus, vaStatus),
2215
244
             val2str(aPower, vaPower),
2216
244
             flags2str(flags, vmon_hw_flags, buf2, sizeof(buf2)));
2217
244
    return mask;
2218
327
}
2219
2220
/* UBX-MON-RF
2221
 * Present in protVer 27+ (9-series)
2222
 * Partially replaces MON-HW
2223
 *
2224
 * Oddly, UBX-MON-RF is output after NAV-EOE.  So too lare for the one
2225
 * TPV for that epoch, and too early for the next epoch.
2226
 */
2227
static gps_mask_t ubx_msg_mon_rf(struct gps_device_t *session,
2228
                                 unsigned char *buf, size_t data_len)
2229
0
{
2230
0
    unsigned i;
2231
0
    gps_mask_t mask = 0;
2232
0
    unsigned version;
2233
0
    unsigned nBlocks;
2234
0
    unsigned blockSize = 0;
2235
0
    bool compact;
2236
2237
0
    if (4 > data_len) {
2238
        // 4 + (nBlocks * 24)
2239
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
2240
0
                 "UBX: MON-RF: runt payload len %zd\n", data_len);
2241
0
        return 0;
2242
0
    }
2243
0
    version = getub(buf, 0);
2244
0
    nBlocks = getub(buf, 1);
2245
2246
0
    if (0 != version) {
2247
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
2248
0
                 "UBX: MON-RF unkwnown version %u\n", version);
2249
0
        return 0;
2250
0
    }
2251
0
    if (0 == nBlocks) {
2252
        // avoid divide by zero
2253
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
2254
0
                 "UBX: MON-RF bBlocks is zero\n");
2255
0
        return 0;
2256
0
    }
2257
0
    blockSize = (data_len - 4) / nBlocks;
2258
2259
0
    GPSD_LOG(LOG_PROG, &session->context->errout,
2260
0
             "UBX: MON-RF: version %u, nblocks %u blockSize %u\n",
2261
0
             version, nBlocks, blockSize);
2262
2263
0
    if (4 != (data_len - blockSize * nBlocks)) {
2264
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
2265
0
                 "UBX-MON-RF:  Bad length %zu s/b %u, nBlocks %u]n",
2266
0
                 data_len, 4 + blockSize * nBlocks, nBlocks);
2267
0
        return 0;
2268
0
    }
2269
2270
0
    if (20 == blockSize) {
2271
        // ZED-F9R HPS 1.30 firmware
2272
0
        compact = true;
2273
0
    } else if (24 == blockSize) {
2274
0
        compact = false;
2275
0
    } else {
2276
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
2277
0
                 "UBX: MON-RF: bad blockSize%u\n", blockSize);
2278
0
        return 0;
2279
0
    }
2280
0
    for (i = 0; i < nBlocks; i++){
2281
0
        if (false == compact) {
2282
            /* ZED-F9N 1 == nBlock
2283
             * ZED-F9P 2 == nBlock
2284
             * what to do with with two jamInd and two antStatus? */
2285
0
            unsigned off = i * 24;
2286
0
            unsigned blockId = getub(buf, 4 + off);
2287
0
            unsigned flags =  getub(buf, 5 + off);
2288
0
            unsigned jammingState = flags & 3;
2289
0
            unsigned antStatus = getub(buf, 6 + off);
2290
0
            unsigned antPower = getub(buf, 7 + off);
2291
0
            unsigned long postStatus = getleu32(buf, 8 + off);
2292
0
            unsigned long reserved1 = getleu32(buf, 12 + off);
2293
0
            unsigned agcCnt = getleu16(buf, 18 + off);         // 0 to 8191
2294
0
            unsigned jamInd = getub(buf, 20 + off);   // aka cwsuppression
2295
0
            int ofsI = getsb(buf, 21 + off);
2296
0
            unsigned magI = getub(buf, 22 + off);
2297
0
            int ofsQ = getsb(buf, 23 + off);
2298
0
            unsigned magQ = getub(buf, 24 + off);
2299
0
            unsigned reserved2 = getleu16(buf, 25 + off);
2300
0
            unsigned ant_stat;
2301
2302
0
            ant_stat = antStat2ant_status(antStatus);
2303
2304
            // use the highest ant_stat and jamInd
2305
0
            if ((unsigned)session->newdata.ant_stat < ant_stat) {
2306
0
                session->newdata.ant_stat = ant_stat;
2307
0
            }
2308
2309
0
            session->newdata.ant_power = aPower2ant_power(antPower);
2310
2311
0
            if ((unsigned)session->newdata.jam < jamInd) {
2312
0
                session->newdata.jam = jamInd;
2313
0
            }
2314
2315
0
            GPSD_LOG(LOG_PROG, &session->context->errout,
2316
0
                     "UBX: MON-RF: blk %u flags x%x jammingState %u "
2317
0
                     "antStatus %u antPower %u postStatus %lu reserved1 x%lx "
2318
0
                     "ageCnt %u "
2319
0
                     "jamInd %u ofsI %d magI %u ofsI %d magQ %u "
2320
0
                     "reserved2 x%x\n",
2321
0
                     blockId, flags, jammingState, antStatus, antPower,
2322
0
                     postStatus, reserved1, agcCnt, jamInd,
2323
0
                     ofsI, magI, ofsQ, magQ, reserved2);
2324
0
            GPSD_LOG(LOG_IO, &session->context->errout,
2325
0
                     "UBX: MON-RF:    blockId (%s) flags (%s) antStatus (%s) "
2326
0
                     "antPower (%s) agc %.1f%%\n",
2327
0
                     val2str(blockId, vmon_rf_blockId),
2328
0
                     val2str(flags, vmon_rf_flags),
2329
0
                     val2str(antStatus, vaStatus),
2330
0
                     val2str(antPower, vaPower), agcCnt / 81.91);
2331
0
        } else {
2332
            // compact, 20 bytes, HPS 1.30
2333
            /* ZED-F9R 2 == nBlock
2334
             * what to do with with two jamInd and two antStatus? */
2335
0
            unsigned off = i * 20;
2336
0
            unsigned blockId = getub(buf, 4 + off);
2337
0
            unsigned antStatus = getub(buf, 5 + off);
2338
0
            unsigned antPower = getub(buf, 6 + off);
2339
0
            unsigned cwSuppression = getub(buf, 7 + off);
2340
0
            unsigned long postStatus = getleu32(buf, 8 + off);
2341
            // reserved1 4 bytes
2342
0
            unsigned noisePerMS = getleu16(buf, 16 + off);
2343
0
            unsigned agcCnt = getleu16(buf, 18 + off);         // 0 to 8191
2344
0
            int ofsI = getsb(buf, 20 + off);
2345
0
            unsigned magI = getub(buf, 21 + off);
2346
0
            int ofsQ = getsb(buf, 22 + off);
2347
0
            unsigned magQ = getub(buf, 23 + off);
2348
0
            unsigned ant_stat;
2349
2350
0
            ant_stat = antStat2ant_status(antStatus);
2351
2352
            // use the highest ant_stat and jamInd
2353
0
            if ((unsigned)session->newdata.ant_stat < ant_stat) {
2354
0
                session->newdata.ant_stat = ant_stat;
2355
0
            }
2356
0
            session->newdata.ant_power = aPower2ant_power(antPower);
2357
2358
0
            GPSD_LOG(LOG_PROG, &session->context->errout,
2359
0
                     "UBX: MON-RF: blk %u antStatus %u antPower %u "
2360
0
                     "cwSuppression %u postStatus %lu ageCnt %u jamInd %u "
2361
0
                     "ofsI %d magI %u ofsI %d magQ %u\n",
2362
0
                     blockId, antStatus, antPower, cwSuppression,
2363
0
                     postStatus, noisePerMS,  agcCnt,
2364
0
                     ofsI, magI, ofsQ, magQ);
2365
0
            GPSD_LOG(LOG_IO, &session->context->errout,
2366
0
                     "UBX: MON-RF:    blockId (%s) antStatus (%s) "
2367
0
                     "antPower (%s) agc %.1f%%\n",
2368
0
                     val2str(blockId, vmon_rf_blockId),
2369
0
                     val2str(antStatus, vaStatus),
2370
0
                     val2str(antPower, vaPower),
2371
0
                     agcCnt / 81.91);
2372
0
        }
2373
0
    }
2374
0
    if (0 < session->newdata.jam ||
2375
0
        ANT_OK <= session->newdata.ant_stat ||
2376
0
        ANT_PWR_UNK != session->newdata.ant_power) {
2377
0
        mask |= REPORT_IS;           // force a new, extra, TPV.
2378
0
    }
2379
0
    return mask;
2380
0
}
2381
2382
/* UBX-MON-RXBUF
2383
 * Present in u-blox 5+ through at least protVer 23.01
2384
 * Supported but deprecated in M9P protVer 27.11, use MON-COMMS
2385
 * Supported but deprecated in M9N protVer 32.00 */
2386
static gps_mask_t ubx_msg_mon_rxbuf(struct gps_device_t *session,
2387
                                    unsigned char *buf, size_t data_len)
2388
0
{
2389
0
    int i;
2390
2391
0
    if (24 != data_len) {
2392
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
2393
0
                 "UBX: MON-RXBUF: runt payload len %zd\n", data_len);
2394
0
        return 0;
2395
0
    }
2396
2397
0
    for (i = 0; i < 6; i++) {
2398
0
        unsigned int pending = getleu16(buf, i * 2);
2399
0
        unsigned int usage =  getub(buf, 12 + i);
2400
0
        unsigned int peakUsage = getub(buf, 18 + i);
2401
2402
0
        GPSD_LOG(LOG_IO, &session->context->errout,
2403
0
                 "UBX: MON-RXBUF: tgt:%s\n",
2404
0
                 val2str(i, vtarget));
2405
0
        GPSD_LOG(LOG_INF, &session->context->errout,
2406
0
                 "UBX: MON-RXBUF: tgt%d pending %4u usage %3u%% peakUsage %3d%%\n",
2407
0
                 i, pending, usage, peakUsage);
2408
0
    }
2409
0
    return 0;
2410
0
}
2411
2412
/* UBX-MON-TXBUF
2413
 * Present in u-blox 5+ through at least protVer 23.01
2414
 * Supported but deprecated in M9P protVer 27.11
2415
 * Supported but deprecated in M9N protVer 32.00 */
2416
static gps_mask_t ubx_msg_mon_txbuf(struct gps_device_t *session,
2417
                                    unsigned char *buf, size_t data_len)
2418
0
{
2419
0
    char buf2[80];
2420
0
    unsigned tUsage, tPeakusage;
2421
0
    unsigned errors, limit, reserved1;
2422
0
    int i;
2423
2424
0
    if (28 != data_len) {
2425
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
2426
0
                 "UBX: MON-TXBUF: runt payload len %zd\n", data_len);
2427
0
        return 0;
2428
0
    }
2429
2430
0
    errors = limit = getub(buf, 26);
2431
2432
0
    for (i = 0; i < 6; i++) {
2433
0
        unsigned int pending = getleu16(buf, i * 2);
2434
0
        unsigned int usage =  getub(buf, 12 + i);
2435
0
        unsigned int peakUsage = getub(buf, 18 + i);
2436
2437
0
        GPSD_LOG(LOG_IO, &session->context->errout,
2438
0
                 "UBX: MON-TXBUF: tgt:%s\n",
2439
0
                 val2str(i, vtarget));
2440
0
        GPSD_LOG(LOG_INF, &session->context->errout,
2441
0
                 "UBX: MON-TXBUF: tgt %d limit %u pending %4u "
2442
0
                 "usage %3u%% peakUsage %3d%%\n",
2443
0
                 i, limit & 1, pending, usage, peakUsage);
2444
0
        limit = limit >> 1;
2445
0
    }
2446
0
    tUsage = getub(buf, 24);
2447
0
    tPeakusage = getub(buf, 25);
2448
0
    reserved1 = getub(buf, 27);
2449
2450
0
    GPSD_LOG(LOG_INF, &session->context->errout,
2451
0
             "UBX: MON-TXBUF: tUsage %3u%%, tPeakusage %3u%%, errors 0x%02x, "
2452
0
             "reserved1 0x%02x\n",
2453
0
             tUsage, tPeakusage, errors, reserved1);
2454
2455
0
    GPSD_LOG(LOG_IO, &session->context->errout,
2456
0
             "UBX: MON-TXBUF: errors:%s\n",
2457
0
             flags2str(errors, vmon_txbuf_errors, buf2, sizeof(buf2)));
2458
0
    return 0;
2459
0
}
2460
2461
/**
2462
 * Receiver/Software Version
2463
 * UBX-MON-VER
2464
 *
2465
 * sadly more info than fits in session->swtype for now.
2466
 * so squish the data hard.
2467
 */
2468
static gps_mask_t ubx_msg_mon_ver(struct gps_device_t *session,
2469
                                  unsigned char *buf,
2470
                                  size_t data_len)
2471
144
{
2472
144
    int n = 0;                           // extended info counter
2473
144
    int num_ext = (data_len - 40) / 30;  // number of extensions
2474
144
    char obuf[128];                      // temp version string buffer
2475
144
    char *cptr;
2476
2477
144
    if (40 > data_len) {
2478
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
2479
0
                 "UBX: MON-VER: runt payload len %zd", data_len);
2480
0
        return 0;
2481
0
    }
2482
2483
    // save SW and HW Version as subtype
2484
144
    (void)snprintf(obuf, sizeof(obuf),
2485
144
                   "SW %.30s,HW %.10s",
2486
144
                   (char *)buf,
2487
144
                   (char *)(buf + 30));
2488
2489
    // save what we can
2490
144
    (void)strlcpy(session->subtype, obuf, sizeof(session->subtype));
2491
2492
144
    obuf[0] = '\0';
2493
    // extract Extended info strings.
2494
1.19k
    for (n = 0; n < num_ext; n++) {
2495
1.05k
        int start_of_str = 40 + (30 * n);
2496
2497
1.05k
        if (0 < n) {
2498
            // commas between elements
2499
980
            (void)strlcat(obuf, ",", sizeof(obuf));
2500
980
        }
2501
1.05k
        (void)strlcat(obuf, (char *)&buf[start_of_str], sizeof(obuf));
2502
1.05k
    }
2503
2504
    // save what we can in subtype1
2505
144
    (void)strlcpy(session->subtype1, obuf, sizeof(session->subtype1));
2506
2507
    // find PROTVER literal, followed by single separator character
2508
144
    cptr = strstr(obuf, "PROTVER=");     // protVer 18 and above
2509
144
    if (NULL == cptr) {
2510
144
        cptr = strstr(obuf, "PROTVER "); // protVer 17 and below
2511
144
    }
2512
144
    if (NULL != cptr) {
2513
0
        int protver = atoi(cptr + 8);
2514
0
        if (7 < protver) {
2515
            /* protver 8, u-blox Antaris, is the oldest we know, but never
2516
             * used explicitly.  protver 15, u-blox 8, is oldest seen. */
2517
0
            session->driver.ubx.protver = protver;
2518
0
        }
2519
0
    }
2520
2521
    /* MON-VER did not contain PROTVER in any extension field (typical for
2522
     * protVer < 15), so use mapping table to try to derive protVer from
2523
     * firmware revision number carried in swVersion field */
2524
144
    if (0 == session->driver.ubx.protver) {
2525
260
        for (n = 0; NULL != fw_protver_map[n].fw_string; n++) {
2526
            // skip "SW " prefix in session->subtype
2527
240
            cptr = strstr(session->subtype + 3, fw_protver_map[n].fw_string);
2528
            // use only when swVersion field starts with fw_string
2529
240
            if (cptr == (session->subtype + 3)) {
2530
0
                session->driver.ubx.protver =
2531
0
                    (unsigned char)fw_protver_map[n].protver;
2532
0
                break;
2533
0
            }
2534
240
        }
2535
20
        if (0 == session->driver.ubx.protver) {
2536
            // Still not found, old chip.  Set to one so we know we tried.
2537
20
            session->driver.ubx.protver = 1;
2538
20
        }
2539
20
    }
2540
2541
    // output SW and HW Version at LOG_INF
2542
144
    GPSD_LOG(LOG_INF, &session->context->errout,
2543
144
             "UBX: MON-VER: %s %s PROTVER %u\n",
2544
144
             session->subtype, session->subtype1,
2545
144
             session->driver.ubx.protver);
2546
2547
2548
144
    return 0;
2549
144
}
2550
2551
/**
2552
 * Clock Solution UBX-NAV-CLOCK
2553
 *
2554
 * Present in:
2555
 *     protVer 8 to 34 (Antaris 4 to M10)
2556
 */
2557
static gps_mask_t ubx_msg_nav_clock(struct gps_device_t *session,
2558
                                    unsigned char *buf, size_t data_len)
2559
132
{
2560
132
    unsigned long tAcc, fAcc;
2561
2562
132
    if (20 > data_len) {
2563
66
        GPSD_LOG(LOG_WARN, &session->context->errout,
2564
66
                 "UBX: NAV-CLOCK: runt payload len %zd", data_len);
2565
66
        return 0;
2566
66
    }
2567
2568
66
    session->driver.ubx.iTOW = getleu32(buf, 0);
2569
    // u-bloc 6 sets clockbias and clockdrift to 0
2570
66
    session->gpsdata.fix.clockbias = getles32(buf, 4);
2571
66
    session->gpsdata.fix.clockdrift = getles32(buf, 8);
2572
66
    tAcc = getleu32(buf, 12);
2573
66
    fAcc = getleu32(buf, 16);
2574
66
    GPSD_LOG(LOG_PROG, &session->context->errout,
2575
66
             "UBX: NAV-CLOCK: iTOW=%lld clkB %ld clkD %ld tAcc %lu fAcc %lu\n",
2576
66
             (long long)session->driver.ubx.iTOW,
2577
66
             session->gpsdata.fix.clockbias,
2578
66
             session->gpsdata.fix.clockdrift,
2579
66
             tAcc, fAcc);
2580
66
    return 0;
2581
132
}
2582
2583
 /**
2584
 * UBX-NAV-DAHEADING, Class 1, ID x45
2585
 * protver 57+
2586
 */
2587
static gps_mask_t ubx_msg_nav_daheading(struct gps_device_t *session,
2588
                                        unsigned char *buf, size_t data_len)
2589
135
{
2590
135
    int version;
2591
135
    unsigned flags;
2592
135
    double accN = NAN, accE = NAN, accD = NAN, accL = NAN, accH = NAN;
2593
135
    gps_mask_t mask = 0;
2594
2595
135
    if (60 > data_len) {
2596
69
        GPSD_LOG(LOG_WARN, &session->context->errout,
2597
69
                 "UBX: NAV-DAHEADING: runt payload len %zd",
2598
69
                 data_len);
2599
69
        return mask;
2600
69
    }
2601
66
    version = getub(buf, 0);
2602
66
    if (2 != version) {
2603
66
        GPSD_LOG(LOG_WARN, &session->context->errout,
2604
66
                 "UBX: NAV-DAHEADING: unknown version %u s/b 2", version);
2605
66
        return mask;
2606
66
    }
2607
0
    session->driver.ubx.iTOW = getleu32(buf, 4);
2608
0
    flags = getleu32(buf, 56);
2609
0
    if (1 != (1 & flags)) {
2610
        // not gnssFixOK
2611
0
        GPSD_LOG(LOG_PROG, &session->context->errout,
2612
0
                 "UBX: NAV-DAHEADING: no fix");
2613
0
        return mask;
2614
0
    }
2615
2616
0
    if (4 & flags) {
2617
        // rePosValid
2618
0
        session->newdata.NED.relPosN = getles32(buf, 8) * 1e-4;
2619
0
        session->newdata.NED.relPosE = getles32(buf, 12) * 1e-4;
2620
0
        session->newdata.NED.relPosD = getles32(buf, 16) * 1e-4;
2621
0
        session->newdata.NED.relPosL = getles32(buf, 20) * 1e-4;
2622
0
        if (0x100 & flags) {
2623
            // relPosHeadingValid
2624
0
            session->newdata.NED.relPosH = 1e-5 * getles32(buf, 24);
2625
0
        }
2626
2627
0
        accN = 1e-4 * getles32(buf, 36);
2628
0
        accE = 1e-4 * getles32(buf, 40);
2629
0
        accD = 1e-4 * getles32(buf, 44);
2630
0
        accL = 1e-4 * getles32(buf, 48);
2631
0
        accH = 1e-4 * getles32(buf, 52);
2632
0
        mask |= NED_SET;
2633
0
    }
2634
2635
0
    GPSD_LOG(LOG_PROG, &session->context->errout,
2636
0
        "UBX: NAV-DAHEADING: version %d iTOW %lld flags x%x"
2637
0
        " relPos N %.4f E %.4f D %.4f L %.4f D %.4f"
2638
0
        " acc N %.4f E %.4f D %.4f L %.4f H %.4f\n",
2639
0
        version,
2640
0
        (long long)session->driver.ubx.iTOW,
2641
0
        flags,
2642
0
        session->newdata.NED.relPosN,
2643
0
        session->newdata.NED.relPosE,
2644
0
        session->newdata.NED.relPosD,
2645
0
        session->newdata.NED.relPosL,
2646
0
        session->newdata.NED.relPosH,
2647
0
        accN, accE, accD, accL, accH);
2648
2649
0
    return mask;
2650
0
}
2651
/**
2652
 * DGPS Data Used for NAV
2653
 *
2654
 * May be good cycle ender
2655
 *
2656
 * Present in u-blox 7
2657
 */
2658
static gps_mask_t ubx_msg_nav_dgps(struct gps_device_t *session,
2659
                                   unsigned char *buf, size_t data_len)
2660
145
{
2661
145
    long age;
2662
2663
145
    if (16 > data_len) {
2664
79
        GPSD_LOG(LOG_WARN, &session->context->errout,
2665
79
                 "UBX: NAV-DGPS: runt payload len %zd", data_len);
2666
79
        return 0;
2667
79
    }
2668
2669
66
    session->driver.ubx.iTOW = getleu32(buf, 0);
2670
66
    age = getleu32(buf, 4);
2671
66
    GPSD_LOG(LOG_PROG, &session->context->errout,
2672
66
             "NAV-DGPS: iTOW=%lld age %ld\n",
2673
66
             (long long)session->driver.ubx.iTOW, age);
2674
66
    return 0;
2675
145
}
2676
2677
/**
2678
 * UBX-NAV-DOP, Dilution of precision message
2679
 *
2680
 * Present in all u-blox (4 to 10)
2681
 */
2682
static gps_mask_t ubx_msg_nav_dop(struct gps_device_t *session,
2683
                                  unsigned char *buf, size_t data_len)
2684
269
{
2685
269
    unsigned u;
2686
269
    gps_mask_t mask = 0;
2687
2688
269
    if (18 > data_len) {
2689
67
        GPSD_LOG(LOG_WARN, &session->context->errout,
2690
67
                 "UBX: NAV-DOP: runt payload len %zd", data_len);
2691
67
        return 0;
2692
67
    }
2693
2694
202
    session->driver.ubx.iTOW = getleu32(buf, 0);
2695
    /*
2696
     * We make a deliberate choice not to clear DOPs from the
2697
     * last skyview here, but rather to treat this as a supplement
2698
     * to our calculations from the visibility matrix, trusting
2699
     * the firmware algorithms over ours.
2700
     */
2701
202
    u = getleu16(buf, 4);
2702
202
    if (9999 > u) {
2703
67
        session->gpsdata.dop.gdop = (double)(u / 100.0);
2704
67
        mask |= DOP_SET;
2705
67
    }
2706
202
    u = getleu16(buf, 6);
2707
202
    if (9999 > u) {
2708
134
        session->gpsdata.dop.pdop = (double)(u / 100.0);
2709
134
        mask |= DOP_SET;
2710
134
    }
2711
202
    u = getleu16(buf, 8);
2712
202
    if (9999 > u) {
2713
68
        session->gpsdata.dop.tdop = (double)(u / 100.0);
2714
68
        mask |= DOP_SET;
2715
68
    }
2716
202
    u = getleu16(buf, 10);
2717
202
    if (9999 > u) {
2718
132
        session->gpsdata.dop.vdop = (double)(u / 100.0);
2719
132
        mask |= DOP_SET;
2720
132
    }
2721
202
    u = getleu16(buf, 12);
2722
202
    if (9999 > u) {
2723
133
        session->gpsdata.dop.hdop = (double)(u / 100.0);
2724
133
        mask |= DOP_SET;
2725
133
    }
2726
    // Northing DOP
2727
202
    u = getleu16(buf, 14);
2728
202
    if (9999 > u) {
2729
68
        session->gpsdata.dop.ydop = (double)(u / 100.0);
2730
68
        mask |= DOP_SET;
2731
68
    }
2732
    // Easting DOP
2733
202
    u = getleu16(buf, 16);
2734
202
    if (9999 > u) {
2735
68
        session->gpsdata.dop.xdop = (double)(u / 100.0);
2736
68
        mask |= DOP_SET;
2737
68
    }
2738
202
    GPSD_LOG(LOG_PROG, &session->context->errout,
2739
202
             "UBX: NAV-DOP: gdop=%.2f pdop=%.2f "
2740
202
             "hdop=%.2f vdop=%.2f tdop=%.2f ydop=%.2f xdop=%.2f\n",
2741
202
             session->gpsdata.dop.gdop,
2742
202
             session->gpsdata.dop.pdop,
2743
202
             session->gpsdata.dop.hdop,
2744
202
             session->gpsdata.dop.vdop,
2745
202
             session->gpsdata.dop.tdop,
2746
202
             session->gpsdata.dop.ydop,
2747
202
             session->gpsdata.dop.xdop);
2748
202
    return mask;
2749
269
}
2750
2751
/**
2752
 * Position error ellipse parameters
2753
 * protVer 19.1 and up
2754
 * Not in u-blox 5, 6 or 7
2755
 * Present in some u-blox 8, 9 and 10 (ADR, HPS)
2756
 */
2757
static gps_mask_t ubx_msg_nav_eell(struct gps_device_t *session,
2758
                                   unsigned char *buf, size_t data_len)
2759
268
{
2760
268
    unsigned version;
2761
268
    unsigned errEllipseOrient;
2762
268
    unsigned long errEllipseMajor, errEllipseMinor;
2763
2764
268
    if (16 > data_len) {
2765
66
        GPSD_LOG(LOG_WARN, &session->context->errout,
2766
66
                 "UBX: NAV-EELL: runt payload len %zd", data_len);
2767
66
        return 0;
2768
66
    }
2769
2770
202
    session->driver.ubx.iTOW = getleu32(buf, 0);
2771
202
    version = getub(buf, 4);
2772
202
    if (0 != version) {
2773
66
        GPSD_LOG(LOG_WARN, &session->context->errout,
2774
66
                 "UBX: NAV-EELL unknown version %u s/b 0", version);
2775
66
        return 0;
2776
66
    }
2777
136
    session->newdata.errEllipseOrient = (double)(getleu16(buf, 6) * 1e-2);
2778
136
    session->newdata.errEllipseMajor = (double)(getleu32(buf, 8) * 1e-3);
2779
136
    session->newdata.errEllipseMinor = (double)(getleu32(buf, 12) * 1e-3);
2780
136
    errEllipseOrient = getleu16(buf, 6);
2781
136
    errEllipseMajor = getleu32(buf, 8);
2782
136
    errEllipseMinor = getleu32(buf, 12);
2783
136
    GPSD_LOG(LOG_PROG, &session->context->errout,
2784
136
             "UBX: NAV-EELL: iTOW %lld version %u errEllipseOrient %u "
2785
136
             "errEllipseMajor %lu errEllipseMinor %lu\n",
2786
136
             (long long)session->driver.ubx.iTOW, version,
2787
136
             errEllipseOrient,
2788
136
             errEllipseMajor,
2789
136
             errEllipseMinor);
2790
136
    return 0;
2791
202
}
2792
2793
/**
2794
 * End of Epoch
2795
 * Not in u-blox 5, 6 or 7
2796
 * Present in:
2797
 *    protVer 18 (8-series, 9)
2798
 */
2799
static gps_mask_t ubx_msg_nav_eoe(struct gps_device_t *session,
2800
                                  unsigned char *buf, size_t data_len)
2801
66
{
2802
66
    if (4 > data_len) {
2803
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
2804
0
                 "UBX: NAV-EOE: runt payload len %zd", data_len);
2805
0
        return 0;
2806
0
    }
2807
2808
66
    session->driver.ubx.iTOW = getleu32(buf, 0);
2809
66
    GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: NAV-EOE: iTOW=%lld\n",
2810
66
             (long long)session->driver.ubx.iTOW);
2811
    // nothing to report, but the iTOW for cycle ender is good
2812
66
    return 0;
2813
66
}
2814
2815
/*
2816
 * UBX-NAV-HPPOSECEF - High Precision Position Solution in ECEF
2817
 *
2818
 * Present in u-blox 8 and above, protVwer 20.00 and up.
2819
 * Only with High Precision firmware.
2820
 */
2821
static gps_mask_t ubx_msg_nav_hpposecef(struct gps_device_t *session,
2822
                                        unsigned char *buf,
2823
                                        size_t data_len UNUSED)
2824
101
{
2825
101
    gps_mask_t mask = ECEF_SET;
2826
2827
101
    unsigned version = getub(buf, 0);
2828
101
    if (0 != version) {
2829
67
        GPSD_LOG(LOG_WARN, &session->context->errout,
2830
67
                 "UBX: NAV-HPPOSECEF unknown version %u s/b 0", version);
2831
67
        return 0;
2832
67
    }
2833
34
    session->driver.ubx.iTOW = getleu32(buf, 4);
2834
34
    session->newdata.ecef.x = getles32x100s8d(buf, 8, 20, 1e-4);
2835
34
    session->newdata.ecef.y = getles32x100s8d(buf, 12, 21, 1e-4);
2836
34
    session->newdata.ecef.z = getles32x100s8d(buf, 16, 22, 1e-4);
2837
2838
34
    session->newdata.ecef.pAcc = getleu32(buf, 24) / (double)10000.0;
2839
    // (long long) cast for 32-bit compat
2840
34
    GPSD_LOG(LOG_PROG, &session->context->errout,
2841
34
        "UBX: NAV-HPPOSECEF: version %d iTOW %lld ecef X %.4f Y %.4f Z %.4f "
2842
34
        "pAcc %.4f\n",
2843
34
        version,
2844
34
        (long long)session->driver.ubx.iTOW,
2845
34
        session->newdata.ecef.x,
2846
34
        session->newdata.ecef.y,
2847
34
        session->newdata.ecef.z,
2848
34
        session->newdata.ecef.pAcc);
2849
34
    return mask;
2850
101
}
2851
2852
 /**
2853
 * High Precision Geodetic Position Solution
2854
 * UBX-NAV-HPPOSLLH, Class 1, ID x14
2855
 *
2856
 * No mode, so limited usefulness.
2857
 *
2858
 * Present in u-blox 8 and above, protVwer 20.00 and up.
2859
 * Only with High Precision firmware.
2860
 */
2861
static gps_mask_t ubx_msg_nav_hpposllh(struct gps_device_t *session,
2862
                                       unsigned char *buf, size_t data_len)
2863
132
{
2864
132
    int version;
2865
132
    gps_mask_t mask = 0;
2866
2867
132
    if (36 > data_len) {
2868
66
        GPSD_LOG(LOG_WARN, &session->context->errout,
2869
66
                 "UBX: NAV-HPPOSLLH: runt payload len %zd", data_len);
2870
66
        return mask;
2871
66
    }
2872
2873
66
    mask = ONLINE_SET | HERR_SET | VERR_SET | LATLON_SET | ALTITUDE_SET;
2874
2875
66
    version = getub(buf, 0);
2876
66
    session->driver.ubx.iTOW = getles32(buf, 4);
2877
66
    session->newdata.longitude = getles32x100s8d(buf, 8, 24, 1e-9);
2878
66
    session->newdata.latitude = getles32x100s8d(buf, 12, 25, 1e-9);
2879
    // altitude WGS84
2880
66
    session->newdata.altHAE = getles32x100s8d(buf, 16, 26, 1e-5);
2881
    // altitude MSL
2882
66
    session->newdata.altMSL = getles32x100s8d(buf, 20, 27, 1e-5);
2883
    // Let gpsd_error_model() deal with geoid_sep
2884
2885
    // Horizontal accuracy estimate in .1 mm, unknown est type
2886
66
    session->newdata.eph = getleu32(buf, 28) * (double)1e-4;
2887
    // Vertical accuracy estimate in .1 mm, unknown est type
2888
66
    session->newdata.epv = getleu32(buf, 32) * (double)1e-4;
2889
2890
66
    GPSD_LOG(LOG_PROG, &session->context->errout,
2891
66
        "UBX: NAV-HPPOSLLH: version %d iTOW=%lld lat=%.4f lon=%.4f "
2892
66
        "altHAE=%.4f\n",
2893
66
        version,
2894
66
        (long long)session->driver.ubx.iTOW,
2895
66
        session->newdata.latitude,
2896
66
        session->newdata.longitude,
2897
66
        session->newdata.altHAE);
2898
66
    return mask;
2899
132
}
2900
2901
/*
2902
 * Navigation Position ECEF message
2903
 *
2904
 * This message does not bother to tell us if it is valid.
2905
 */
2906
static gps_mask_t ubx_msg_nav_posecef(struct gps_device_t *session,
2907
                                      unsigned char *buf, size_t data_len)
2908
133
{
2909
133
    gps_mask_t mask = ECEF_SET;
2910
2911
133
    if (20 > data_len) {
2912
67
        GPSD_LOG(LOG_WARN, &session->context->errout,
2913
67
                 "UBX: NAV-POSECEF: runt payload len %zd", data_len);
2914
67
        return 0;
2915
67
    }
2916
2917
66
    session->driver.ubx.iTOW = getleu32(buf, 0);
2918
    // all in cm
2919
66
    session->newdata.ecef.x = getles32(buf, 4) * 1e-2;
2920
66
    session->newdata.ecef.y = getles32(buf, 8) * 1e-2;
2921
66
    session->newdata.ecef.z = getles32(buf, 12) * 1e-2;
2922
66
    session->newdata.ecef.pAcc = getleu32(buf, 16) * 1e-2;
2923
2924
    // (long long) cast for 32-bit compat
2925
66
    GPSD_LOG(LOG_PROG, &session->context->errout,
2926
66
        "UBX: NAV-POSECEF: iTOW=%lld ECEF x=%.2f y=%.2f z=%.2f pAcc=%.2f\n",
2927
66
        (long long)session->driver.ubx.iTOW,
2928
66
        session->newdata.ecef.x,
2929
66
        session->newdata.ecef.y,
2930
66
        session->newdata.ecef.z,
2931
66
        session->newdata.ecef.pAcc);
2932
66
    return mask;
2933
133
}
2934
2935
 /**
2936
 * Geodetic position solution message
2937
 * UBX-NAV-POSLLH, Class 1, ID 2
2938
 *
2939
 * This message does not bother to tell us if it is valid.
2940
 * No mode, so limited usefulness
2941
 */
2942
static gps_mask_t ubx_msg_nav_posllh(struct gps_device_t *session,
2943
                                     unsigned char *buf,
2944
                                     size_t data_len UNUSED)
2945
145
{
2946
145
    gps_mask_t mask = 0;
2947
2948
145
    if (28 > data_len) {
2949
71
        GPSD_LOG(LOG_WARN, &session->context->errout,
2950
71
                 "UBX: NAV-POSLLH: runt payload len %zd", data_len);
2951
71
        return 0;
2952
71
    }
2953
2954
74
    session->driver.ubx.iTOW = getleu32(buf, 0);
2955
74
    session->newdata.longitude = 1e-7 * getles32(buf, 4);
2956
74
    session->newdata.latitude = 1e-7 * getles32(buf, 8);
2957
    // altitude WGS84
2958
74
    session->newdata.altHAE = 1e-3 * getles32(buf, 12);
2959
    // altitude MSL
2960
74
    session->newdata.altMSL = 1e-3 * getles32(buf, 16);
2961
    // Let gpsd_error_model() deal with geoid_sep
2962
2963
    // Horizontal accuracy estimate in mm, unknown type
2964
74
    session->newdata.eph = getleu32(buf, 20) * 1e-3;
2965
    // Vertical accuracy estimate in mm, unknown type
2966
74
    session->newdata.epv = getleu32(buf, 24) * 1e-3;
2967
2968
74
    GPSD_LOG(LOG_PROG, &session->context->errout,
2969
74
        "UBX: NAV-POSLLH: iTOW=%lld lat=%.3f lon=%.3f altHAE=%.3f "
2970
74
        "eph %.3f epv %.3f\n",
2971
74
        (long long)session->driver.ubx.iTOW,
2972
74
        session->newdata.latitude,
2973
74
        session->newdata.longitude,
2974
74
        session->newdata.altHAE,
2975
74
        session->newdata.eph,
2976
74
        session->newdata.epv);
2977
2978
74
    mask = ONLINE_SET | HERR_SET | VERR_SET | LATLON_SET | ALTITUDE_SET;
2979
74
    return mask;
2980
145
}
2981
2982
/**
2983
 * Navigation Position Attitude Velocity Time solution message
2984
 * UBX-NAV-PVAT Class 1, ID 17
2985
 *
2986
 * Like UBX-NAV-PVT, plus parts of UBX-HNR-ATT, UBX-NAV-EELL, and NAV-TIMEUTC
2987
 *
2988
 * Present in:
2989
 *   protver 30  (ADR/DBD/HPS/LAP/MDR 9-series firmware)
2990
 *
2991
 * Not present in:
2992
 *    u-blox 5, 6, 7 or 8
2993
 */
2994
static gps_mask_t ubx_msg_nav_pvat(struct gps_device_t *session,
2995
                                  unsigned char *buf, size_t data_len UNUSED)
2996
570
{
2997
570
    char buf2[80];
2998
570
    char buf3[80];
2999
570
    char buf4[80];
3000
570
    unsigned version;
3001
570
    unsigned year, month, day, hour, min, sec;
3002
570
    double tAcc, hAcc, vAcc, sAcc;
3003
570
    double velN, velE, velD;
3004
570
    double vehRoll, vehPitch, vehHeading, motHeading;
3005
570
    double accRoll, accPitch, accHeading;
3006
570
    double magDec = NAN;
3007
570
    double magAcc = NAN;
3008
570
    long nano;
3009
570
    unsigned fixType;
3010
570
    unsigned flags;
3011
570
    unsigned flags2;
3012
570
    unsigned numSV;
3013
570
    unsigned valid;
3014
570
    int *status = &session->newdata.status;
3015
570
    int *mode = &session->newdata.mode;
3016
570
    gps_mask_t mask = 0;
3017
3018
570
    version = getub(buf, 4);
3019
570
    if (0 != version) {
3020
66
        GPSD_LOG(LOG_WARN, &session->context->errout,
3021
66
                 "UBX: NAV-PVAT unknown version %u s/b 0", version);
3022
66
        return 0;
3023
66
    }
3024
504
    session->driver.ubx.iTOW = getleu32(buf, 0);
3025
504
    valid = getub(buf, 5);
3026
504
    year = getleu16(buf, 6);
3027
504
    month = getub(buf, 8);
3028
504
    day = getub(buf, 9);
3029
504
    hour = getub(buf, 10);
3030
504
    min = getub(buf, 11);
3031
504
    sec = getub(buf, 12);
3032
    // 13, 14, 15 reserved
3033
504
    tAcc = 1e-3 * getleu32(buf, 16);
3034
504
    nano = getles32(buf, 20);
3035
504
    fixType = getub(buf, 24);
3036
504
    flags = getub(buf, 25);
3037
504
    flags2 = getub(buf, 26);
3038
504
    numSV = getub(buf, 27);
3039
3040
    // u-blox doc admits this may differ from skyview data.
3041
504
    session->gpsdata.satellites_used = numSV;
3042
3043
504
    switch (fixType) {
3044
0
    case UBX_MODE_TMONLY:
3045
        // 5 - Surveyed-in, so a precise 3D.
3046
0
        *mode = MODE_3D;
3047
0
        *status = STATUS_TIME;
3048
0
        mask |= STATUS_SET | LATLON_SET | ALTITUDE_SET | MODE_SET | SPEED_SET;
3049
0
        break;
3050
3051
67
    case UBX_MODE_3D:
3052
        // 3
3053
67
        *mode = MODE_3D;
3054
67
        *status = STATUS_GPS;
3055
67
        mask |= STATUS_SET | LATLON_SET | ALTITUDE_SET | MODE_SET | SPEED_SET;
3056
67
        break;
3057
3058
1
    case UBX_MODE_GPSDR:
3059
        // 4
3060
1
        *mode = MODE_3D;
3061
1
        *status = STATUS_GNSSDR;
3062
1
        mask |= STATUS_SET | LATLON_SET | ALTITUDE_SET | MODE_SET | SPEED_SET;
3063
1
        break;
3064
3065
23
    case UBX_MODE_2D:
3066
        // 2
3067
23
        *mode = MODE_2D;
3068
23
        *status = STATUS_GPS;
3069
23
        mask |= LATLON_SET | SPEED_SET | MODE_SET | STATUS_SET;
3070
23
        break;
3071
3072
18
    case UBX_MODE_DR:           // consider this too as 2D
3073
        // 1
3074
18
        *mode = MODE_2D;
3075
18
        *status = STATUS_DR;
3076
18
        mask |= LATLON_SET | SPEED_SET | MODE_SET | STATUS_SET;
3077
18
        break;
3078
3079
65
    case UBX_MODE_NOFIX:
3080
        // 0
3081
65
        FALLTHROUGH
3082
395
    default:
3083
        // huh?
3084
395
        *mode = MODE_NO_FIX;
3085
395
        *status = STATUS_UNK;
3086
395
        mask |= MODE_SET | STATUS_SET;
3087
395
        break;
3088
504
    }
3089
3090
504
    if (UBX_NAV_PVT_FLAG_DGPS == (flags & UBX_NAV_PVT_FLAG_DGPS)) {
3091
324
        if (STATUS_TIME == *status) {
3092
            // leave it
3093
324
        } else if (UBX_NAV_PVT_FLAG_RTK_FIX ==
3094
324
                   (flags & UBX_NAV_PVT_FLAG_RTK_FIX)) {
3095
135
            *status = STATUS_RTK_FIX;
3096
189
        } else if (UBX_NAV_PVT_FLAG_RTK_FLT ==
3097
189
                   (flags & UBX_NAV_PVT_FLAG_RTK_FLT)) {
3098
120
            *status = STATUS_RTK_FLT;
3099
120
        } else {
3100
69
            *status = STATUS_DGPS;
3101
69
        }
3102
324
        mask |= STATUS_SET;
3103
324
    }
3104
3105
504
    if ((valid & UBX_NAV_PVT_VALID_DATE_TIME) == UBX_NAV_PVT_VALID_DATE_TIME) {
3106
358
        struct tm unpacked_date = {0};
3107
3108
358
        unpacked_date.tm_year = year - 1900;
3109
358
        unpacked_date.tm_mon = month - 1;
3110
358
        unpacked_date.tm_mday = day;
3111
358
        unpacked_date.tm_hour = hour;
3112
358
        unpacked_date.tm_min = min;
3113
358
        unpacked_date.tm_sec = sec;
3114
358
        session->newdata.time.tv_sec = mkgmtime(&unpacked_date);
3115
        // nano, can be negative! So normalize
3116
358
        session->newdata.time.tv_nsec = nano;
3117
358
        TS_NORM(&session->newdata.time);
3118
        // compute GPS week
3119
358
        fix_week(session);
3120
3121
358
        mask |= TIME_SET | NTPTIME_IS | GOODTIME_IS;
3122
358
    }
3123
3124
504
    if (LATLON_SET == (mask & LATLON_SET)) {
3125
109
        session->newdata.longitude = 1e-7 * getles32(buf, 28);
3126
109
        session->newdata.latitude = 1e-7 * getles32(buf, 32);
3127
109
        if (ALTITUDE_SET == (mask & ALTITUDE_SET)) {
3128
            // altitude WGS84
3129
68
            session->newdata.altHAE = 1e-3 * getles32(buf, 36);
3130
            // altitude MSL
3131
68
            session->newdata.altMSL = 1e-3 * getles32(buf, 40);
3132
            // Let gpsd_error_model() deal with geoid_sep
3133
68
        }
3134
109
    }
3135
504
    hAcc = 1e-3 * getleu32(buf, 44);
3136
504
    vAcc = 1e-3 * getleu32(buf, 48);
3137
504
    velN = 1e-3 * getles32(buf, 52);
3138
504
    velE = 1e-3 * getles32(buf, 56);
3139
504
    velD = 1e-3 * getles32(buf, 60);
3140
3141
    // Seems to be always valid.
3142
504
    session->newdata.eph = hAcc;
3143
504
    session->newdata.epv = vAcc;
3144
504
    session->newdata.NED.velN = velN;
3145
504
    session->newdata.NED.velE = velE;
3146
504
    session->newdata.NED.velD = velD;
3147
504
    mask |= VNED_SET;
3148
3149
    // gSpeed, seems to be always valid.
3150
504
    session->newdata.speed = 1e-3 * getles32(buf, 64);
3151
504
    sAcc = 1e-3 * getleu32(buf, 68);
3152
504
    session->newdata.eps = sAcc;
3153
504
    mask |= SPEED_SET;
3154
3155
504
    vehRoll = 1e-5 * getles32(buf, 72);
3156
504
    vehPitch = 1e-5 * getles32(buf, 76);
3157
504
    vehHeading = 1e-5 * getles32(buf, 80);
3158
3159
    // accuracies
3160
504
    accRoll = 1e-3 * getles32(buf, 88);
3161
504
    accPitch = 1e-3 * getles32(buf, 90);
3162
504
    accHeading = 1e-3 * getles32(buf, 92);
3163
3164
504
    if (0.0 != accRoll) {
3165
413
        session->gpsdata.attitude.roll = vehRoll;
3166
413
        mask |= ATTITUDE_SET;
3167
413
    }
3168
504
    if (0.0 != accPitch) {
3169
377
        session->gpsdata.attitude.pitch = vehPitch;
3170
377
        mask |= ATTITUDE_SET;
3171
377
    }
3172
504
    if (0.0 != accHeading) {
3173
        // seems to be true heading
3174
376
        session->gpsdata.attitude.heading = vehHeading;
3175
376
        mask |= ATTITUDE_SET;
3176
376
    }
3177
3178
504
    motHeading = 1e-5 * getles32(buf, 84);
3179
504
    if (flags & UBX_NAV_PVT_FLAG_HDG_OK) {
3180
        // u-blox calls this Heading of motion (2-D)
3181
272
        session->newdata.track = motHeading;
3182
272
        mask |= TRACK_SET;
3183
272
    }
3184
3185
504
    if (ATTITUDE_SET == (mask & ATTITUDE_SET)) {
3186
438
        session->gpsdata.attitude.mtime = session->newdata.time;
3187
438
    }
3188
3189
504
    if (valid & UBX_NAV_PVT_VALID_MAG) {
3190
381
        magDec = (double)(getles16(buf, 94) * 1e-2);
3191
381
        magAcc = (double)(getleu16(buf, 96) * 1e-2);
3192
381
    }
3193
504
    session->newdata.errEllipseOrient = (double)(getleu16(buf, 98) * 1e-2);
3194
504
    session->newdata.errEllipseMajor = (double)(getleu32(buf, 100) * 1e-3);
3195
504
    session->newdata.errEllipseMinor = (double)(getleu32(buf, 104) * 1e-3);
3196
3197
    // if cycle ender worked, could get rid of this REPORT_IS.
3198
    // mask |= REPORT_IS;
3199
3200
504
    GPSD_LOG(LOG_PROG, &session->context->errout,
3201
504
         "UBX: NAV-PVAT: iTOW %lld version %d valid x%02x "
3202
504
         "time %u/%02u/%02u %02u:%02u:%02u tAcc %.3f nano %lu fixType %u "
3203
504
         "flags x%x flags2 x%x numSV %u lat %.2f lon %.2f "
3204
504
         "altHAE %.2f altMSL %.2f "
3205
504
         "hAcc %.3f vAcc %.3f valNED %.3f %.3f %.3f speed %.3f sAcc %.3f "
3206
504
         "vehRPH %.5f %.5f %.5f "
3207
504
         "track %.2f accRPH %.3f %.3f %.3f "
3208
504
         "mode %d status %d used %d magDec %.2f magAcc %.2f "
3209
504
         "errEllipse %.2f %.3f %.3f\n",
3210
504
         (long long)session->driver.ubx.iTOW, version,
3211
         // timespec_str(&session->newdata.time, ts_buf, sizeof(ts_buf)),
3212
504
         valid, year, month, day, hour, min, sec, tAcc, nano, fixType,
3213
504
         flags, flags2, numSV,
3214
504
         session->newdata.latitude,
3215
504
         session->newdata.longitude,
3216
504
         session->newdata.altHAE,
3217
504
         session->newdata.altMSL,
3218
504
         hAcc, vAcc, velN, velE, velD, session->newdata.speed, sAcc,
3219
504
         vehRoll, vehPitch, vehHeading,
3220
504
         motHeading,
3221
         //session->newdata.track,
3222
504
         accRoll, accPitch, accHeading,
3223
504
         session->newdata.mode,
3224
504
         session->newdata.status,
3225
504
         session->gpsdata.satellites_used,
3226
504
         magDec, magAcc,
3227
504
         session->newdata.errEllipseOrient,
3228
504
         session->newdata.errEllipseMajor,
3229
504
         session->newdata.errEllipseMinor);
3230
504
    GPSD_LOG(LOG_IO, &session->context->errout,
3231
504
             "UBX: NAV-PVAT: fixType(%s) flags(%s) flags2(%s) valid(%s)\n",
3232
504
             val2str(fixType, vpvt_fixType),
3233
504
             flags2str(flags, fnav_pvt_flags, buf2, sizeof(buf2)),
3234
504
             flags2str(flags2, fpvt_flags2, buf3, sizeof(buf3)),
3235
504
             flags2str(valid, fpvt_valid, buf4, sizeof(buf4)));
3236
504
    return mask;
3237
504
}
3238
3239
/**
3240
 * Navigation Position Velocity Time solution message
3241
 * UBX-NAV-PVT Class 1, ID 7
3242
 *
3243
 * Includes part of UBX-NAV-TIMEUTC
3244
 *
3245
 * Present in:
3246
 *   protver 14  (6-series w/ GLONASS, 7-series)
3247
 *
3248
 * Not present in:
3249
 *    u-blox 5 or 6
3250
 */
3251
static gps_mask_t ubx_msg_nav_pvt(struct gps_device_t *session,
3252
                                  unsigned char *buf, size_t data_len UNUSED)
3253
498
{
3254
498
    char buf2[80];
3255
498
    char buf3[80];
3256
498
    char buf4[80];
3257
498
    char buf5[80];
3258
498
    unsigned dgps_age;
3259
498
    struct tm unpacked_date = {0};
3260
498
    int *status = &session->newdata.status;
3261
498
    int *mode = &session->newdata.mode;
3262
498
    gps_mask_t mask = 0;
3263
498
    char ts_buf[TIMESPEC_LEN];
3264
3265
498
    session->driver.ubx.iTOW = getleu32(buf, 0);
3266
498
    unsigned valid = getub(buf, 11);
3267
498
    unsigned fixType = getub(buf, 20);
3268
498
    unsigned flags = getub(buf, 21);
3269
498
    unsigned flags2 = getub(buf, 22);
3270
498
    unsigned numSV = getub(buf, 23);
3271
498
    unsigned flags3 = getleu16(buf, 78);
3272
3273
    // session->gpsdata.satellites_used = numSV;
3274
3275
498
    switch (fixType) {
3276
10
    case UBX_MODE_TMONLY:
3277
        // 5 - Surveyed-in, so a precise 3D.
3278
10
        *mode = MODE_3D;
3279
10
        *status = STATUS_TIME;
3280
10
        mask |= STATUS_SET | MODE_SET;
3281
10
        break;
3282
3283
37
    case UBX_MODE_3D:
3284
        // 3
3285
37
        *mode = MODE_3D;
3286
37
        *status = STATUS_GPS;
3287
37
        mask |= STATUS_SET | LATLON_SET | MODE_SET;
3288
37
        break;
3289
3290
62
    case UBX_MODE_GPSDR:
3291
        // 4
3292
62
        *mode = MODE_3D;
3293
62
        *status = STATUS_GNSSDR;
3294
62
        mask |= STATUS_SET | LATLON_SET | MODE_SET;
3295
62
        break;
3296
3297
75
    case UBX_MODE_2D:
3298
        // 2
3299
75
        *mode = MODE_2D;
3300
75
        *status = STATUS_GPS;
3301
75
        mask |= LATLON_SET | SPEED_SET | MODE_SET | STATUS_SET;
3302
75
        break;
3303
3304
33
    case UBX_MODE_DR:           // consider this too as 2D
3305
        // 1
3306
33
        *mode = MODE_2D;
3307
33
        *status = STATUS_DR;
3308
33
        mask |= LATLON_SET | SPEED_SET | MODE_SET | STATUS_SET;
3309
33
        break;
3310
3311
101
    case UBX_MODE_NOFIX:
3312
        // 0
3313
101
        FALLTHROUGH
3314
281
    default:
3315
        // huh?
3316
281
        *mode = MODE_NO_FIX;
3317
281
        *status = STATUS_UNK;
3318
281
        mask |= MODE_SET | STATUS_SET;
3319
281
        break;
3320
498
    }
3321
3322
498
    if (UBX_NAV_PVT_FLAG_DGPS == (flags & UBX_NAV_PVT_FLAG_DGPS)) {
3323
        // RTK flags not before protoVer 20.
3324
312
        if (STATUS_TIME == *status) {
3325
            // leave it
3326
302
        } else if (UBX_NAV_PVT_FLAG_RTK_FIX ==
3327
302
                   (flags & UBX_NAV_PVT_FLAG_RTK_FIX)) {
3328
66
            *status = STATUS_RTK_FIX;
3329
236
        } else if (UBX_NAV_PVT_FLAG_RTK_FLT ==
3330
236
                   (flags & UBX_NAV_PVT_FLAG_RTK_FLT)) {
3331
75
            *status = STATUS_RTK_FLT;
3332
161
        } else {
3333
161
            *status = STATUS_DGPS;
3334
161
        }
3335
3336
312
        dgps_age = (flags3 >> 1) & 0x0f;
3337
312
        if (0 < dgps_age) {
3338
245
            if (ROWS(pvt_dgps_age) <= dgps_age) {
3339
85
                dgps_age = ROWS(pvt_dgps_age) - 1;
3340
85
            }
3341
245
            session->newdata.dgps_age = pvt_dgps_age[dgps_age];
3342
245
        }
3343
312
        mask |= STATUS_SET;
3344
312
    }
3345
3346
498
    if ((valid & UBX_NAV_PVT_VALID_DATE_TIME) == UBX_NAV_PVT_VALID_DATE_TIME) {
3347
329
        unpacked_date.tm_year = (uint16_t)getleu16(buf, 4) - 1900;
3348
329
        unpacked_date.tm_mon = (uint8_t)getub(buf, 6) - 1;
3349
329
        unpacked_date.tm_mday = (uint8_t)getub(buf, 7);
3350
329
        unpacked_date.tm_hour = (uint8_t)getub(buf, 8);
3351
329
        unpacked_date.tm_min = (uint8_t)getub(buf, 9);
3352
329
        unpacked_date.tm_sec = (uint8_t)getub(buf, 10);
3353
329
        unpacked_date.tm_isdst = 0;
3354
329
        unpacked_date.tm_wday = 0;
3355
329
        unpacked_date.tm_yday = 0;
3356
329
        session->newdata.time.tv_sec = mkgmtime(&unpacked_date);
3357
        // field 16, nano, can be negative! So normalize
3358
329
        session->newdata.time.tv_nsec = getles32(buf, 16);
3359
329
        TS_NORM(&session->newdata.time);
3360
        // compute GPS week
3361
329
        fix_week(session);
3362
3363
329
        mask |= TIME_SET | NTPTIME_IS | GOODTIME_IS;
3364
329
    }
3365
3366
498
    session->newdata.longitude = 1e-7 * getles32(buf, 24);
3367
498
    session->newdata.latitude = 1e-7 * getles32(buf, 28);
3368
    // altitude WGS84
3369
498
    session->newdata.altHAE = 1e-3 * getles32(buf, 32);
3370
    // altitude MSL
3371
498
    session->newdata.altMSL = 1e-3 * getles32(buf, 36);
3372
    // Let gpsd_error_model() deal with geoid_sep
3373
3374
498
    session->newdata.speed = 1e-3 * (int32_t)getles32(buf, 60);
3375
    // u-blox calls this Heading of motion (2-D)
3376
498
    session->newdata.track = 1e-5 * (int32_t)getles32(buf, 64);
3377
    // FIXME!!!!!
3378
498
    mask |= LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET;
3379
3380
    /* u-blox does not document the basis for the following "accuracy"
3381
     * estimates.  Maybe CEP(50), one sigma, two sigma, CEP(99), etc. */
3382
3383
    // Horizontal Accuracy estimate, in mm
3384
498
    session->newdata.eph = (double)(getles32(buf, 40) / 1000.0);
3385
    // Vertical Accuracy estimate, in mm
3386
498
    session->newdata.epv = (double)(getles32(buf, 44) / 1000.0);
3387
    // Speed Accuracy estimate, in mm/s
3388
498
    session->newdata.eps = (double)(getles32(buf, 68) / 1000.0);
3389
    // let gpsd_error_model() do the rest
3390
3391
498
    mask |= HERR_SET | SPEEDERR_SET | VERR_SET;
3392
    // if cycle ender worked, could get rid of this REPORT_IS.
3393
    // mask |= REPORT_IS;
3394
3395
498
    GPSD_LOG(LOG_PROG, &session->context->errout,
3396
498
         "UBX: NAV-PVT: flags %02x time %s valid x%x lat %.2f lon %.2f "
3397
498
         "altHAE %.2f "
3398
498
         "track %.2f speed %.2f mode %d status %d used %d dgps_age %.0f\n",
3399
498
         flags,
3400
498
         timespec_str(&session->newdata.time, ts_buf, sizeof(ts_buf)),
3401
498
         valid,
3402
498
         session->newdata.latitude,
3403
498
         session->newdata.longitude,
3404
498
         session->newdata.altHAE,
3405
498
         session->newdata.track,
3406
498
         session->newdata.speed,
3407
498
         session->newdata.mode,
3408
498
         session->newdata.status,
3409
498
         numSV,   // session->gpsdata.satellites_used,
3410
498
         session->newdata.dgps_age);
3411
498
    GPSD_LOG(LOG_IO, &session->context->errout,
3412
498
             "UBX: NAV-PVT: fixType %s flags(%s_ flags2(%s) flags3(%s) "
3413
498
             "valid(%s)\n",
3414
498
             val2str(fixType, vpvt_fixType),
3415
498
             flags2str(flags, fnav_pvt_flags, buf2, sizeof(buf2)),
3416
498
             flags2str(flags2, fpvt_flags2, buf3, sizeof(buf3)),
3417
498
             flags2str(flags3, fpvt_flags3, buf4, sizeof(buf4)),
3418
498
             flags2str(valid, fpvt_valid, buf5, sizeof(buf5)));
3419
498
    if (92 <= data_len) {
3420
        // u-blox 8 and 9 extended
3421
422
        double magDec = NAN;
3422
422
        double magAcc = NAN;
3423
#ifdef __UNUSED
3424
        if (flags & UBX_NAV_PVT_FLAG_HDG_OK) {
3425
            /* u-blox calls this Heading of vehicle (2-D)
3426
             * why is it different than earlier track? */
3427
            session->newdata.track = (double)(getles32(buf, 84) * 1e-5);
3428
        }
3429
#endif  // __UNUSED
3430
422
        if (valid & UBX_NAV_PVT_VALID_MAG) {
3431
209
            magDec = (double)(getles16(buf, 88) * 1e-2);
3432
209
            magAcc = (double)(getleu16(buf, 90) * 1e-2);
3433
209
        }
3434
422
        GPSD_LOG(LOG_PROG, &session->context->errout,
3435
422
             " UBX: NAV-PVT: headVeh %.5f magDec %.2f magAcc %.2f\n",
3436
422
             session->newdata.track, magDec, magAcc);
3437
422
    }
3438
498
    return mask;
3439
498
}
3440
3441
 /**
3442
 * High Precision Relative Positioning Information in NED frame
3443
 * UBX-NAV-RELPOSNED, Class 1, ID x3c
3444
 * HP GNSS only, protver 20+
3445
 */
3446
static gps_mask_t ubx_msg_nav_relposned(struct gps_device_t *session,
3447
                                        unsigned char *buf, size_t data_len)
3448
464
{
3449
464
    int version;
3450
464
    unsigned flags;
3451
464
    double accN = NAN, accE = NAN, accD = NAN, accL = NAN, accH = NAN;
3452
464
    gps_mask_t mask = 0;
3453
3454
464
    if (40 > data_len) {
3455
66
        GPSD_LOG(LOG_WARN, &session->context->errout,
3456
66
                 "UBX: NAV-RELPOSNED:0: runt payload len %zd",
3457
66
                 data_len);
3458
66
        return mask;
3459
66
    }
3460
398
    version = getub(buf, 0);
3461
    /* WTF?  u-blox did not make this sentence upward compatible
3462
     * 40 bytes in Version 0, protVer 20 to 27
3463
     * 64 bytes in Version 1, protVer 27.11+ */
3464
3465
398
    session->newdata.dgps_station = getleu16(buf, 2);          // 0 to 4095
3466
398
    session->driver.ubx.iTOW = getleu32(buf, 4);
3467
398
    if (1 > version) {
3468
        // version 0
3469
166
        flags = getleu32(buf, 36);
3470
166
        if (1 != (1 & flags)) {
3471
            // not gnssFixOK
3472
34
            GPSD_LOG(LOG_PROG, &session->context->errout,
3473
34
                     "UBX: NAV-RELPOSNED:0 no fix");
3474
34
            return mask;
3475
34
        }
3476
132
        if (4 & flags) {
3477
            // rePosValid
3478
66
            session->newdata.NED.relPosN = getles32x100s8d(buf, 8, 20, 1e-4);
3479
66
            session->newdata.NED.relPosE = getles32x100s8d(buf, 12, 21, 1e-4);
3480
66
            session->newdata.NED.relPosD = getles32x100s8d(buf, 16, 22, 1e-4);
3481
3482
66
            accN = 1e-4 * getles32(buf, 24);
3483
66
            accE = 1e-4 * getles32(buf, 28);
3484
66
            accD = 1e-4 * getles32(buf, 32);
3485
66
            mask |= NED_SET;
3486
66
        }
3487
232
    } else {
3488
        // assume version 1
3489
232
        if (64 > data_len) {
3490
0
            GPSD_LOG(LOG_WARN, &session->context->errout,
3491
0
                     "UBX: NAV-RELPOSNED:1: runt payload len %zd",
3492
0
                     data_len);
3493
0
            return mask;
3494
0
        }
3495
232
        flags = getleu32(buf, 60);
3496
232
        if (1 != (1 & flags)) {
3497
            // not gnssFixOK
3498
66
            GPSD_LOG(LOG_PROG, &session->context->errout,
3499
66
                     "UBX: NAV-RELPOSNED:1 no fix");
3500
66
            return mask;
3501
66
        }
3502
166
        if (4 & flags) {
3503
            // rePosValid
3504
100
            session->newdata.NED.relPosN = getles32x100s8d(buf, 8, 32, 1e-4);
3505
100
            session->newdata.NED.relPosE = getles32x100s8d(buf, 12, 33, 1e-4);
3506
100
            session->newdata.NED.relPosD = getles32x100s8d(buf, 16, 34, 1e-4);
3507
100
            session->newdata.NED.relPosL = getles32x100s8d(buf, 20, 35, 1e-4);
3508
3509
100
            accN = 1e-4 * getles32(buf, 36);
3510
100
            accE = 1e-4 * getles32(buf, 40);
3511
100
            accD = 1e-4 * getles32(buf, 44);
3512
100
            accL = 1e-4 * getles32(buf, 48);
3513
100
            accH = 1e-4 * getles32(buf, 52);
3514
100
            if (0x100 & flags) {
3515
                // relPosHeadingValid
3516
66
                session->newdata.NED.relPosH = 1e-5 * getles32(buf, 24);
3517
66
            }
3518
100
            mask |= NED_SET;
3519
            // FIXME: RTK flags?
3520
100
        }
3521
166
    }
3522
3523
298
    GPSD_LOG(LOG_PROG, &session->context->errout,
3524
298
        "UBX: NAV-RELPOSNED: version %d iTOW=%lld refStationId %u flags x%x\n"
3525
298
        "UBX: NAV-RELPOSNED: relPos N=%.4f E=%.4f D=%.4f\n"
3526
298
        "UBX: NAV-RELPOSNED: acc N=%.4f E=%.4f D=%.4f L=%.4f H=%.4f\n",
3527
298
        version,
3528
298
        (long long)session->driver.ubx.iTOW,
3529
298
        session->newdata.dgps_station,
3530
298
        flags,
3531
298
        session->newdata.NED.relPosN,
3532
298
        session->newdata.NED.relPosE,
3533
298
        session->newdata.NED.relPosD,
3534
298
        accN, accE, accD, accL, accH);
3535
3536
298
    if (5 != (flags & 5)) {
3537
        // gnssFixOK or relPosValid are false, no fix
3538
132
        return 0;
3539
132
    }
3540
166
    return mask;
3541
298
}
3542
3543
/**
3544
 * GPS Satellite Info -- new style UBX-NAV-SAT
3545
 * Not present in:
3546
 *     u-blox 5
3547
 *     protVer 12  5 and 6-series
3548
 * Present in:
3549
 *    protVer 15_  8-series
3550
 *    protVer 27   (ZED-F9P)
3551
 */
3552
static gps_mask_t ubx_msg_nav_sat(struct gps_device_t *session,
3553
                                  unsigned char *buf, size_t data_len)
3554
440
{
3555
440
    char buf2[80];
3556
440
    unsigned i, nchan, ver;
3557
440
    unsigned seen = 0, used_tot = 0;
3558
440
    timespec_t ts_tow;
3559
440
    char ts_buf[TIMESPEC_LEN];
3560
3561
440
    if (8 > data_len) {
3562
77
        GPSD_LOG(LOG_PROG, &session->context->errout,
3563
77
                 "UBX: NAV-SAT runt datalen %zd\n", data_len);
3564
77
        return 0;
3565
77
    }
3566
3567
363
    session->driver.ubx.iTOW = getleu32(buf, 0);
3568
363
    MSTOTS(&ts_tow, session->driver.ubx.iTOW);
3569
363
    session->gpsdata.skyview_time =
3570
363
        gpsd_gpstime_resolv(session, session->context->gps_week, ts_tow);
3571
3572
363
    ver = getub(buf, 4);
3573
363
    if (1 != ver) {
3574
85
        GPSD_LOG(LOG_WARN, &session->context->errout,
3575
85
                 "UBX: NAV-SAT unknown version %d\n", ver);
3576
85
        return 0;
3577
85
    }
3578
278
    nchan = getub(buf, 5);
3579
278
    if (nchan > MAXCHANNELS) {
3580
80
        GPSD_LOG(LOG_WARN, &session->context->errout,
3581
80
                 "UBX: NAV-SAT: too many channels %d\n",
3582
80
                 nchan);
3583
80
        return 0;
3584
80
    }
3585
198
    if ((8 + 12 * nchan) > data_len) {
3586
66
        GPSD_LOG(LOG_WARN, &session->context->errout,
3587
66
                 "UBX: NAV-SAT: runt payload %zd channels %u\n",
3588
66
                 data_len, nchan);
3589
66
        return 0;
3590
66
    }
3591
3592
#ifdef __UNUSED__    // debug
3593
    GPSD_LOG(LOG_SHOUT, &session->context->errout,
3594
             "UBX: NAV-SAT: gpsd_zero_satellites()\n");
3595
#endif
3596
132
    gpsd_zero_satellites(&session->gpsdata);
3597
3598
198
    for (i = 0; i < nchan; i++) {
3599
66
        unsigned off = 8 + 12 * i;
3600
66
        short nmea_PRN = 0;
3601
66
        uint8_t gnssId = getub(buf, off + 0);
3602
66
        uint8_t svId = getub(buf, off + 1);
3603
66
        uint8_t cno = getub(buf, off + 2);
3604
66
        int elev = getsb(buf, off + 3);
3605
66
        int azim = getles16(buf, off + 4);
3606
66
        int prRes = getles16(buf, off + 6);
3607
        // health data in flags
3608
66
        unsigned long flags = getleu32(buf, off + 8);
3609
66
        bool used = (bool)(flags  & 0x08);
3610
        // Notice NO sigid!
3611
3612
66
        nmea_PRN = ubx2_to_prn(gnssId, svId);
3613
66
        if (0 >= nmea_PRN) {
3614
            // bad PRN??
3615
66
            GPSD_LOG(LOG_WARN, &session->context->errout,
3616
66
                     "UBX: NAV-SAT(%d) Bad PRN: gnssid %u, svid %u PRN %d\n",
3617
66
                     seen, gnssId, svId, nmea_PRN);
3618
66
            continue;
3619
66
        }
3620
0
        session->gpsdata.skyview[seen].gnssid = gnssId;
3621
0
        session->gpsdata.skyview[seen].svid = svId;
3622
0
        session->gpsdata.skyview[seen].PRN = nmea_PRN;
3623
3624
0
        session->gpsdata.skyview[seen].ss = (double)cno;
3625
0
        if (90 >= abs(elev)) {
3626
0
            session->gpsdata.skyview[seen].elevation = (double)elev;
3627
0
        }
3628
        /* For some reason UBX allows 360 == azim here, but gpsd json does not
3629
         * so fix that.  Other UBX specifies 0-359. */
3630
0
        if (360 == azim) {
3631
0
            azim = 0;
3632
0
        }
3633
0
        if (360 > azim &&
3634
0
            0 <= azim) {
3635
0
            session->gpsdata.skyview[seen].azimuth = (double)azim;
3636
0
        }
3637
0
        session->gpsdata.skyview[seen].used = used;
3638
        // sbas_in_use is not same as used
3639
0
        if (used) {
3640
0
            used_tot++;
3641
0
        }
3642
0
        session->gpsdata.skyview[seen].prRes = prRes / 10.0;
3643
        // by some coincidence, our health flags matches u-blox's
3644
0
        session->gpsdata.skyview[seen].health = (flags >> 4) & 3;
3645
0
        session->gpsdata.skyview[seen].qualityInd = flags & 7;
3646
        // FIXME: sigid?
3647
0
        GPSD_LOG(LOG_PROG, &session->context->errout,
3648
0
                 "UBX: NAV-SAT(%d) gnssid %u, svid %u PRN %d "
3649
0
                 "prRes %d cno %u el %.1f az %.1f qual %d flags x%lx\n",
3650
0
                 seen, gnssId, svId, nmea_PRN, prRes, cno,
3651
0
                 session->gpsdata.skyview[seen].elevation,
3652
0
                 session->gpsdata.skyview[seen].azimuth,
3653
0
                 session->gpsdata.skyview[seen].qualityInd,
3654
0
                 flags);
3655
0
        GPSD_LOG(LOG_IO, &session->context->errout,
3656
0
                 "UBX: NAV-SAT: gnssId:%s flags:%s quality:%s\n",
3657
0
                 val2str(gnssId, vgnssId),
3658
0
                 flags2str(flags, fsat_flags, buf2, sizeof(buf2)),
3659
0
                 val2str(flags & 7, vquality));
3660
3661
0
        seen++;
3662
0
    }
3663
3664
132
    session->gpsdata.satellites_visible = seen;
3665
132
    session->gpsdata.satellites_used = used_tot;
3666
132
    GPSD_LOG(LOG_PROG, &session->context->errout,
3667
132
             "UBX: NAV-SAT: time %s visible %d used %d "
3668
132
             "mask={SATELLITE|USED}\n",
3669
132
             timespec_str(&session->gpsdata.skyview_time,
3670
132
                          ts_buf, sizeof(ts_buf)),
3671
132
             session->gpsdata.satellites_visible,
3672
132
             session->gpsdata.satellites_used);
3673
132
    return SATELLITE_SET | USED_IS;
3674
198
}
3675
3676
/*
3677
 * SBAS Info UBX-NAV-SBAS
3678
 * in u-blox 4_
3679
 * in NEO-M9N
3680
 * Not in some u-blox 9
3681
 * Decode looks good, but data only goes to log.
3682
 */
3683
static gps_mask_t ubx_msg_nav_sbas(struct gps_device_t *session,
3684
                                   unsigned char *buf, size_t data_len)
3685
1.11k
{
3686
1.11k
    unsigned i, cnt;
3687
1.11k
    unsigned ubx_PRN;
3688
1.11k
    short nmea_PRN;
3689
1.11k
    gnssid_t gnssid = 0;
3690
1.11k
    unsigned char svid = 0;
3691
3692
1.11k
    if (12 > data_len) {
3693
69
        GPSD_LOG(LOG_WARN, &session->context->errout,
3694
69
                 "UBX: NAV-SBAS: runt payload len %zd", data_len);
3695
69
        return 0;
3696
69
    }
3697
3698
1.04k
    session->driver.ubx.iTOW = getleu32(buf, 0);
3699
1.04k
    ubx_PRN = getub(buf, 4);
3700
1.04k
    cnt = getub(buf, 8);
3701
1.04k
    GPSD_LOG(LOG_PROG, &session->context->errout,
3702
1.04k
             "UBX: NAV-SBAS iTOW %lu geo %u mode %u sys %u service x%x "
3703
1.04k
             "cnt %u\n",
3704
1.04k
             (unsigned long)session->driver.ubx.iTOW,
3705
1.04k
             ubx_PRN, (unsigned)getub(buf, 5),
3706
1.04k
             (unsigned)getub(buf, 6), (unsigned)getub(buf, 7),
3707
1.04k
             cnt);
3708
3709
1.04k
    if (MAXCHANNELS < cnt) {
3710
        // too many sats for us, pacify coverity
3711
66
        cnt = MAXCHANNELS;
3712
66
    }
3713
1.04k
    if (data_len < (12 + (12 * cnt))) {
3714
        // length check, pacify coverity
3715
132
        GPSD_LOG(LOG_WARN, &session->context->errout,
3716
132
                 "UBX: NAV-SBAS: bad length %zd", data_len);
3717
132
        return 0;
3718
132
    }
3719
1.67k
    for (i = 0; i < cnt; i++) {
3720
761
        unsigned off = 12 + (12 * i);
3721
761
        unsigned svID = getub(buf, off);
3722
761
        unsigned flags = getub(buf, off + 1);
3723
        // User Differential Range Error (udre)
3724
761
        unsigned udre = getub(buf, off + 2);
3725
761
        int svSys = getsb(buf, off + 3);
3726
761
        unsigned svService = getub(buf, off + 4);
3727
761
        int prc = getles16(buf, off + 6);
3728
761
        int ic = getles16(buf, off + 10);
3729
761
        GPSD_LOG(LOG_PROG, &session->context->errout,
3730
761
                 "UBX: NAV-SBAS SV%3u flags x%02x udre %u svSys %2d "
3731
761
                 "svService x%x prc %d ic %d\n",
3732
761
                 svID, flags, udre, svSys, svService, prc, ic);
3733
761
    }
3734
    /* really 'in_use' depends on the sats info, EGNOS is still
3735
     * in test.  In WAAS areas one might also check for the type of
3736
     * corrections indicated
3737
     */
3738
3739
911
    nmea_PRN = ubx_to_prn(ubx_PRN, &gnssid, &svid);
3740
#ifdef __UNUSED
3741
    // debug
3742
    GPSD_LOG(LOG_ERROR, &session->context->errout,
3743
             "UBX: NAV-SBAS ubx_prn %d gnssid %d, svid %d nmea_PRN %d\n",
3744
             ubx_PRN, gnssid, svid, nmea_PRN);
3745
#endif  // __UNUSED
3746
911
    session->driver.ubx.sbas_in_use = nmea_PRN;
3747
911
    return 0;
3748
1.04k
}
3749
3750
/**
3751
 * Satellite Info -- UBX-NAV-SIG
3752
 *
3753
 * Like NAV-SAT, but NAV-SIG has no elevation and azimuth!  So we need both.
3754
 * Assume NAV-SAT was sent in this epoch before NAV-SIG.
3755
 * Seems like NAV-SAT always sent just before NAV-SIG.
3756
 *
3757
 * Present in:
3758
 *    protVer 27 (9-series and 10)
3759
 * Not present in:
3760
 *    protVer 12 6-eries
3761
 *    before protVer 27
3762
 */
3763
static gps_mask_t ubx_msg_nav_sig(struct gps_device_t *session,
3764
                                  unsigned char *buf, size_t data_len)
3765
562
{
3766
562
    unsigned int i, nchan, ver;
3767
562
    int seen = 0, used_tot = 0;
3768
562
    timespec_t ts_tow;
3769
    // saved skyview, hopefully from NAV-SAT
3770
562
    struct satellite_t skyview_old[MAXCHANNELS];
3771
562
    char ts_buf[TIMESPEC_LEN];
3772
3773
562
    if (8 > data_len) {
3774
68
        GPSD_LOG(LOG_PROG, &session->context->errout,
3775
68
                 "UBX: NAV-SIG runt datalen %zd\n", data_len);
3776
68
        return 0;
3777
68
    }
3778
3779
494
    session->driver.ubx.iTOW = getleu32(buf, 0);
3780
494
    MSTOTS(&ts_tow, session->driver.ubx.iTOW);
3781
494
    session->gpsdata.skyview_time =
3782
494
        gpsd_gpstime_resolv(session, session->context->gps_week, ts_tow);
3783
3784
494
    ver = getub(buf, 4);
3785
494
    if (0 != ver) {
3786
92
        GPSD_LOG(LOG_WARN, &session->context->errout,
3787
92
                 "UBX: NAV-SIG unknown version %d s/b 0", ver);
3788
92
        return 0;
3789
92
    }
3790
402
    nchan = getub(buf, 5);
3791
402
    if (nchan > MAXCHANNELS) {
3792
66
        GPSD_LOG(LOG_WARN, &session->context->errout,
3793
66
                 "UBX: NAV-SIG: nchan %u > MAXCHANNELS %d",
3794
66
                 nchan, MAXCHANNELS);
3795
66
        return 0;
3796
66
    }
3797
336
    if ((8 + 16 * nchan) > data_len) {
3798
205
        GPSD_LOG(LOG_WARN, &session->context->errout,
3799
205
                 "UBX: NAV-SIG: runt payload %zd channels %u\n",
3800
205
                 data_len, nchan);
3801
205
        return 0;
3802
205
    }
3803
3804
    // two "unused" bytes at buf[6:7]
3805
3806
    /* elevation and azimuth are in NAV-SAT, make a copy of any NAV-SAT
3807
     * data before initializiing it. */
3808
131
    memcpy(skyview_old, session->gpsdata.skyview, sizeof(skyview_old));
3809
3810
#ifdef __UNUSED__    // debug
3811
    GPSD_LOG(LOG_SHOUT, &session->context->errout,
3812
             "UBX: NAV-SIG: gpsd_zero_satellites()\n");
3813
#endif
3814
131
    gpsd_zero_satellites(&session->gpsdata);
3815
3816
781
    for (i = 0; i < nchan; i++) {
3817
        // like NAV-SAT, but 16 bytes instead of 12, no elevation or azimuth
3818
650
        char buf2[80];
3819
650
        int sat_old;
3820
650
        unsigned off = 8 + 16 * i;
3821
650
        short nmea_PRN = 0;
3822
650
        uint8_t gnssId = getub(buf, off + 0);
3823
650
        uint8_t svId = getub(buf, off + 1);
3824
650
        uint8_t sigId = getub(buf, off + 2);
3825
650
        uint8_t freqid = getub(buf, off + 3);
3826
650
        int16_t prRes = getles16(buf, off + 4);     // 0.1 m
3827
650
        uint8_t cno = getub(buf, off + 6);          // dBHz
3828
650
        uint8_t qualityInd = getub(buf, off + 7);   // quality indicator
3829
        // not exactly right?
3830
650
        bool used = (4 <= qualityInd) ? true : false;
3831
650
        uint8_t corrSource = getub(buf, off + 8);   // correlation source
3832
650
        uint8_t ionoModel = getub(buf, off + 9);    // Ionospheric model used:
3833
650
        unsigned sigFlags = getleu16(buf, off + 10);
3834
3835
        // last 4 bytes, reserved
3836
650
        uint32_t reserved = getleu32(buf, 12);
3837
3838
650
        nmea_PRN = ubx2_to_prn(gnssId, svId);
3839
650
        if (0 >= nmea_PRN) {
3840
508
            if (-1 == nmea_PRN) {
3841
                // ignore GLONASS 255
3842
0
                continue;
3843
0
            }
3844
            // bad PRN??
3845
508
            GPSD_LOG(LOG_PROG, &session->context->errout,
3846
508
                     "UBX: NAV-SIG(%d) Bad PRN: gnssid %u, svid %u PRN %d\n",
3847
508
                     seen, gnssId, svId, nmea_PRN);
3848
508
            continue;
3849
508
        }
3850
3851
142
        session->gpsdata.skyview[seen].gnssid = gnssId;
3852
142
        session->gpsdata.skyview[seen].svid = svId;
3853
142
        session->gpsdata.skyview[seen].sigid = sigId;
3854
142
        session->gpsdata.skyview[seen].freqid = freqid;
3855
142
        session->gpsdata.skyview[seen].PRN = nmea_PRN;
3856
142
        session->gpsdata.skyview[seen].prRes = prRes / 10.0;
3857
142
        session->gpsdata.skyview[seen].qualityInd = qualityInd;
3858
3859
142
        session->gpsdata.skyview[seen].ss = (double)cno;
3860
142
        session->gpsdata.skyview[seen].used = used;
3861
        // sbas_in_use is not same as used
3862
142
        if (used) {
3863
89
            used_tot++;
3864
89
        }
3865
        // by some coincidence, our health flags matches u-blox's
3866
142
        session->gpsdata.skyview[seen].health = sigFlags & 3;
3867
        // try to keep elevation and azimuth from NAV-SAT
3868
1.76k
        for (sat_old = 0; sat_old < MAXCHANNELS; sat_old++) {
3869
1.76k
            if (0 >= skyview_old[sat_old].PRN) {
3870
                // end of list, not found
3871
71
                break;
3872
71
            }
3873
1.69k
            if (nmea_PRN != skyview_old[sat_old].PRN) {
3874
                // not this one
3875
1.62k
                continue;
3876
1.62k
            }
3877
            // found it, grab the data
3878
71
            session->gpsdata.skyview[seen].azimuth =
3879
71
                skyview_old[sat_old].azimuth;
3880
71
            session->gpsdata.skyview[seen].elevation =
3881
71
                skyview_old[sat_old].elevation;
3882
71
            break;
3883
1.69k
        }
3884
142
        GPSD_LOG(LOG_PROG, &session->context->errout,
3885
142
                 "UBX: NAV-SIG gnssid %u, svid %u sigid %u PRN %d freqid %u "
3886
142
                 "prRes %d cno %u qual %u corr %u, iono %u flags x%x res x%x "
3887
142
                 "az %.1f el %.1f\n",
3888
142
                 gnssId, svId, sigId, nmea_PRN, freqid, prRes, cno,
3889
142
                 qualityInd, corrSource, ionoModel, sigFlags, reserved,
3890
142
                 session->gpsdata.skyview[seen].azimuth,
3891
142
                 session->gpsdata.skyview[seen].elevation);
3892
142
        GPSD_LOG(LOG_IO, &session->context->errout,
3893
142
                 "UBX: NAV-SIG(%d): gnssId:%s flags:%s quality:%s "
3894
142
                 "courrSource:%s ionoModel:%s\n",
3895
142
                 seen, val2str(gnssId, vgnssId),
3896
142
                 flags2str(sigFlags, fsig_sigFlags, buf2, sizeof(buf2)),
3897
142
                 val2str(qualityInd, vquality),
3898
142
                 val2str(corrSource, vsig_corrsource),
3899
142
                 val2str(ionoModel, vsig_ionomodel));
3900
3901
142
        seen++;
3902
142
    }
3903
3904
131
    session->gpsdata.satellites_visible = seen;
3905
131
    session->gpsdata.satellites_used = used_tot;
3906
131
    GPSD_LOG(LOG_PROG, &session->context->errout,
3907
131
             "UBX: NAV-SIG: time %s visible=%d used=%d"
3908
131
             " mask={SATELLITE|USED}\n",
3909
131
             timespec_str(&session->gpsdata.skyview_time,
3910
131
                          ts_buf, sizeof(ts_buf)),
3911
131
             session->gpsdata.satellites_visible,
3912
131
             session->gpsdata.satellites_used);
3913
131
    return SATELLITE_SET | USED_IS;
3914
336
}
3915
3916
/**
3917
 * Navigation solution message: UBX-NAV-SOL
3918
 *
3919
 * Present in:
3920
 *    protVer 7 ( Antaris)
3921
 *    protVer up to 23,01
3922
 *
3923
 * Deprecated in:
3924
 *    protVer 13 (6-series)
3925
 *
3926
 * Not present in:
3927
 *    protVer 27 (9-series)
3928
 *    Use UBX-NAV-PVT instead
3929
 *
3930
 * UBX-NAV-SOL has ECEF and VECEF, so no need for UBX-NAV-POSECEF and
3931
 * UBX-NAV-VELECEF
3932
 */
3933
static gps_mask_t ubx_msg_nav_sol(struct gps_device_t *session,
3934
                                  unsigned char *buf, size_t data_len)
3935
1.44k
{
3936
1.44k
    char buf2[80];
3937
1.44k
    unsigned flags, pdop;
3938
1.44k
    unsigned gpsFix;
3939
1.44k
    gps_mask_t mask = 0;
3940
1.44k
    char ts_buf[TIMESPEC_LEN];
3941
3942
1.44k
    if (52 > data_len) {
3943
76
        GPSD_LOG(LOG_WARN, &session->context->errout,
3944
76
                 "UBX: NAV-SOL: runt payload len %zd", data_len);
3945
76
        return 0;
3946
76
    }
3947
3948
1.36k
    session->driver.ubx.iTOW = getleu32(buf, 0);
3949
1.36k
    gpsFix = getub(buf, 10);
3950
1.36k
    flags = getub(buf, 11);
3951
1.36k
    mask = 0;
3952
2.73k
#define DATE_VALID      (UBX_SOL_VALID_WEEK | UBX_SOL_VALID_TIME)
3953
1.36k
    if ((flags & DATE_VALID) == DATE_VALID) {
3954
597
        unsigned short week;
3955
597
        timespec_t ts_tow;
3956
3957
597
        MSTOTS(&ts_tow, session->driver.ubx.iTOW);
3958
597
        ts_tow.tv_nsec += (long)getles32(buf, 4);
3959
597
        TS_NORM(&ts_tow);
3960
597
        week = (unsigned short)getles16(buf, 8);
3961
597
        session->newdata.time = gpsd_gpstime_resolv(session, week, ts_tow);
3962
597
        mask |= TIME_SET | NTPTIME_IS | GOODTIME_IS;
3963
597
    }
3964
1.36k
#undef DATE_VALID
3965
3966
1.36k
    session->newdata.ecef.x = getles32(buf, 12) / 100.0;
3967
1.36k
    session->newdata.ecef.y = getles32(buf, 16) / 100.0;
3968
1.36k
    session->newdata.ecef.z = getles32(buf, 20) / 100.0;
3969
1.36k
    session->newdata.ecef.pAcc = getleu32(buf, 24) / 100.0;
3970
1.36k
    session->newdata.ecef.vx = getles32(buf, 28) / 100.0;
3971
1.36k
    session->newdata.ecef.vy = getles32(buf, 32) / 100.0;
3972
1.36k
    session->newdata.ecef.vz = getles32(buf, 36) / 100.0;
3973
1.36k
    session->newdata.ecef.vAcc = getleu32(buf, 40) / 100.0;
3974
1.36k
    mask |= ECEF_SET | VECEF_SET;
3975
3976
1.36k
    session->newdata.eps = (double)(getles32(buf, 40) / 100.0);
3977
1.36k
    mask |= SPEEDERR_SET;
3978
3979
1.36k
    pdop = getleu16(buf, 44);
3980
1.36k
    if (9999 > pdop) {
3981
1.08k
        session->gpsdata.dop.pdop = (double)(pdop / 100.0);
3982
1.08k
        mask |= DOP_SET;
3983
1.08k
    }
3984
1.36k
    session->gpsdata.satellites_used = (int)getub(buf, 47);
3985
3986
1.36k
    switch (gpsFix) {
3987
164
    case UBX_MODE_TMONLY:
3988
        // Surveyed-in, better not have moved
3989
164
        session->newdata.mode = MODE_3D;
3990
164
        session->newdata.status = STATUS_TIME;
3991
164
        break;
3992
118
    case UBX_MODE_3D:
3993
118
        session->newdata.mode = MODE_3D;
3994
118
        session->newdata.status = STATUS_GPS;
3995
118
        break;
3996
116
    case UBX_MODE_2D:
3997
116
        session->newdata.mode = MODE_2D;
3998
116
        session->newdata.status = STATUS_GPS;
3999
116
        break;
4000
53
    case UBX_MODE_DR:           // consider this too as 2D
4001
53
        session->newdata.mode = MODE_2D;
4002
53
        session->newdata.status = STATUS_DR;
4003
53
        break;
4004
164
    case UBX_MODE_GPSDR:        // DR-aided GPS is valid 3D
4005
164
        session->newdata.mode = MODE_3D;
4006
164
        session->newdata.status = STATUS_GNSSDR;
4007
164
        break;
4008
753
    default:
4009
753
        session->newdata.mode = MODE_NO_FIX;
4010
753
        session->newdata.status = STATUS_UNK;
4011
753
        break;
4012
1.36k
    }
4013
4014
1.36k
    if (0 != (flags & UBX_SOL_FLAG_DGPS))
4015
542
        session->newdata.status = STATUS_DGPS;
4016
4017
1.36k
    mask |= MODE_SET | STATUS_SET;
4018
    // older u-blox, cycle ender may be iffy
4019
    // so err o nthe side of over-reporting TPV
4020
1.36k
    mask |= REPORT_IS;
4021
4022
1.36k
    GPSD_LOG(LOG_PROG, &session->context->errout,
4023
1.36k
             "UBX: NAV-SOL: time=%s ecef x:%.2f y:%.2f z:%.2f track=%.2f "
4024
1.36k
             "speed=%.2f mode=%d status=%d used=%d\n",
4025
1.36k
             timespec_str(&session->newdata.time, ts_buf, sizeof(ts_buf)),
4026
1.36k
             session->newdata.ecef.x,
4027
1.36k
             session->newdata.ecef.y,
4028
1.36k
             session->newdata.ecef.z,
4029
1.36k
             session->newdata.track,
4030
1.36k
             session->newdata.speed,
4031
1.36k
             session->newdata.mode,
4032
1.36k
             session->newdata.status,
4033
1.36k
             session->gpsdata.satellites_used);
4034
1.36k
    GPSD_LOG(LOG_IO, &session->context->errout,
4035
1.36k
             "UBX: NAV-SOL-PVT: gpsFix:%s flags:%s\n",
4036
1.36k
             val2str(gpsFix, vpvt_fixType),
4037
1.36k
             flags2str(flags, fhnr_pvt_flags, buf2, sizeof(buf2)));
4038
1.36k
    return mask;
4039
1.36k
}
4040
4041
4042
/**
4043
 * Receiver navigation status
4044
 * UBX-NAV-STATUS Class 1, ID 3
4045
 *
4046
 * Present in Antaris to 9-series
4047
 */
4048
static gps_mask_t
4049
ubx_msg_nav_status(struct gps_device_t *session, unsigned char *buf,
4050
                   size_t data_len)
4051
767
{
4052
767
    uint8_t gpsFix;
4053
767
    uint8_t flags;
4054
767
    uint8_t fixStat;
4055
767
    uint8_t flags2;
4056
767
    uint32_t ttff;
4057
767
    uint32_t msss;
4058
767
    int *status = &session->newdata.status;
4059
767
    int *mode = &session->newdata.mode;
4060
767
    gps_mask_t mask = 0;
4061
4062
767
    if (16 > data_len) {
4063
68
        GPSD_LOG(LOG_WARN, &session->context->errout,
4064
68
                 "UBX: NAV-STATUS: runt payload len %zd", data_len);
4065
68
        return 0;
4066
68
    }
4067
4068
699
    session->driver.ubx.iTOW = getleu32(buf, 0);
4069
699
    gpsFix = getub(buf, 4);
4070
699
    flags = getub(buf, 5);
4071
699
    fixStat = getub(buf, 6);
4072
699
    flags2 = getub(buf, 7);
4073
699
    ttff = getleu32(buf, 8);
4074
699
    msss = getleu32(buf, 12);
4075
4076
    // FIXME: how does this compare with other places ubx sets mode/status?
4077
699
    if (0 == (1 & flags)) {
4078
        // gpsFix not OK
4079
87
        *mode = MODE_NO_FIX;
4080
87
        *status = STATUS_UNK;
4081
612
    } else {
4082
612
        switch (gpsFix) {
4083
67
        case UBX_MODE_TMONLY:
4084
            // 5 - Surveyed-in, so a precise 3D.
4085
67
            *mode = MODE_3D;
4086
67
            *status = STATUS_TIME;
4087
67
            break;
4088
4089
66
        case UBX_MODE_3D:
4090
            // 3
4091
66
            *mode = MODE_3D;
4092
66
            *status = STATUS_GPS;
4093
66
            break;
4094
4095
67
        case UBX_MODE_GPSDR:
4096
            // 4
4097
67
            *mode = MODE_3D;
4098
67
            *status = STATUS_GNSSDR;
4099
67
            break;
4100
4101
279
        case UBX_MODE_2D:
4102
            // 2
4103
279
            *mode = MODE_2D;
4104
279
            if (2 == (2 & fixStat)) {
4105
212
                *status = STATUS_DGPS;
4106
212
            } else {
4107
67
                *status = STATUS_GPS;
4108
67
            }
4109
279
            break;
4110
4111
66
        case UBX_MODE_DR:           // consider this too as 2D
4112
            // 1
4113
66
            *mode = MODE_2D;
4114
66
            *status = STATUS_DR;
4115
66
            break;
4116
4117
67
        case UBX_MODE_NOFIX:
4118
            // 0
4119
67
            FALLTHROUGH
4120
67
        default:
4121
            // > 5, huh??
4122
67
            *mode = MODE_NO_FIX;
4123
67
            *status = STATUS_UNK;
4124
67
            break;
4125
612
        }
4126
612
        if (2 == (2 & fixStat)) {
4127
345
            if (0x40 == (0x40 & flags2)) {
4128
126
                *status = STATUS_RTK_FLT;
4129
219
            } else if (0x80 == (0x80 & flags2)) {
4130
152
                *status = STATUS_RTK_FIX;
4131
152
            }
4132
            // else ??
4133
345
        } else if (1 == (1 & fixStat)) {
4134
67
            *status = STATUS_DGPS;
4135
67
        }
4136
612
    }
4137
699
    mask |= STATUS_SET | MODE_SET;
4138
4139
699
    GPSD_LOG(LOG_PROG, &session->context->errout,
4140
699
         "UBX: NAV-STATUS: iTOW %lld gpsFix %u flags %02x fixStat %02x "
4141
699
         "flags2=%02x ttff=%llu msss=%llu mode=%u status=%u\n",
4142
699
         (long long)session->driver.ubx.iTOW,
4143
699
         gpsFix,
4144
699
         flags,
4145
699
         fixStat,
4146
699
         flags2,
4147
699
         (long long unsigned)ttff,
4148
699
         (long long unsigned)msss,
4149
699
         session->newdata.mode,
4150
699
         session->newdata.status);
4151
699
    return mask;
4152
699
}
4153
4154
/**
4155
 * Survey-in data - UBX-NAV-SVIN
4156
 * Time Sync products only
4157
 */
4158
static gps_mask_t ubx_msg_nav_svin(struct gps_device_t *session,
4159
                                   unsigned char *buf, size_t data_len UNUSED)
4160
34
{
4161
34
    gps_mask_t mask = ONLINE_SET;
4162
34
    unsigned version = getub(buf, 0);
4163
    // 3 reserved bytes
4164
34
    unsigned long iTOW = getleu32(buf, 4);
4165
34
    unsigned long dur = getleu32(buf, 0);
4166
34
    long long meanX = getles32(buf, 12);             // cm
4167
34
    long long meanY = getles32(buf, 16);             // cm
4168
34
    long long meanZ = getles32(buf, 20);             // cm
4169
34
    int meanXHP = getsb(buf, 24);                    // 0.1 mm
4170
34
    int meanYHP = getsb(buf, 25);                    // 0.1 mm
4171
34
    int meanZHP = getsb(buf, 26);                    // 0.1 mm
4172
    // 1 reserved byte
4173
34
    unsigned long meanAcc = getleu32(buf, 28);  // 0.1 mm
4174
34
    unsigned long obs = getleu32(buf, 32);
4175
34
    unsigned valid = getub(buf, 36);
4176
34
    unsigned active = getub(buf, 37);
4177
    // 2 reserved
4178
4179
    // Only version 0 is defined up to ub-blox 9
4180
34
    if (0 != version) {
4181
34
        GPSD_LOG(LOG_WARN, &session->context->errout,
4182
34
                 "UBX: NAV-SVIN: unknown version $u %u", version);
4183
34
        return 0;
4184
34
    }
4185
4186
0
    session->driver.ubx.iTOW = iTOW;
4187
0
    meanX = (meanX * 10) + meanXHP;
4188
0
    meanY = (meanY * 10) + meanYHP;
4189
0
    meanZ = (meanZ * 10) + meanZHP;
4190
4191
    // casts for 32 bit compatibility
4192
0
    GPSD_LOG(LOG_PROG, &session->context->errout,
4193
0
             "UBX: NAV-SVIN: iTOW %llu dur=%lu meanX=%lld meanY=%lld "
4194
0
             "meanZ=%lld meanAcc=%lu "
4195
0
             "obs=%lu valid=%u(%s) active=%u(%s)\n",
4196
0
             (long long)iTOW, dur, meanX, meanY, meanZ,
4197
0
             meanAcc, obs, valid, val2str(valid, vsvin_valid),
4198
0
             active, val2str(active, vsvin_active));
4199
0
    return mask;
4200
34
}
4201
4202
/**
4203
 * GPS Satellite Info -- deprecated - UBX-NAV-SVINFO
4204
 * Present in:
4205
 *    protver < 27
4206
 * Not present in:
4207
      protver >= 27 (9-series), use UBX-NAV-SAT instead
4208
 */
4209
static gps_mask_t ubx_msg_nav_svinfo(struct gps_device_t *session,
4210
                                     unsigned char *buf, size_t data_len)
4211
668
{
4212
668
    char buf2[80];
4213
668
    unsigned i, nchan;
4214
668
    int seen = 0, used_tot = 0;
4215
668
    unsigned chipGen;
4216
668
    unsigned globalFlags;
4217
668
    timespec_t ts_tow;
4218
    // chipGen to protVer, Antaris 4, u-blox 4, 5, 6, 7 and 8
4219
668
    static unsigned gen2ver[] = {8, 10, 12, 13, 15};
4220
668
    char ts_buf[TIMESPEC_LEN];
4221
4222
668
    if (8 > data_len) {
4223
83
        GPSD_LOG(LOG_PROG, &session->context->errout,
4224
83
                 "UBX: NAV-SVINFO runt datalen %zd\n", data_len);
4225
83
        return 0;
4226
83
    }
4227
4228
585
    session->driver.ubx.iTOW = getleu32(buf, 0);
4229
585
    MSTOTS(&ts_tow, session->driver.ubx.iTOW);
4230
585
    session->gpsdata.skyview_time =
4231
585
        gpsd_gpstime_resolv(session, session->context->gps_week, ts_tow);
4232
4233
585
    nchan = getub(buf, 4);
4234
585
    if (nchan > MAXCHANNELS) {
4235
66
        GPSD_LOG(LOG_WARN, &session->context->errout,
4236
66
                 "UBX: NAV SVINFO: too many channels %u\n",
4237
66
                 nchan);
4238
66
        return 0;
4239
66
    }
4240
519
    if ((8 + 12 * nchan) > data_len) {
4241
67
        GPSD_LOG(LOG_WARN, &session->context->errout,
4242
67
                 "UBX: NAV-SVINFO: runt payload %zd channels %u\n",
4243
67
                 data_len, nchan);
4244
67
        return 0;
4245
67
    }
4246
452
    globalFlags = getub(buf, 5);
4247
452
    chipGen = globalFlags & 0x07;
4248
452
    if (ROWS(gen2ver) > chipGen &&
4249
        // put a floor under protVer
4250
384
        gen2ver[chipGen] > session->driver.ubx.protver) {
4251
182
        session->driver.ubx.protver = gen2ver[chipGen];
4252
182
    }
4253
4254
#ifdef __UNUSED__    // debug
4255
    GPSD_LOG(LOG_SHOUT, &session->context->errout,
4256
             "UBX: NAV-SVINFO: gpsd_zero_satellites()\n");
4257
#endif
4258
452
    gpsd_zero_satellites(&session->gpsdata);
4259
4260
68.5k
    for (i = 0; i < nchan; i++) {
4261
68.0k
        unsigned off = 8 + 12 * i;
4262
68.0k
        short nmea_PRN;
4263
68.0k
        unsigned chan = getub(buf, off);
4264
68.0k
        unsigned ubx_PRN = getub(buf, off + 1);
4265
68.0k
        unsigned flags = getub(buf, off + 2);
4266
68.0k
        unsigned quality = getub(buf, off + 3);
4267
68.0k
        unsigned cno = getub(buf, off + 4);
4268
68.0k
        bool used = (bool)(flags & 0x01);
4269
68.0k
        int el = getsb(buf, off + 5);
4270
68.0k
        int az = getles16(buf, off + 6);
4271
68.0k
        int prRes = getles16(buf, off + 7);
4272
4273
68.0k
        nmea_PRN = ubx_to_prn(ubx_PRN,
4274
68.0k
                              &session->gpsdata.skyview[seen].gnssid,
4275
68.0k
                              &session->gpsdata.skyview[seen].svid);
4276
4277
68.0k
        if (1 > nmea_PRN) {
4278
            // skip bad PRN
4279
62.7k
            GPSD_LOG(LOG_PROG, &session->context->errout,
4280
62.7k
                     "UBX: NAV-SVINFO bad NMEA PRN %d\n", nmea_PRN);
4281
62.7k
            continue;
4282
62.7k
        }
4283
5.32k
        session->gpsdata.skyview[seen].PRN = nmea_PRN;
4284
4285
5.32k
        session->gpsdata.skyview[seen].ss = (double)cno;
4286
5.32k
        if (90 >= abs(el)) {
4287
4.26k
            session->gpsdata.skyview[seen].elevation = (double)el;
4288
4.26k
        }
4289
5.32k
        if (360 > az &&
4290
2.76k
            0 <= az) {
4291
620
            session->gpsdata.skyview[seen].azimuth = (double)az;
4292
620
        }
4293
5.32k
        session->gpsdata.skyview[seen].prRes = prRes / 100.0;
4294
5.32k
        session->gpsdata.skyview[seen].qualityInd = quality;
4295
5.32k
        session->gpsdata.skyview[seen].used = used;
4296
        // sbas_in_use is not same as used
4297
5.32k
        if (used) {
4298
            // not really 'used', just integrity data from there
4299
2.33k
            used_tot++;
4300
2.33k
        }
4301
5.32k
        if (0x10 == (0x10 & flags)) {
4302
2.76k
           session->gpsdata.skyview[seen].health = SAT_HEALTH_BAD;
4303
2.76k
        } else {
4304
2.56k
           session->gpsdata.skyview[seen].health = SAT_HEALTH_OK;
4305
2.56k
        }
4306
4307
5.32k
        GPSD_LOG(LOG_PROG, &session->context->errout,
4308
5.32k
                 "UBX: NAV-SVINFO chan %u ubx_prn %d gnssid %d svid %d "
4309
5.32k
                 "nmea_PRN %d flags x%x az %.0f el %.0f cno %.0f prRes %.2f "
4310
5.32k
                 "quality %u\n",
4311
5.32k
                 chan, ubx_PRN,
4312
5.32k
                 session->gpsdata.skyview[seen].gnssid,
4313
5.32k
                 session->gpsdata.skyview[seen].svid, nmea_PRN, flags,
4314
5.32k
                 session->gpsdata.skyview[seen].azimuth,
4315
5.32k
                 session->gpsdata.skyview[seen].elevation,
4316
5.32k
                 session->gpsdata.skyview[seen].ss,
4317
5.32k
                 session->gpsdata.skyview[seen].prRes,
4318
5.32k
                 session->gpsdata.skyview[seen].qualityInd);
4319
5.32k
        GPSD_LOG(LOG_IO, &session->context->errout,
4320
5.32k
                 "UBX: NAV-SVINFO: flags (%s) quality %s\n",
4321
5.32k
                 flags2str(flags, fsvinfo_flags, buf2, sizeof(buf2)),
4322
5.32k
                 val2str(quality, vquality));
4323
4324
5.32k
        seen++;
4325
5.32k
    }
4326
4327
452
    session->gpsdata.satellites_visible = seen;
4328
452
    session->gpsdata.satellites_used = used_tot;
4329
452
    GPSD_LOG(LOG_PROG, &session->context->errout,
4330
452
             "UBX: NAV-SVINFO: time %s visible %d used %d "
4331
452
             " mask {SATELLITE|USED} gFlags x%x\n",
4332
452
             timespec_str(&session->gpsdata.skyview_time,
4333
452
                          ts_buf, sizeof(ts_buf)),
4334
452
             session->gpsdata.satellites_visible,
4335
452
             session->gpsdata.satellites_used,
4336
452
             globalFlags);
4337
452
    GPSD_LOG(LOG_IO, &session->context->errout,
4338
452
             "UBX: NAV-SVINFO: chipGen %s\n",
4339
452
             val2str(chipGen, vglobalFlags));
4340
452
    return SATELLITE_SET | USED_IS;
4341
519
}
4342
4343
/**
4344
 * GPS Leap Seconds - UBX-NAV-TIMEGPS
4345
 * Present in:
4346
 *     protVer 8 (Antaris 4)
4347
 *     protVer 27 (F9P)
4348
 *     protVer 34 (M10)
4349
 *
4350
 * Not in:
4351
 *     protVer 24 (NEO-D9S)
4352
 */
4353
static gps_mask_t ubx_msg_nav_timegps(struct gps_device_t *session,
4354
                                      unsigned char *buf, size_t data_len)
4355
540
{
4356
540
    char buf2[80];
4357
540
    uint8_t valid;         // Validity Flags
4358
540
    gps_mask_t mask = 0;
4359
540
    char ts_buf[TIMESPEC_LEN];
4360
4361
540
    if (16 > data_len) {
4362
66
        GPSD_LOG(LOG_WARN, &session->context->errout,
4363
66
                 "UBX: NAV-TIMEGPS: runt payload len %zd", data_len);
4364
66
        return 0;
4365
66
    }
4366
4367
474
    session->driver.ubx.iTOW = getleu32(buf, 0);
4368
474
    valid = getub(buf, 11);
4369
    // Valid leap seconds ?
4370
474
    if ((valid & UBX_TIMEGPS_VALID_LEAP_SECOND) ==
4371
474
        UBX_TIMEGPS_VALID_LEAP_SECOND) {
4372
67
        session->context->leap_seconds = (int)getub(buf, 10);
4373
67
        session->context->valid |= LEAP_SECOND_VALID;
4374
67
    }
4375
    // Valid GPS time of week and week number
4376
948
#define VALID_TIME (UBX_TIMEGPS_VALID_TIME | UBX_TIMEGPS_VALID_WEEK)
4377
474
    if ((valid & VALID_TIME) == VALID_TIME) {
4378
406
#undef VALID_TIME
4379
406
        uint16_t week;
4380
406
        double tAcc;      // Time Accuracy Estimate in ns
4381
406
        timespec_t ts_tow;
4382
4383
406
        week = getles16(buf, 8);
4384
406
        MSTOTS(&ts_tow, session->driver.ubx.iTOW);
4385
406
        ts_tow.tv_nsec += (long)getles32(buf, 4);
4386
406
        TS_NORM(&ts_tow);
4387
406
        session->newdata.time = gpsd_gpstime_resolv(session, week, ts_tow);
4388
4389
406
        tAcc = (double)getleu32(buf, 12);     // tAcc in ns
4390
406
        session->newdata.ept = tAcc / 1e9;
4391
406
        mask |= (TIME_SET | NTPTIME_IS);
4392
406
    }
4393
4394
474
    GPSD_LOG(LOG_PROG, &session->context->errout,
4395
474
             "UBX: NAV-TIMEGPS: time=%s mask={TIME} valid x%x\n",
4396
474
             timespec_str(&session->newdata.time, ts_buf, sizeof(ts_buf)),
4397
474
             valid);
4398
474
    GPSD_LOG(LOG_IO, &session->context->errout,
4399
474
             "UBX: NAV-TIMEGPS: valid %s\n",
4400
474
             flags2str(valid, vtimegps_valid, buf2, sizeof(buf2)));
4401
474
    return mask;
4402
540
}
4403
4404
/**
4405
 * Navigation time to leap second: UBX-NAV-TIMELS
4406
 *
4407
 * Sets leap_notify if leap second is < 23 hours away.
4408
 * Present in:
4409
 *     protVer 15 (8-series)
4410
 * Not in:
4411
 *     protVer 12 (5-series)
4412
 *     protVer 13 (6-series)
4413
 *     protVer 14 (6-series / GLONASS, 6-series)
4414
 */
4415
static gps_mask_t ubx_msg_nav_timels(struct gps_device_t *session,
4416
                                     unsigned char *buf, size_t data_len)
4417
466
{
4418
466
    char buf2[80];
4419
466
    unsigned version;
4420
466
    unsigned valid;
4421
466
    int valid_curr_ls;
4422
466
    int valid_time_to_ls_event;
4423
4424
466
#define UBX_TIMELS_VALID_CURR_LS 0x01
4425
466
#define UBX_TIMELS_VALID_TIME_LS_EVT 0x01
4426
4427
466
    if (24 > data_len) {
4428
66
        GPSD_LOG(LOG_WARN, &session->context->errout,
4429
66
                 "UBX: NAV-TIMELS: runt %zd, expecting 24\n",
4430
66
                 data_len);
4431
66
        return 0;
4432
66
    }
4433
4434
400
    session->driver.ubx.iTOW = getleu32(buf, 0);
4435
400
    version = getub(buf, 4);
4436
    // Only version 0 is defined up to ub-blox 9
4437
400
    if (0 != version) {
4438
66
        GPSD_LOG(LOG_WARN, &session->context->errout,
4439
66
                 "UBX: NAV-TIMELS: unknown version $u %u", version);
4440
66
        return 0;
4441
66
    }
4442
334
    valid = getub(buf, 23);
4443
334
    GPSD_LOG(LOG_PROG, &session->context->errout,
4444
334
             "UBX: NAV-TIMELS: valid x%x version %d\n", valid, version);
4445
334
    GPSD_LOG(LOG_INF, &session->context->errout,
4446
334
             "UBX: NAV-TIMELS: valid %s\n",
4447
334
             flags2str(valid, vtimels_valid, buf2, sizeof(buf2)));
4448
4449
334
    valid_curr_ls = valid & UBX_TIMELS_VALID_CURR_LS;
4450
334
    valid_time_to_ls_event = valid & UBX_TIMELS_VALID_TIME_LS_EVT;
4451
334
    if (valid_curr_ls) {
4452
268
        unsigned int src_of_curr_ls = getub(buf,8);
4453
268
        int curr_ls = getsb(buf,9);
4454
4455
268
        GPSD_LOG(LOG_PROG, &session->context->errout,
4456
268
                 "UBX: NAV-TIMELS: srcOfCurrLs %u(%s) curr_ls %d\n",
4457
268
                 src_of_curr_ls,
4458
268
                 val2str(src_of_curr_ls, vsrcOfCurrLs),
4459
268
                 curr_ls);
4460
268
        session->context->leap_seconds = curr_ls;
4461
268
        session->context->valid |= LEAP_SECOND_VALID;
4462
268
    }  // Valid current leap second
4463
4464
334
    if (valid_time_to_ls_event) {
4465
268
        unsigned int src_of_ls_change;
4466
268
        unsigned short dateOfLSGpsWn, dateOfLSGpsDn;
4467
268
        int lsChange = getsb(buf, 11);
4468
268
        int timeToLsEvent = getles32(buf, 12);
4469
4470
268
        src_of_ls_change = getub(buf,10);
4471
4472
268
        dateOfLSGpsWn = getles16(buf,16);
4473
268
        dateOfLSGpsDn = getles16(buf,18);
4474
268
        GPSD_LOG(LOG_PROG, &session->context->errout,
4475
268
                 "UBX: NAV-TIMELS: srcOfCurLsChange %u(%s) lsChange %d "
4476
268
                 "timeToLsEvent %d dateOfLSGpsWn %d dateOfLSGpsDn %d\n",
4477
268
                 src_of_ls_change,
4478
268
                 val2str(src_of_ls_change, vsrcOfLsChange),
4479
268
                 lsChange, timeToLsEvent,
4480
268
                 dateOfLSGpsWn,dateOfLSGpsDn);
4481
4482
268
        if ((0 != lsChange) && (0 < timeToLsEvent) &&
4483
134
            ((60 * 60 * 23) > timeToLsEvent)) {
4484
66
            if (1 == lsChange) {
4485
0
                session->context->leap_notify = LEAP_ADDSECOND;
4486
0
                GPSD_LOG(LOG_WARN, &session->context->errout,
4487
0
                         "UBX: NAV-TIMELS: leap_notify %d "
4488
0
                         "Positive leap second today\n",
4489
0
                         session->context->leap_notify);
4490
66
            } else if (-1 == lsChange) {
4491
0
                session->context->leap_notify = LEAP_DELSECOND;
4492
0
                GPSD_LOG(LOG_WARN, &session->context->errout,
4493
0
                         "UBX: NAV-TIMELS:leap_notify %d "
4494
0
                         " Negative leap second today\n",
4495
0
                         session->context->leap_notify);
4496
0
            }
4497
202
        } else {
4498
202
            session->context->leap_notify = LEAP_NOWARNING;
4499
202
            GPSD_LOG(LOG_PROG, &session->context->errout,
4500
202
                     "UBX: NAV-TIMELS: leap_notify %d, none today\n",
4501
202
                     session->context->leap_notify);
4502
202
        }
4503
268
    }
4504
334
    return 0;
4505
400
}
4506
4507
/**
4508
 * UBX-NAV-TIMEUTC
4509
 */
4510
static gps_mask_t ubx_msg_nav_timeutc(struct gps_device_t *session,
4511
                                      unsigned char *buf, size_t data_len)
4512
455
{
4513
455
    uint8_t valid;         // Validity Flags
4514
455
    gps_mask_t mask = 0;
4515
4516
455
    if (20 > data_len) {
4517
70
        GPSD_LOG(LOG_WARN, &session->context->errout,
4518
70
                 "UBX: NAV-TIMEUTC: runt payload len %zd", data_len);
4519
70
        return 0;
4520
70
    }
4521
4522
385
    session->driver.ubx.iTOW = getleu32(buf, 0);
4523
385
    valid = getub(buf, 19);
4524
385
    if (4 == (4 & valid)) {
4525
        // UTC is valid
4526
351
        struct tm date = {0};
4527
        // mask |= (TIME_SET | NTPTIME_IS);
4528
351
        uint32_t tAcc = getleu32(buf, 4);          // tAcc in ns
4529
        // nano can be negative, so this is not normalized UTC.
4530
351
        int32_t nano = getles32(buf, 8);           // fract sec in ns
4531
351
        date.tm_year = getleu16(buf, 12) - 1900;   // year, 1999..2099
4532
351
        date.tm_mon = getub(buf, 14) - 1;          // month 1..12
4533
351
        date.tm_mday = getub(buf, 15);             // day 1..31
4534
351
        date.tm_hour = getub(buf, 16);             // hour 0..23
4535
351
        date.tm_min = getub(buf, 17);              // min 0..59
4536
351
        date.tm_sec = getub(buf, 18);              // sec 0..60
4537
351
        session->newdata.time.tv_sec = mkgmtime(&date);
4538
351
        session->newdata.time.tv_nsec = nano;
4539
        // nano, can be negative! So normalize
4540
351
        TS_NORM(&session->newdata.time);
4541
        // other timestamped messages lack nano, so time will jump around...
4542
351
        mask |= TIME_SET | NTPTIME_IS | GOODTIME_IS;
4543
4544
351
        GPSD_LOG(LOG_PROG, &session->context->errout,
4545
351
                 "UBX: NAV-TIMEUTC: iTOW=%lld valid=%02x %04d-%02d-%02d "
4546
351
                 "%02d:%02d:%02d.%09d tAcc=%llu time %lld.%09lld\n",
4547
351
                 (long long)session->driver.ubx.iTOW,
4548
351
                 valid, date.tm_year + 1900, date.tm_mon + 1, date.tm_mday,
4549
351
                 date.tm_hour, date.tm_min, date.tm_sec, nano,
4550
351
                 (long long unsigned)tAcc,
4551
351
                 (long long)session->newdata.time.tv_sec,
4552
351
                 (long long)session->newdata.time.tv_nsec);
4553
351
    } else {
4554
34
        GPSD_LOG(LOG_PROG, &session->context->errout,
4555
34
                 "UBX: NAV-TIMEUTC: iTOW=%lld valid=%02x\n",
4556
34
                 (long long)session->driver.ubx.iTOW,
4557
34
                 valid);
4558
34
    }
4559
385
    return mask;
4560
455
}
4561
4562
/*
4563
 * Velocity Position ECEF message, UBX-NAV-VELECEF
4564
 */
4565
static gps_mask_t ubx_msg_nav_velecef(struct gps_device_t *session,
4566
                                      unsigned char *buf, size_t data_len)
4567
134
{
4568
134
    gps_mask_t mask = VECEF_SET;
4569
4570
134
    if (20 > data_len) {
4571
66
        GPSD_LOG(LOG_WARN, &session->context->errout,
4572
66
                 "UBX: NAV-VELECEF: runt payload len %zd", data_len);
4573
66
        return 0;
4574
66
    }
4575
4576
68
    session->driver.ubx.iTOW = getleu32(buf, 0);
4577
68
    session->newdata.ecef.vx = getles32(buf, 4) / 100.0;
4578
68
    session->newdata.ecef.vy = getles32(buf, 8) / 100.0;
4579
68
    session->newdata.ecef.vz = getles32(buf, 12) / 100.0;
4580
68
    session->newdata.ecef.vAcc = getleu32(buf, 16) / 100.0;
4581
68
    GPSD_LOG(LOG_PROG, &session->context->errout,
4582
68
        "UBX: NAV-VELECEF: iTOW=%lld ECEF vx=%.2f vy=%.2f vz=%.2f vAcc=%.2f\n",
4583
68
        (long long)session->driver.ubx.iTOW,
4584
68
        session->newdata.ecef.vx,
4585
68
        session->newdata.ecef.vy,
4586
68
        session->newdata.ecef.vz,
4587
68
        session->newdata.ecef.vAcc);
4588
68
    return mask;
4589
134
}
4590
4591
/*
4592
 * Velocity NED message, UBX-NAV-VELNED
4593
 * protocol versions 15+
4594
 */
4595
static gps_mask_t ubx_msg_nav_velned(struct gps_device_t *session,
4596
                                     unsigned char *buf, size_t data_len)
4597
173
{
4598
173
    gps_mask_t mask = VNED_SET;
4599
4600
173
    if (36 > data_len) {
4601
105
        GPSD_LOG(LOG_WARN, &session->context->errout,
4602
105
                 "UBX: NAV-VELNED: runt payload len %zd", data_len);
4603
105
        return 0;
4604
105
    }
4605
4606
68
    session->driver.ubx.iTOW = getleu32(buf, 0);
4607
68
    session->newdata.NED.velN = getles32(buf, 4) / 100.0;
4608
68
    session->newdata.NED.velE = getles32(buf, 8) / 100.0;
4609
68
    session->newdata.NED.velD = getles32(buf, 12) / 100.0;
4610
    // ignore speed for now
4611
68
    GPSD_LOG(LOG_PROG, &session->context->errout,
4612
68
        "UBX: NAV-VELNED: iTOW=%lld NED velN=%.2f velE=%.2f velD=%.2f\n",
4613
68
         (long long)session->driver.ubx.iTOW,
4614
68
        session->newdata.NED.velN,
4615
68
        session->newdata.NED.velE,
4616
68
        session->newdata.NED.velD);
4617
68
    return mask;
4618
173
}
4619
4620
// UBX-MON-COMMS txErrors
4621
static const struct flist_t rxm_cor_statusInfo[] = {
4622
    {0, 0x1f, "UNk proto,"},
4623
    {1, 0x1f, "RTCM3,"},
4624
    {2, 0x1f, "SPARTN,"},
4625
    {29, 0x1f, "RXM-PMP,"},
4626
    {30, 0x1f, "RXM-QZSSL6,"},
4627
    {0, 0x60, "Err Unk,"},
4628
    {0x20, 0x60, "No Err,"},
4629
    {0x40, 0x60, "Error,"},
4630
    {0x80, 0x180, "Unused,"},
4631
    {0x100, 0x180, "Used,"},
4632
    // bits 9 to 24, correction Id
4633
    {0x200000, 0x200000, "msgTypeValid"},
4634
};
4635
4636
static const struct vlist_t rxm_spart_flags[] = {
4637
    {0, "Unknown"},
4638
    {1, "Not Used"},
4639
    {2, "Used"},
4640
    {3, "Reserved"},
4641
};
4642
4643
static const struct vlist_t spartn_mtypes[] = {
4644
    {0, "Orbit"},
4645
    {1, "HPAC"},
4646
    {2, "GAD"},
4647
    {3, "BDS"},
4648
};
4649
4650
static const struct vlist_t spartn_mstypes[] = {
4651
    {0, "GPS"},
4652
    {1, "GLO"},
4653
    {2, "GAL"},
4654
    {3, "BDS"},
4655
};
4656
4657
/**
4658
 * UBX-RXM-COR -- Differential Correction Input Messages
4659
 *
4660
 * Present in ZED-F9P, HPG 1.50, protVer 27.50
4661
 */
4662
static gps_mask_t ubx_msg_rxm_cor(struct gps_device_t *session,
4663
                                  unsigned char *buf, size_t data_len UNUSED)
4664
0
{
4665
0
    unsigned version = getub(buf, 0);
4666
0
    unsigned ebno = getub(buf, 1);
4667
0
    unsigned long statusInfo = getleu32(buf, 4);
4668
0
    unsigned msgType = getleu16(buf, 8);
4669
0
    unsigned msgsubType = getleu16(buf, 10);
4670
0
    char buf2[80];
4671
4672
0
    if (1 != version) {
4673
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
4674
0
                 "UBX: RXM-COR, unknown version %u\n", version);
4675
0
        return 0;
4676
0
    }
4677
0
    GPSD_LOG(LOG_PROG, &session->context->errout,
4678
0
             "UBX: RXM-COR, version %u ebno %u statusInfo x%lx "
4679
0
             "msgtype %u msgsubType %u\n", version, ebno, statusInfo,
4680
0
             msgType, msgsubType);
4681
0
    GPSD_LOG(LOG_IO, &session->context->errout,
4682
0
            "UBX: RXM-COR: statusInfo (%s)  msgType (%s) msgsubType (%s)\n",
4683
0
            flags2str(statusInfo, rxm_cor_statusInfo, buf2, sizeof(buf2)),
4684
0
            val2str(msgType, spartn_mtypes),
4685
0
            val2str(msgsubType, spartn_mstypes));
4686
0
    return 0;
4687
0
}
4688
4689
/*
4690
 * Multi-GNSS Raw measurement Data -- UBX-RXM-RAWX
4691
 * Not in u-blox 5, 6 or 7
4692
 * u-blox 9, message version 0 (but no version byte!)
4693
 * u-blox 9, message version 1
4694
 */
4695
static gps_mask_t ubx_msg_rxm_rawx(struct gps_device_t *session,
4696
                                   const unsigned char *buf,
4697
                                   size_t data_len)
4698
539
{
4699
539
    double rcvTow;
4700
539
    uint16_t week;
4701
539
    int8_t leapS;
4702
539
    unsigned numMeas;
4703
539
    uint8_t recStat;
4704
539
    uint8_t version;
4705
539
    unsigned i;
4706
539
    const char * obs_code;
4707
539
    timespec_t ts_tow;
4708
4709
539
    if (16 > data_len) {
4710
66
        GPSD_LOG(LOG_WARN, &session->context->errout,
4711
66
                 "UBX: RXM-RAWX: runt payload len %zd", data_len);
4712
66
        return 0;
4713
66
    }
4714
4715
    // zero the raw data area.
4716
473
    memset((void *)&session->gpsdata.raw, 0, sizeof(session->gpsdata.raw));
4717
4718
    // Note: this is "approximately" GPS TOW, this is not iTOW
4719
473
    rcvTow = getled64((const char *)buf, 0);   // time of week in seconds
4720
473
    week = getleu16(buf, 8);
4721
473
    leapS = getsb(buf, 10);
4722
473
    numMeas = getub(buf, 11);
4723
473
    recStat = getub(buf, 12);
4724
    /* byte 13 is version on u-blox 9, reserved on u-blox 8
4725
     * how is that supposed to work?? */
4726
473
    version = getub(buf, 13);
4727
4728
473
    GPSD_LOG(LOG_PROG, &session->context->errout,
4729
473
             "UBX: RXM-RAWX: rcvTow %f week %u leapS %d numMeas %u recStat %d"
4730
473
             " version %u\n",
4731
473
             rcvTow, week, leapS, numMeas, recStat, version);
4732
4733
473
    if (recStat & 1) {
4734
        // Valid leap seconds
4735
95
        session->context->leap_seconds = leapS;
4736
95
        session->context->valid |= LEAP_SECOND_VALID;
4737
95
    }
4738
    // RINEX 3 wants GPS time, not UTC time, do not add leap seconds.
4739
473
    DTOTS(&ts_tow, rcvTow);
4740
    // Do not set newdata.time.  set gpsdata.raw.mtime
4741
    // RINEX 3 "GPS time", not UTC, no leap seconds
4742
473
    session->gpsdata.raw.mtime = gpsd_gpstime(session, week, ts_tow);
4743
4744
473
    if (numMeas > MAXCHANNELS) {
4745
66
        GPSD_LOG(LOG_WARN, &session->context->errout,
4746
66
                 "UBX: RXM-RAWX: too many measurements (%u)",
4747
66
                 numMeas);
4748
66
        return 0;
4749
66
    }
4750
407
    if ((16 + 32 * numMeas) > data_len) {
4751
69
        GPSD_LOG(LOG_WARN, &session->context->errout,
4752
69
                 "UBX: RXM-RAWX: runt payload %zu numMeas %u\n",
4753
69
                 data_len, numMeas);
4754
69
        return 0;
4755
69
    }
4756
4757
16.0k
    for (i = 0; i < numMeas; i++) {
4758
15.6k
        int off = 32 * i;
4759
        // pseudorange in meters
4760
15.6k
        double prMes = getled64((const char *)buf, off + 16);
4761
        // carrier phase in cycles
4762
15.6k
        double cpMes = getled64((const char *)buf, off + 24);
4763
        // doppler in Hz, positive towards sat
4764
15.6k
        double doMes = getlef32((const char *)buf, off + 32);
4765
15.6k
        uint8_t gnssId = getub(buf, off + 36);
4766
15.6k
        uint8_t svId = getub(buf, off + 37);
4767
        // reserved in u-blox 8, sigId in u-blox 9 (version 1)
4768
15.6k
        uint8_t sigId = getub(buf, off + 38);
4769
        // GLONASS frequency slot
4770
15.6k
        uint8_t freqId = getub(buf, off + 39);
4771
        // carrier phase locktime in ms, max 64500ms
4772
15.6k
        uint16_t locktime = getleu16(buf, off + 40);
4773
        // carrier-to-noise density ratio dB-Hz
4774
15.6k
        uint8_t cno = getub(buf, off + 42);
4775
15.6k
        uint8_t prStdev = getub(buf, off + 43) & 0x0f;
4776
15.6k
        uint8_t cpStdev = getub(buf, off + 44) & 0x0f;
4777
15.6k
        uint8_t doStdev = getub(buf, off + 45) & 0x0f;
4778
        /* tracking stat
4779
         * bit 0 - prMes valid
4780
         * bit 1 - cpMes valid
4781
         * bit 2 - halfCycle valid
4782
         * bit 3 - halfCycle subtracted from phase
4783
         */
4784
15.6k
        uint8_t trkStat = getub(buf, off + 46);
4785
15.6k
        GPSD_LOG(LOG_PROG, &session->context->errout,
4786
15.6k
                 "UBX: RXM-RAWX: %u:%u:%u freqId %u prMes %f cpMes %f "
4787
15.6k
                 "doMes %f locktime %u\n"
4788
15.6k
                 "cno %u prStdev %u cpStdev %u doStdev %u rtkStat %u\n",
4789
15.6k
                 gnssId, svId, sigId, freqId, prMes, cpMes, doMes, locktime,
4790
15.6k
                 cno, prStdev, cpStdev, doStdev, trkStat);
4791
4792
15.6k
        session->gpsdata.raw.meas[i].gnssid = gnssId;
4793
15.6k
        session->gpsdata.raw.meas[i].sigid = sigId;
4794
4795
        /* some of these are GUESSES as the u-blox codes do not
4796
         * match RINEX codes */
4797
15.6k
        obs_code = sigid2obs(gnssId, sigId);
4798
4799
15.6k
        (void)strlcpy(session->gpsdata.raw.meas[i].obs_code, obs_code,
4800
15.6k
                      sizeof(session->gpsdata.raw.meas[i].obs_code));
4801
4802
15.6k
        session->gpsdata.raw.meas[i].svid = svId;
4803
15.6k
        session->gpsdata.raw.meas[i].freqid = freqId;
4804
15.6k
        session->gpsdata.raw.meas[i].snr = cno;
4805
15.6k
        session->gpsdata.raw.meas[i].satstat = trkStat;
4806
15.6k
        if (trkStat & 1) {
4807
            // prMes valid
4808
794
            session->gpsdata.raw.meas[i].pseudorange = prMes;
4809
14.8k
        } else {
4810
14.8k
            session->gpsdata.raw.meas[i].pseudorange = NAN;
4811
14.8k
        }
4812
15.6k
        if ((trkStat & 2) && (5 >= cpStdev)) {
4813
            // cpMes valid, RTKLIB uses 5 < cpStdev
4814
544
            session->gpsdata.raw.meas[i].carrierphase = cpMes;
4815
15.1k
        } else {
4816
15.1k
            session->gpsdata.raw.meas[i].carrierphase = NAN;
4817
15.1k
        }
4818
15.6k
        session->gpsdata.raw.meas[i].doppler = doMes;
4819
15.6k
        session->gpsdata.raw.meas[i].codephase = NAN;
4820
15.6k
        session->gpsdata.raw.meas[i].deltarange = NAN;
4821
15.6k
        session->gpsdata.raw.meas[i].locktime = locktime;
4822
15.6k
        if (0 == locktime) {
4823
            // possible slip
4824
13.9k
            session->gpsdata.raw.meas[i].lli = 2;
4825
13.9k
        }
4826
15.6k
    }
4827
4828
338
    return RAW_IS;
4829
407
}
4830
4831
/*
4832
 * Raw Subframes - UBX-RXM-SFRB
4833
 * In u-blox 7, only in raw firmware option
4834
 * Not in u-blox 8 or 9
4835
 */
4836
static gps_mask_t ubx_msg_rxm_sfrb(struct gps_device_t *session,
4837
                                   unsigned char *buf, size_t data_len)
4838
137
{
4839
137
    unsigned int i, chan, svid;
4840
137
    uint32_t words[10];
4841
4842
137
    if (42 > data_len) {
4843
70
        GPSD_LOG(LOG_WARN, &session->context->errout,
4844
70
                 "UBX: RXM-SFRB: runt payload len %zd", data_len);
4845
70
        return 0;
4846
70
    }
4847
4848
67
    chan = (unsigned int)getub(buf, 0);
4849
67
    svid = (unsigned int)getub(buf, 1);
4850
67
    GPSD_LOG(LOG_PROG, &session->context->errout,
4851
67
             "UBX: RXM-SFRB: %u %u\n", chan, svid);
4852
4853
    // UBX does all the parity checking, but still bad data gets through
4854
737
    for (i = 0; i < 10; i++) {
4855
        // bits 24 to 31 undefined, remove them.
4856
670
        words[i] = (uint32_t)getleu32(buf, 4 * i + 2) & 0x00ffffff;
4857
670
    }
4858
4859
    // probably GPS, could be SBAS
4860
67
    return gpsd_interpret_subframe(session, GNSSID_GPS, svid, words);
4861
137
}
4862
4863
/*
4864
 * Raw Subframes - UBX-RXM-SFRBX
4865
 * in u-blox 8, protver 17 and up, time sync firmware only
4866
 * in u-blox F9P and HPG only
4867
 * in u-blox F10N, protVer 27 and up
4868
 * not present  before u-blox8
4869
 */
4870
static gps_mask_t ubx_msg_rxm_sfrbx(struct gps_device_t *session,
4871
                                    unsigned char *buf, size_t data_len)
4872
314
{
4873
314
    unsigned i;
4874
314
    uint32_t words[33];
4875
314
    char *chn_s;
4876
314
    gps_mask_t mask = 0;
4877
4878
314
    gnssid_t gnssId = getub(buf, 0);
4879
314
    unsigned svId = getub(buf, 1);
4880
    // reserved in Version 1, and some Version2.  Valid in protVer 27.31 and up
4881
314
    unsigned sigId = getub(buf, 2);
4882
314
    unsigned freqId = getub(buf, 3);
4883
314
    unsigned numWords = getub(buf, 4);
4884
314
    unsigned chn = getub(buf, 5);
4885
314
    unsigned version = getub(buf, 6);
4886
4887
314
    if (1 < version) {
4888
        // receiver channel in version 2 and up.
4889
        // valid range 0 to 13?
4890
161
        chn_s = "chn";
4891
161
    } else {
4892
153
        chn_s = "reserved";
4893
153
    }
4894
4895
314
    GPSD_LOG(LOG_PROG, &session->context->errout,
4896
314
             "UBX: RXM-SFRBX: version %u gnssId %u %s %u svId %u "
4897
314
             "sigId %u freqId %u words %u\n",
4898
314
             version, gnssId, chn_s, chn, svId, sigId, freqId, numWords);
4899
314
    GPSD_LOG(LOG_IO, &session->context->errout,
4900
314
             "UBX: RXM-SFRBX:   %s\n",
4901
314
             val2str((gnssId << 8) | sigId, vgnss_sig_ids));
4902
4903
314
    if (!IN(1, version, 2)) {
4904
        // unknown ersion
4905
134
        GPSD_LOG(LOG_WARN, &session->context->errout,
4906
134
                 "UBX: RXM-SFRBX: unknown version %u", version);
4907
134
        return 0;
4908
134
    }
4909
180
    if (data_len != (size_t)(8 + (4 * numWords)) ||
4910
113
        ROWS(words) < numWords) {
4911
        // test numwords directly to shut up Coverity
4912
113
        GPSD_LOG(LOG_WARN, &session->context->errout,
4913
113
                 "UBX: RXM-SFRBX: wrong payload len %zd, numwords %u "
4914
113
                 "s/b %u",
4915
113
                 data_len, 8 + (4 * numWords), numWords);
4916
113
        return 0;
4917
113
    }
4918
4919
67
    memset(words, 0, sizeof(words));
4920
201
    for (i = 0; i < numWords; i++) {
4921
        // grab the words, don't mangle them
4922
134
        words[i] = (uint32_t)getleu32(buf, 4 * i + 8);
4923
134
    }
4924
4925
    // do we need freqId or chn?
4926
67
    mask = gpsd_interpret_subframe_raw(session, gnssId, sigId,
4927
67
                                       svId, words, numWords);
4928
67
    GPSD_LOG(LOG_IO, &session->context->errout,
4929
67
       "UBX: RXM-SFRBX: mask %s\n",
4930
67
       gps_maskdump(mask));
4931
67
    return mask;
4932
180
}
4933
4934
/**
4935
 * UBX-RXM-SPARTN -- Differential Correction Input Messages
4936
 *
4937
 * Present in ZED-F9P, HPG 1.50, protVer 27.50
4938
 */
4939
static gps_mask_t ubx_msg_rxm_spartn(struct gps_device_t *session,
4940
                                     unsigned char *buf,
4941
                                     size_t data_len UNUSED)
4942
10
{
4943
10
    unsigned version = getub(buf, 0);
4944
10
    unsigned flags = getub(buf, 1);
4945
10
    unsigned msgsubType = getleu16(buf, 2);
4946
10
    unsigned msgType = getleu16(buf, 6);
4947
4948
10
    if (1 != version) {
4949
10
        GPSD_LOG(LOG_WARN, &session->context->errout,
4950
10
                 "UBX: RXM-SPARTN, unknown version %u\n", version);
4951
10
        return 0;
4952
10
    }
4953
0
    GPSD_LOG(LOG_PROG, &session->context->errout,
4954
0
             "UBX: RXM-SPARTN: version %u flags x%x msgType %u msgsubType %u\n",
4955
0
             version, flags, msgType, msgsubType);
4956
0
    GPSD_LOG(LOG_IO, &session->context->errout,
4957
0
            "UBX: RXM-SPARTN: flags (%s) msgType (%s) msgsubType (%s)\n",
4958
0
            val2str((flags >> 1) & 3, rxm_spart_flags),
4959
0
            val2str(msgType, spartn_mtypes),
4960
0
            val2str(msgsubType, spartn_mstypes));
4961
0
    return 0;
4962
10
}
4963
4964
/**
4965
 * SV Status Info
4966
 *
4967
 * May be good cycle ender
4968
 *
4969
 * Present in u-blox 7
4970
 */
4971
static gps_mask_t ubx_msg_rxm_svsi(struct gps_device_t *session,
4972
                                   unsigned char *buf, size_t data_len)
4973
70
{
4974
70
    unsigned numVis, numSV;
4975
4976
70
    if (8 > data_len) {
4977
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
4978
0
                 "UBX: RXM-SVSI: runt payload len %zd", data_len);
4979
0
        return 0;
4980
0
    }
4981
4982
70
    session->driver.ubx.iTOW = getleu32(buf, 0);
4983
70
    session->context->gps_week = getleu16(buf, 4);
4984
70
    numVis = getub(buf, 6);
4985
70
    numSV = getub(buf, 7);
4986
70
    GPSD_LOG(LOG_PROG, &session->context->errout,
4987
70
             "UBX: RXM-SVSI: iTOW=%lld week %d numVis %u numSV %u\n",
4988
70
             (long long)session->driver.ubx.iTOW,
4989
70
            session->context->gps_week, numVis, numSV);
4990
70
    return 0;
4991
70
}
4992
4993
/**
4994
 * Unique chip ID
4995
 * UBX-SEC-UNIQID
4996
 *
4997
 * grab the 5 byte serial number / chip id
4998
 */
4999
static gps_mask_t ubx_msg_sec_uniqid(struct gps_device_t *session,
5000
                                     unsigned char *buf,
5001
                                     size_t data_len)
5002
0
{
5003
0
    unsigned version;
5004
5005
0
    if (9 > data_len) {
5006
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
5007
0
                 "UBX: SEC-UNIQID: runt payload len %zd\n", data_len);
5008
0
        return 0;
5009
0
    }
5010
5011
0
    version = getub(buf, 0);
5012
0
    switch (version) {
5013
0
    case 1:
5014
        /* string of length 10 bytes
5015
         * PROTVER 18 -> 23 has five bytes of unique id.
5016
         * F10 is PROTVER 34, still has 5 bytes */
5017
0
        (void)snprintf(session->gpsdata.dev.sernum,
5018
0
                       sizeof(session->gpsdata.dev.sernum),
5019
0
                       "%02x%02x%02x%02x%02x",
5020
0
                       getub(buf, 4),
5021
0
                       getub(buf, 5),
5022
0
                       getub(buf, 6),
5023
0
                       getub(buf, 7),
5024
0
                       getub(buf, 8));
5025
0
        break;
5026
0
    case 2:
5027
        /* string of length 12 bytes
5028
         * some PROTVER 34 and beyond (for now) have six bytes of unique id.
5029
         * Such as MAX-M10S. */
5030
0
        (void)snprintf(session->gpsdata.dev.sernum,
5031
0
                       sizeof(session->gpsdata.dev.sernum),
5032
0
                       "%02x%02x%02x%02x%02x%02x",
5033
0
                       getub(buf, 4),
5034
0
                       getub(buf, 5),
5035
0
                       getub(buf, 6),
5036
0
                       getub(buf, 7),
5037
0
                       getub(buf, 8),
5038
0
                       getub(buf, 9));
5039
0
        break;
5040
0
    default:
5041
        // unknown version
5042
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
5043
0
                 "UBX: SEC-UNIQID bad version\n");
5044
0
        return 0;
5045
0
    }
5046
5047
    // output chip id at LOG_INF
5048
0
    GPSD_LOG(LOG_INF, &session->context->errout,
5049
0
             "UBX: SEC-UNIQID: %s\n",
5050
0
             session->gpsdata.dev.sernum);
5051
0
    return 0;
5052
0
}
5053
5054
/**
5055
 * Survey-in data - UBX-TIM-SVIN
5056
 * Time Sync products only
5057
 */
5058
static gps_mask_t ubx_msg_tim_svin(struct gps_device_t *session,
5059
                                   unsigned char *buf, size_t data_len UNUSED)
5060
1
{
5061
1
    gps_mask_t mask = ONLINE_SET;
5062
1
    unsigned long dur = getleu32(buf, 0);
5063
1
    long meanX = getles32(buf, 4);
5064
1
    long meanY = getles32(buf, 8);
5065
1
    long meanZ = getles32(buf, 12);
5066
1
    unsigned long meanV = getleu32(buf, 16);
5067
1
    unsigned long obs = getleu32(buf, 20);
5068
1
    unsigned valid = getub(buf, 24);
5069
1
    unsigned active = getub(buf, 25);
5070
    // two reserved bytes
5071
5072
    // casts for 32 bit compatibility
5073
1
    GPSD_LOG(LOG_PROG, &session->context->errout,
5074
1
             "UBX: TIM-SVIN: dur=%lu meanX=%ld meanY=%ld meanZ=%ld meanV=%lu "
5075
1
             "obs=%lu valid=%u(%s) active=%u(%s)\n",
5076
1
             dur, meanX, meanY, meanZ, meanV, obs,
5077
1
             valid, val2str(valid, vsvin_valid),
5078
1
             active, val2str(active, vsvin_active));
5079
1
    return mask;
5080
1
}
5081
5082
/**
5083
 * Time Pulse Timedata - UBX-TIM-TP
5084
 */
5085
static gps_mask_t ubx_msg_tim_tp(struct gps_device_t *session,
5086
                                 unsigned char *buf, size_t data_len)
5087
0
{
5088
0
    gps_mask_t mask = ONLINE_SET;
5089
0
    uint32_t towMS;
5090
0
    uint32_t towSubMS;
5091
0
    uint64_t tow_tmp;    // temp to convert towSubMS to nano seconds.
5092
0
    int32_t qErr;
5093
0
    uint16_t week;
5094
0
    uint8_t flags;
5095
0
    uint8_t refInfo;
5096
0
    timespec_t ts_tow;
5097
0
    char buf2[80];
5098
0
    char buf3[80];
5099
0
    const char *warn_msg = "";
5100
5101
0
    static const struct flist_t tim_tp_flags[] = {
5102
0
        {0, 1, "timebase:GNSS"},
5103
0
        {1, 1, "timebase:UTC"},
5104
0
        {0, 2, "UTC:NA"},
5105
0
        {2, 2, "UTC:OK"},
5106
0
        {0, 0x0c, "RAIM:NA"},
5107
0
        {4, 0x0c, "RAIM:inactive"},
5108
0
        {8, 0x0c, "RAIM:active"},
5109
0
        {0x0c, 0x0c, "RAIM:Unk"},
5110
        // qErrValid  9-series, protVer 32 and up.
5111
0
        {0, 0x10, "qErr:Valid"},
5112
0
        {0x10, 0x10, "qErr:Invalid"},
5113
        // TpNotLocked, 9-series, protVer 32 and up.
5114
0
        {0, 0x20, "TP:Locked"},
5115
0
        {0x20, 0x20, "TP:Unlocked"},
5116
0
        {0, 0, NULL},
5117
0
    };
5118
5119
0
    static const struct flist_t tim_tp_refInfo[] = {
5120
0
        {0, 0x0f, "GNSS:GPS"},
5121
0
        {1, 0x0f, "GNSS:GLONASS"},
5122
0
        {2, 0x0f, "GNSS:BeiDou"},
5123
0
        {3, 0x0f, "GNSS:Galileo"},
5124
0
        {4, 0x0f, "GNSS:NavIc"},
5125
0
        {5, 0x0f, "GNSS:Unk5"},
5126
0
        {6, 0x0f, "GNSS:Unk6"},
5127
0
        {7, 0x0f, "GNSS:Unk7"},
5128
0
        {8, 0x0f, "GNSS:Unk8"},
5129
0
        {9, 0x0f, "GNSS:Unk9"},
5130
0
        {10, 0x0f, "GNSS:Unk10"},
5131
0
        {11, 0x0f, "GNSS:Unk11"},
5132
0
        {12, 0x0f, "GNSS:Unk12"},
5133
0
        {13, 0x0f, "GNSS:Unk13"},
5134
0
        {14, 0x0f, "GNSS:Unk14"},
5135
0
        {15, 0x0f, "GNSS:Unk"},
5136
0
        {0x00, 0xf0, "UTC:Unk"},
5137
0
        {0x10, 0xf0, "UTC:CRL"},
5138
0
        {0x20, 0xf0, "UTC:NIST"},
5139
0
        {0x30, 0xf0, "UTC:USNO"},
5140
0
        {0x40, 0xf0, "UTC:BIPM"},
5141
0
        {0x50, 0xf0, "UTC:EL"},
5142
0
        {0x60, 0xf0, "UTC:SU"},
5143
0
        {0x70, 0xf0, "UTC:NTSC"},
5144
0
        {0x80, 0xf0, "UTC:NPLI"},
5145
0
        {0x90, 0xf0, "UTC:Unk9"},
5146
0
        {0xa0, 0xf0, "UTC:Unk10"},
5147
0
        {0xb0, 0xf0, "UTC:Unk11"},
5148
0
        {0xc0, 0xf0, "UTC:Unk12"},
5149
0
        {0xd0, 0xf0, "UTC:Unk13"},
5150
0
        {0xe0, 0xf0, "UTC:Unk14"},
5151
0
        {0xf0, 0xf0, "UTC:Unk"},
5152
0
        {0, 0, NULL},
5153
0
    };
5154
5155
0
    if (16 > data_len) {
5156
0
        GPSD_LOG(LOG_WARN, &session->context->errout,
5157
0
                 "UBX: TIM-TP: runt payload len %zd", data_len);
5158
0
        return 0;
5159
0
    }
5160
5161
0
    towMS = getleu32(buf, 0);
5162
    /* towSubMS is usually zero, but have seen 128, and 4294967168.
5163
     * towSubMs == 1 is 233 femto seconds!
5164
     * towSubMS == 128 is 29.802 pico seconds!
5165
     * towSubMS == 4294967168 is 0.9999999701976775 milli seconds
5166
     */
5167
0
    towSubMS = getleu32(buf, 4);
5168
0
    qErr = getles32(buf, 8);
5169
0
    week = getleu16(buf, 12);
5170
0
    flags = buf[14];
5171
0
    refInfo = buf[15];
5172
5173
0
    MSTOTS(&ts_tow, towMS);
5174
    /* scale towSubMS to nano seconds, add in 500 pico seconds for rounding
5175
     * then remove the u-blox scaling. */
5176
0
    tow_tmp = (((uint64_t)towSubMS * 1000000UL) + 500000UL) >> 32;
5177
0
    ts_tow.tv_nsec += tow_tmp;
5178
0
    TS_NORM(&ts_tow);       // can happen on rounding 0.999999999 to 1.0
5179
5180
    // check that it is close to top of second??
5181
0
    if (3 != (flags & 0x03)) {
5182
0
        warn_msg = " Not locked to UTC";
5183
0
    } else {
5184
        // are we UTC, and towSubMs is zer
5185
5186
        // leap already added!?!?
5187
0
        int saved_leap = session->context->leap_seconds;
5188
        // remove it!
5189
0
        session->context->leap_seconds = 0;
5190
5191
        // good, save qErr and qErr_time
5192
0
        session->gpsdata.qErr = qErr;
5193
        // FIXME?  save as ftow??
5194
0
        session->gpsdata.qErr_time = gpsd_gpstime_resolv(session, week, ts_tow);
5195
5196
        // restore leap
5197
0
        session->context->leap_seconds = saved_leap;
5198
0
    }
5199
5200
    // casts for 32 bit compatibility
5201
0
    GPSD_LOG(LOG_PROG, &session->context->errout,
5202
0
             "UBX: TIM-TP: towMS %lu, towSubMS %ld, qErr %ld week %u "
5203
0
             "flags x%02x, refInfo x%02x\n",
5204
0
             (unsigned long)towMS, (long)towSubMS, (long)qErr,
5205
0
              week, flags, refInfo);
5206
0
    GPSD_LOG(LOG_IO, &session->context->errout,
5207
0
             "UBX: TIM-TP: flags (%s) refInfo (%s) tos_tmp %llu %s\n",
5208
0
             flags2str(flags, tim_tp_flags, buf2, sizeof(buf2)),
5209
0
             flags2str(refInfo, tim_tp_refInfo, buf3, sizeof(buf3)),
5210
0
             (long long unsigned)tow_tmp, warn_msg);
5211
5212
0
    return mask;
5213
0
}
5214
5215
static gps_mask_t ubx_parse(struct gps_device_t * session, unsigned char *buf,
5216
                            size_t len)
5217
19.3k
{
5218
19.3k
    size_t data_len;
5219
19.3k
    unsigned short msgid;
5220
19.3k
    gps_mask_t mask = 0;
5221
19.3k
    unsigned char min_protver = 0;
5222
5223
    // the packet at least contains a head long enough for an empty message
5224
19.3k
    if (UBX_PREFIX_LEN > len) {
5225
0
        return 0;
5226
0
    }
5227
5228
19.3k
    session->cycle_end_reliable = true;
5229
19.3k
    session->driver.ubx.iTOW = -1;        // set by decoder
5230
5231
    // extract message id and length
5232
19.3k
    msgid = getbes16(buf, 2);
5233
19.3k
    data_len = (size_t) getles16(buf, 4);
5234
5235
    /* FIXME: make each case just call one function.
5236
     / then this switch can be turned into a table. */
5237
19.3k
    switch (msgid) {
5238
69
    case UBX_ACK_ACK:
5239
69
        FALLTHROUGH
5240
135
    case UBX_ACK_NAK:
5241
135
        mask = ubx_msg_ack(session, buf, data_len);
5242
135
        break;
5243
5244
    /* UBX-AID-*
5245
     * removed in protVer 32 */
5246
132
    case UBX_CFG_DOSC:
5247
132
        mask = ubx_msg_cfg_dosc(session, &buf[UBX_PREFIX_LEN], data_len);
5248
132
        break;
5249
0
    case UBX_CFG_ESRC:
5250
0
        mask = ubx_msg_cfg_esrc(session, &buf[UBX_PREFIX_LEN], data_len);
5251
0
        break;
5252
66
    case UBX_CFG_NAV5:
5253
        // deprecated in u-blox 10
5254
66
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: CFG-NAV5\n");
5255
66
        break;
5256
67
    case UBX_CFG_NAVX5:
5257
        // deprecated in u-blox 10
5258
67
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: CFG-NAVX5\n");
5259
67
        break;
5260
138
    case UBX_CFG_PRT:
5261
        // deprecated in u-blox 10
5262
138
        if (session->driver.ubx.port_id != buf[UBX_PREFIX_LEN + 0] ) {
5263
73
            session->driver.ubx.port_id = buf[UBX_PREFIX_LEN + 0];
5264
73
            GPSD_LOG(LOG_INF, &session->context->errout,
5265
73
                     "UBX: CFG-PRT: port %d\n", session->driver.ubx.port_id);
5266
73
        }
5267
138
        break;
5268
0
    case UBX_CFG_RATE:
5269
        // deprecated in u-blox 10
5270
0
        mask = ubx_msg_cfg_rate(session, &buf[UBX_PREFIX_LEN], data_len);
5271
0
        break;
5272
0
    case UBX_CFG_VALGET:
5273
0
        min_protver = 24;
5274
0
        mask = ubx_msg_cfg_valget(session, &buf[UBX_PREFIX_LEN], data_len);
5275
0
        break;
5276
5277
176
    case UBX_ESF_ALG:
5278
176
        mask = ubx_msg_esf_alg(session, &buf[UBX_PREFIX_LEN], data_len);
5279
176
        break;
5280
341
    case UBX_ESF_INS:
5281
341
        mask = ubx_msg_esf_ins(session, &buf[UBX_PREFIX_LEN], data_len);
5282
341
        break;
5283
132
    case UBX_ESF_MEAS:
5284
132
        mask = ubx_msg_esf_meas(session, &buf[UBX_PREFIX_LEN], data_len);
5285
132
        break;
5286
18
    case UBX_ESF_RAW:
5287
18
        mask = ubx_msg_esf_raw(session, &buf[UBX_PREFIX_LEN], data_len);
5288
18
        break;
5289
206
    case UBX_ESF_STATUS:
5290
206
        mask = ubx_msg_esf_status(session, &buf[UBX_PREFIX_LEN], data_len);
5291
206
        break;
5292
5293
68
    case UBX_HNR_ATT:
5294
68
        min_protver = 19;       // actually 19.2
5295
68
        mask = ubx_msg_hnr_att(session, &buf[UBX_PREFIX_LEN], data_len);
5296
68
        break;
5297
0
    case UBX_HNR_INS:
5298
0
        min_protver = 19;       // actually 19.1
5299
0
        mask = ubx_msg_hnr_ins(session, &buf[UBX_PREFIX_LEN], data_len);
5300
0
        break;
5301
0
    case UBX_HNR_PVT:
5302
0
        min_protver = 19;
5303
0
        mask = ubx_msg_hnr_pvt(session, &buf[UBX_PREFIX_LEN], data_len);
5304
0
        break;
5305
5306
36
    case UBX_INF_DEBUG:
5307
36
        FALLTHROUGH
5308
130
    case UBX_INF_ERROR:
5309
130
        FALLTHROUGH
5310
164
    case UBX_INF_NOTICE:
5311
164
        FALLTHROUGH
5312
164
    case UBX_INF_TEST:
5313
164
        FALLTHROUGH
5314
230
    case UBX_INF_USER:
5315
230
        FALLTHROUGH
5316
296
    case UBX_INF_WARNING:
5317
296
        min_protver = 13;
5318
296
        mask = ubx_msg_inf(session, buf, data_len);
5319
296
        break;
5320
5321
34
    case UBX_LOG_BATCH:
5322
34
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: LOG-BATCH\n");
5323
34
        mask = ubx_msg_log_batch(session, &buf[UBX_PREFIX_LEN], data_len);
5324
34
        break;
5325
0
    case UBX_LOG_INFO:
5326
0
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: LOG-INFO\n");
5327
0
        mask = ubx_msg_log_info(session, &buf[UBX_PREFIX_LEN], data_len);
5328
0
        break;
5329
0
    case UBX_LOG_RETRIEVEPOS:
5330
0
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: LOG-RETRIEVEPOS\n");
5331
0
        mask = ubx_msg_log_retrievepos(session, &buf[UBX_PREFIX_LEN], data_len);
5332
0
        break;
5333
0
    case UBX_LOG_RETRIEVEPOSEXTRA:
5334
0
        GPSD_LOG(LOG_PROG, &session->context->errout,
5335
0
                 "UBX: LOG-RETRIEVEPOSEXTRA\n");
5336
0
        mask = ubx_msg_log_retrieveposextra(session, &buf[UBX_PREFIX_LEN],
5337
0
                                            data_len);
5338
0
        break;
5339
0
    case UBX_LOG_RETRIEVESTRING:
5340
0
        GPSD_LOG(LOG_PROG, &session->context->errout,
5341
0
                 "UBX: LOG-RETRIEVESTRING\n");
5342
0
        mask = ubx_msg_log_retrievestring(session, &buf[UBX_PREFIX_LEN],
5343
0
                                          data_len);
5344
0
        break;
5345
5346
0
    case UBX_MON_BATCH:
5347
0
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: MON-BATCH\n");
5348
0
        break;
5349
0
    case UBX_MON_COMMS:
5350
0
        mask = ubx_msg_mon_comms(session, &buf[UBX_PREFIX_LEN], data_len);
5351
0
        break;
5352
18
    case UBX_MON_EXCEPT:
5353
18
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: MON-EXCEPT\n");
5354
18
        break;
5355
0
    case UBX_MON_GNSS:
5356
0
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: MON-GNSS\n");
5357
0
        break;
5358
327
    case UBX_MON_HW:
5359
327
        min_protver = 12;
5360
327
        mask = ubx_msg_mon_hw(session, &buf[UBX_PREFIX_LEN], data_len);
5361
327
        break;
5362
34
    case UBX_MON_HW2:
5363
         // Deprecated in protVer 32 (9-series, 10-series)
5364
34
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: MON-HW2\n");
5365
34
        break;
5366
0
    case UBX_MON_HW3:
5367
0
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: MON-HW3\n");
5368
0
        break;
5369
0
    case UBX_MON_IO:
5370
0
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: MON-IO\n");
5371
0
        break;
5372
66
    case UBX_MON_IPC:
5373
66
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: MON-IPC\n");
5374
66
        break;
5375
66
    case UBX_MON_MSGPP:
5376
66
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: MON-MSGPP\n");
5377
66
        break;
5378
0
    case UBX_MON_PATCH:
5379
0
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: MON-PATCH\n");
5380
0
        break;
5381
0
    case UBX_MON_RF:
5382
0
        mask = ubx_msg_mon_rf(session, &buf[UBX_PREFIX_LEN], data_len);
5383
0
        break;
5384
0
    case UBX_MON_RXBUF:
5385
0
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: MON-RXBUF\n");
5386
0
        mask = ubx_msg_mon_rxbuf(session, &buf[UBX_PREFIX_LEN], data_len);
5387
0
        break;
5388
18
    case UBX_MON_RXR:
5389
18
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: MON-RXR\n");
5390
18
        break;
5391
66
    case UBX_MON_SCHED:
5392
66
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: MON-SCHED\n");
5393
66
        break;
5394
18
    case UBX_MON_SMGR:
5395
18
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: MON-SMGR\n");
5396
18
        break;
5397
66
    case UBX_MON_SPAN:
5398
66
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: MON-SPAN\n");
5399
66
        break;
5400
0
    case UBX_MON_TXBUF:
5401
0
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: MON-TXBUF\n");
5402
0
        mask = ubx_msg_mon_txbuf(session, &buf[UBX_PREFIX_LEN], data_len);
5403
0
        break;
5404
66
    case UBX_MON_USB:
5405
66
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: MON-USB\n");
5406
66
        break;
5407
144
    case UBX_MON_VER:
5408
144
        mask = ubx_msg_mon_ver(session, &buf[UBX_PREFIX_LEN], data_len);
5409
144
        break;
5410
5411
67
    case UBX_NAV_AOPSTATUS:
5412
67
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: NAV-AOPSTATUS\n");
5413
67
        break;
5414
74
    case UBX_NAV_ATT:
5415
74
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: NAV-ATT\n");
5416
74
        break;
5417
132
    case UBX_NAV_CLOCK:
5418
132
        mask = ubx_msg_nav_clock(session, &buf[UBX_PREFIX_LEN], data_len);
5419
132
        break;
5420
135
    case UBX_NAV_DAHEADING:
5421
135
        mask = ubx_msg_nav_daheading(session, &buf[UBX_PREFIX_LEN], data_len);
5422
135
        break;
5423
145
    case UBX_NAV_DGPS:
5424
145
        mask = ubx_msg_nav_dgps(session, &buf[UBX_PREFIX_LEN], data_len);
5425
145
        break;
5426
269
    case UBX_NAV_DOP:
5427
        // DOP seems to be the last NAV sent in a cycle, unless NAV-EOE
5428
269
        mask = ubx_msg_nav_dop(session, &buf[UBX_PREFIX_LEN], data_len);
5429
269
        break;
5430
268
    case UBX_NAV_EELL:
5431
268
        min_protver = 18;
5432
268
        mask = ubx_msg_nav_eell(session, &buf[UBX_PREFIX_LEN], data_len);
5433
268
        break;
5434
66
    case UBX_NAV_EKFSTATUS:
5435
66
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: NAV-EKFSTATUS\n");
5436
66
        break;
5437
66
    case UBX_NAV_EOE:
5438
66
        min_protver = 18;
5439
66
        mask = ubx_msg_nav_eoe(session, &buf[UBX_PREFIX_LEN], data_len);
5440
66
        break;
5441
66
    case UBX_NAV_GEOFENCE:
5442
66
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: NAV-GEOFENCE\n");
5443
66
        break;
5444
167
    case UBX_NAV_HPPOSECEF:
5445
167
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: NAV-HPPOSECEF\n");
5446
167
        if (28 > data_len) {
5447
66
            GPSD_LOG(LOG_WARN, &session->context->errout,
5448
66
                     "UBX: NAV-HPPOSECEF: runt payload len %zd", data_len);
5449
101
        } else {
5450
101
            mask = ubx_msg_nav_hpposecef(session, &buf[UBX_PREFIX_LEN],
5451
101
                                         data_len);
5452
101
        }
5453
167
        break;
5454
132
    case UBX_NAV_HPPOSLLH:
5455
132
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: NAV-HPPOSLLH\n");
5456
132
        mask = ubx_msg_nav_hpposllh(session, &buf[UBX_PREFIX_LEN], data_len);
5457
132
        break;
5458
66
    case UBX_NAV_ODO:
5459
66
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: NAV-ODO\n");
5460
66
        break;
5461
66
    case UBX_NAV_ORB:
5462
66
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: NAV-ORB\n");
5463
66
        break;
5464
133
    case UBX_NAV_POSECEF:
5465
133
        mask = ubx_msg_nav_posecef(session, &buf[UBX_PREFIX_LEN], data_len);
5466
133
        break;
5467
145
    case UBX_NAV_POSLLH:
5468
145
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: NAV-POSLLH\n");
5469
145
        mask = ubx_msg_nav_posllh(session, &buf[UBX_PREFIX_LEN], data_len);
5470
145
        break;
5471
66
    case UBX_NAV_POSUTM:
5472
66
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: NAV-POSUTM\n");
5473
66
        break;
5474
637
    case UBX_NAV_PVAT:
5475
637
        min_protver = 30;
5476
637
        if (116 > data_len) {
5477
67
            GPSD_LOG(LOG_WARN, &session->context->errout,
5478
67
                     "UBX: NAV-PVAT: runt payload len %zd", data_len);
5479
570
        } else {
5480
570
            mask = ubx_msg_nav_pvat(session, &buf[UBX_PREFIX_LEN], data_len);
5481
570
        }
5482
637
        break;
5483
573
    case UBX_NAV_PVT:
5484
573
        min_protver = 14;
5485
        // u-blox 6 and 7 are 84 bytes, u-blox 8 and 9 are 92 bytes
5486
573
        if (84 > data_len) {
5487
75
            GPSD_LOG(LOG_WARN, &session->context->errout,
5488
75
                     "UBX: NAV-PVT: runt payload len %zd", data_len);
5489
498
        } else {
5490
498
            mask = ubx_msg_nav_pvt(session, &buf[UBX_PREFIX_LEN], data_len);
5491
498
        }
5492
573
        break;
5493
464
    case UBX_NAV_RELPOSNED:
5494
464
        min_protver = 20;
5495
464
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: NAV-RELPOSNED\n");
5496
464
        mask = ubx_msg_nav_relposned(session, &buf[UBX_PREFIX_LEN], data_len);
5497
464
        break;
5498
66
    case UBX_NAV_RESETODO:
5499
66
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: NAV-RESETODO\n");
5500
66
        break;
5501
440
    case UBX_NAV_SAT:
5502
440
        min_protver = 15;
5503
440
        mask = ubx_msg_nav_sat(session, &buf[UBX_PREFIX_LEN], data_len);
5504
440
        break;
5505
1.11k
    case UBX_NAV_SBAS:
5506
1.11k
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: NAV-SBAS\n");
5507
1.11k
        mask = ubx_msg_nav_sbas(session, &buf[UBX_PREFIX_LEN], data_len);
5508
1.11k
        break;
5509
562
    case UBX_NAV_SIG:
5510
562
        min_protver = 27;
5511
562
        mask = ubx_msg_nav_sig(session, &buf[UBX_PREFIX_LEN], data_len);
5512
562
        break;
5513
1.44k
    case UBX_NAV_SOL:
5514
        /* UBX-NAV-SOL deprecated in u-blox 6,
5515
         * removed in protVer 32 (9 and 10 series).
5516
         * Use UBX-NAV-PVT instead */
5517
1.44k
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: NAV-SOL\n");
5518
1.44k
        mask = ubx_msg_nav_sol(session, &buf[UBX_PREFIX_LEN], data_len);
5519
1.44k
        break;
5520
767
    case UBX_NAV_STATUS:
5521
767
        mask = ubx_msg_nav_status(session, &buf[UBX_PREFIX_LEN], data_len);
5522
767
        break;
5523
197
    case UBX_NAV_SVIN:
5524
197
        if (40 > data_len) {
5525
163
            GPSD_LOG(LOG_WARN, &session->context->errout,
5526
163
                     "UBX: NAV-SVIN: runt payload len %zd", data_len);
5527
163
            break;
5528
163
        }
5529
34
        mask = ubx_msg_nav_svin(session, &buf[UBX_PREFIX_LEN], data_len);
5530
34
        break;
5531
668
    case UBX_NAV_SVINFO:
5532
668
        mask = ubx_msg_nav_svinfo(session, &buf[UBX_PREFIX_LEN], data_len);
5533
668
        break;
5534
66
    case UBX_NAV_TIMEBDS:
5535
66
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: NAV-TIMEBDS\n");
5536
66
        break;
5537
66
    case UBX_NAV_TIMEGAL:
5538
66
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: NAV-TIMEGAL\n");
5539
66
        break;
5540
66
    case UBX_NAV_TIMEGLO:
5541
66
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: NAV-TIMEGLO\n");
5542
66
        break;
5543
540
    case UBX_NAV_TIMEGPS:
5544
540
        mask = ubx_msg_nav_timegps(session, &buf[UBX_PREFIX_LEN], data_len);
5545
540
        break;
5546
466
    case UBX_NAV_TIMELS:
5547
466
        mask = ubx_msg_nav_timels(session, &buf[UBX_PREFIX_LEN], data_len);
5548
466
        break;
5549
66
    case UBX_NAV_TIMEQZSS:
5550
66
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: NAV-TIMEQZSS\n");
5551
66
        break;
5552
455
    case UBX_NAV_TIMEUTC:
5553
455
        mask = ubx_msg_nav_timeutc(session, &buf[UBX_PREFIX_LEN], data_len);
5554
455
        break;
5555
134
    case UBX_NAV_VELECEF:
5556
134
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: NAV-VELECEF\n");
5557
134
        mask = ubx_msg_nav_velecef(session, &buf[UBX_PREFIX_LEN], data_len);
5558
134
        break;
5559
173
    case UBX_NAV_VELNED:
5560
173
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: NAV-VELNED\n");
5561
173
        mask = ubx_msg_nav_velned(session, &buf[UBX_PREFIX_LEN], data_len);
5562
173
        break;
5563
5564
0
    case UBX_MGA_ACK:
5565
0
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: MGA-ACK\n");
5566
0
        break;
5567
0
    case UBX_MGA_DBD:
5568
0
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: MGA-DBD\n");
5569
0
        break;
5570
5571
66
    case UBX_RXM_ALM:
5572
66
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: RXM-ALM\n");
5573
66
        break;
5574
0
    case UBX_RXM_COR:
5575
0
        min_protver = 27;
5576
0
        if (12 > data_len) {
5577
0
            GPSD_LOG(LOG_WARN, &session->context->errout,
5578
0
                     "UBX: RXM-COR: runt payload len %zd", data_len);
5579
0
            return 0;
5580
0
        }
5581
0
        mask = ubx_msg_rxm_cor(session, &buf[UBX_PREFIX_LEN], data_len);
5582
0
        break;
5583
34
    case UBX_RXM_EPH:
5584
34
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: RXM-EPH\n");
5585
34
        break;
5586
18
    case UBX_RXM_IMES:
5587
        // Removed in protVer 32 (9-series)
5588
18
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: RXM-IMES\n");
5589
18
        break;
5590
67
    case UBX_RXM_MEASX:
5591
67
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: RXM-MEASX\n");
5592
67
        break;
5593
0
    case UBX_RXM_PMREQ:
5594
0
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: RXM-PMREQ\n");
5595
0
        break;
5596
0
    case UBX_RXM_POSREQ:
5597
0
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: RXM-POSREQ\n");
5598
0
        break;
5599
66
    case UBX_RXM_RAW:
5600
66
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: RXM-RAW\n");
5601
66
        break;
5602
539
    case UBX_RXM_RAWX:
5603
539
        mask = ubx_msg_rxm_rawx(session, &buf[UBX_PREFIX_LEN], data_len);
5604
539
        break;
5605
0
    case UBX_RXM_RLM:
5606
0
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: RXM-RLM\n");
5607
0
        break;
5608
66
    case UBX_RXM_RTCM:
5609
66
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: RXM-RTCM\n");
5610
66
        break;
5611
137
    case UBX_RXM_SFRB:
5612
137
        mask = ubx_msg_rxm_sfrb(session, &buf[UBX_PREFIX_LEN], data_len);
5613
137
        break;
5614
314
    case UBX_RXM_SFRBX:
5615
314
        min_protver = 17;
5616
314
        if (8 > data_len) {
5617
0
            GPSD_LOG(LOG_WARN, &session->context->errout,
5618
0
                     "UBX: RXM-SFRBX: runt payload len %zd", data_len);
5619
0
            break;
5620
0
        }
5621
314
        mask = ubx_msg_rxm_sfrbx(session, &buf[UBX_PREFIX_LEN], data_len);
5622
314
        break;
5623
10
    case UBX_RXM_SPARTN:
5624
10
        min_protver = 27;
5625
10
        if (8 > data_len) {
5626
0
            GPSD_LOG(LOG_WARN, &session->context->errout,
5627
0
                     "UBX: RXM-SPARTN: runt payload len %zd", data_len);
5628
0
            return 0;
5629
0
        }
5630
10
        mask = ubx_msg_rxm_spartn(session, &buf[UBX_PREFIX_LEN], data_len);
5631
10
        break;
5632
70
    case UBX_RXM_SVSI:
5633
        // Removed in protVer 32 (9-series)
5634
        // Use UBX-NAV-ORB instead
5635
70
        mask = ubx_msg_rxm_svsi(session, &buf[UBX_PREFIX_LEN], data_len);
5636
70
        break;
5637
5638
    // undocumented
5639
    // case UBX_SEC_SESSID:
5640
    //     GPSD_LOG(LOG_PROG, &session->context->errout, "UBX-SEC-SESSID\n");
5641
    //     break;
5642
0
    case UBX_SEC_SIGN:
5643
0
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: SEC_SIGN\n");
5644
0
        break;
5645
0
    case UBX_SEC_UNIQID:
5646
0
        mask = ubx_msg_sec_uniqid(session, &buf[UBX_PREFIX_LEN], data_len);
5647
0
        break;
5648
0
    case UBX_TIM_DOSC:
5649
0
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: TIM-DOSC\n");
5650
0
        break;
5651
0
    case UBX_TIM_FCHG:
5652
0
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: TIM-FCHG\n");
5653
0
        break;
5654
0
    case UBX_TIM_HOC:
5655
0
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: TIM-HOC\n");
5656
0
        break;
5657
34
    case UBX_TIM_SMEAS:
5658
34
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: TIM-SMEAS\n");
5659
34
        break;
5660
1
    case UBX_TIM_SVIN:
5661
1
        if (28 > data_len) {
5662
0
            GPSD_LOG(LOG_WARN, &session->context->errout,
5663
0
                     "UBX: TIM-SVIN: runt payload len %zd", data_len);
5664
0
            break;
5665
0
        }
5666
1
        mask = ubx_msg_tim_svin(session, &buf[UBX_PREFIX_LEN], data_len);
5667
1
        break;
5668
0
    case UBX_TIM_TM:
5669
0
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: TIM-TM\n");
5670
0
        break;
5671
0
    case UBX_TIM_TM2:
5672
0
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: TIM-TM2\n");
5673
0
        break;
5674
0
    case UBX_TIM_TP:
5675
0
        mask = ubx_msg_tim_tp(session, &buf[UBX_PREFIX_LEN], data_len);
5676
0
        break;
5677
0
    case UBX_TIM_TOS:
5678
0
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: TIM-TOS\n");
5679
0
        break;
5680
0
    case UBX_TIM_VCOCAL:
5681
0
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: TIM-VCOCAL\n");
5682
0
        break;
5683
0
    case UBX_TIM_VRFY:
5684
0
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX: TIM-VRFY\n");
5685
0
        break;
5686
5687
4.21k
    default:
5688
4.21k
        GPSD_LOG(LOG_WARN, &session->context->errout,
5689
19.3k
                 "UBX: unknown packet id x%04hx (length %zd)\n",
5690
19.3k
                 msgid, len);
5691
19.3k
    }
5692
#ifdef __UNUSED
5693
    // debug
5694
    GPSD_LOG(LOG_PROG, &session->context->errout,
5695
             "UBX: msgid x%04x end x%04x last x%04x iTOW %lld last %lld\n",
5696
             msgid,
5697
             session->driver.ubx.end_msgid,
5698
             session->driver.ubx.last_msgid,
5699
             (long long)session->driver.ubx.iTOW,
5700
             (long long)session->driver.ubx.last_iTOW);
5701
#endif
5702
5703
    // iTOW drives the cycle start/end detection
5704
    // iTOW is in ms, can go forward or backward
5705
19.3k
    if (-1 < session->driver.ubx.iTOW) {
5706
8.85k
        int64_t iTOW_diff;
5707
5708
        // this sentence has a (maybe good) time
5709
        // end of cycle ?
5710
8.85k
        if (session->driver.ubx.end_msgid == msgid) {
5711
            // got known cycle ender.  Assume end of cycle, report it
5712
737
            GPSD_LOG(LOG_PROG, &session->context->errout,
5713
737
                     "UBX: cycle end x%04x iTOW %lld\n",
5714
737
                     msgid, (long long)session->driver.ubx.iTOW);
5715
737
            mask |= REPORT_IS;
5716
737
        }
5717
5718
        // start of cycle?  Start can equal end if only one message per epoch
5719
        // u-blox iTOW can have ms jitter in the same epoch!
5720
8.85k
        iTOW_diff = session->driver.ubx.last_iTOW - session->driver.ubx.iTOW;
5721
8.85k
        if (10 < llabs(iTOW_diff)) {
5722
            // time changed more than 10 ms (100 Hz), cycle start
5723
5724
1.98k
            if (session->driver.ubx.end_msgid !=
5725
1.98k
                session->driver.ubx.last_msgid) {
5726
                // new cycle ender
5727
286
                GPSD_LOG(LOG_PROG, &session->context->errout,
5728
286
                         "UBX: new ender x%04x was x%04x iTOW %lld was %lld\n",
5729
286
                         session->driver.ubx.last_msgid,
5730
286
                         session->driver.ubx.end_msgid,
5731
286
                         (long long)session->driver.ubx.iTOW,
5732
286
                         (long long)session->driver.ubx.last_iTOW);
5733
286
                session->driver.ubx.end_msgid = session->driver.ubx.last_msgid;
5734
286
            }
5735
1.98k
            session->driver.ubx.last_iTOW = session->driver.ubx.iTOW;
5736
1.98k
            mask |= CLEAR_IS;
5737
1.98k
        }
5738
5739
8.85k
        session->driver.ubx.last_msgid = msgid;
5740
        // FIXME: last_time never used...
5741
8.85k
        session->driver.ubx.last_time = session->newdata.time;
5742
10.5k
    } else {
5743
        // no time
5744
        /* debug
5745
        GPSD_LOG(LOG_ERROR, &session->context->errout,
5746
                 "UBX: No time, msgid %x\n", msgid);
5747
         */
5748
10.5k
    }
5749
5750
    // Did protver change?
5751
19.3k
    if (min_protver > session->driver.ubx.protver) {
5752
        // this GPS is at least min_protver
5753
638
        session->driver.ubx.protver = min_protver;
5754
638
    }
5755
19.3k
    if (session->driver.ubx.last_protver != session->driver.ubx.protver) {
5756
        /* Assumption: we just did init, but did not have
5757
         * protver then, so init is not complete.  Finish now.
5758
         * unless user requested passive mode */
5759
840
        if (session->mode == O_OPTIMIZE &&
5760
0
            !session->context->passive) {
5761
0
            ubx_mode(session, MODE_BINARY);
5762
0
        }
5763
840
        GPSD_LOG(LOG_PROG, &session->context->errout,
5764
840
                 "UBX: new PROTVER %u was %u\n",
5765
840
                 session->driver.ubx.protver,
5766
840
                 session->driver.ubx.last_protver);
5767
840
        session->driver.ubx.last_protver = session->driver.ubx.protver;
5768
        // restart init queue
5769
840
        session->queue = 0;
5770
840
    }
5771
5772
19.3k
    if (!session->context->readonly &&
5773
13.0k
        0 <= session->queue &&
5774
13.0k
        100 > session->queue &&
5775
12.9k
        0 < session->driver.ubx.protver) {
5776
5.81k
        unsigned char msg[4] = {0};
5777
5778
5.81k
        GPSD_LOG(LOG_DATA, &session->context->errout,
5779
5.81k
                 "UBX: queue %d\n", session->queue);
5780
5781
        /* handle the init queue.  Some u-blox parts get cranky when they
5782
         * get too many configuration changes at once.
5783
         */
5784
5785
5.81k
        if (50 <= session->queue &&
5786
654
            !session->context->passive) {
5787
            // turn off common NMEA, every 3rd queue turn.
5788
470
            int i = session->queue - 50;
5789
470
            if (0 == (i % 3)) {
5790
164
                int j = i / 3;
5791
164
                if (j < (int)sizeof(nmea_off)) {
5792
121
                    msg[0] = 0xf0;          // class, NMEA
5793
121
                    msg[2] = 0x00;          // rate, off
5794
121
                    msg[1] = nmea_off[j];    // msg id to turn off
5795
121
                    (void)ubx_write(session, UBX_CLASS_CFG, 0x01, msg, 3);
5796
121
                }
5797
164
            }
5798
470
        }
5799
5800
5.81k
        switch (session->queue) {
5801
572
        case 0:
5802
            /* need to doi this right away, so there are UBX message
5803
            * to push this queue forward */
5804
572
            if (!session->context->passive) {
5805
                // turn on common UBX-NAV
5806
364
                unsigned i;
5807
5808
364
                msg[0] = 0x01;          // class, UBX-NAV
5809
364
                msg[2] = 0x01;          // rate, one
5810
1.45k
                for (i = 0; i < ROWS(ubx_nav_on); i++) {
5811
1.09k
                    msg[1] = ubx_nav_on[i];          // msg id to turn on
5812
1.09k
                    (void)ubx_write(session, UBX_CLASS_CFG, 0x01, msg, 3);
5813
1.09k
                }
5814
364
            }
5815
572
            break;
5816
150
        case 10:
5817
            /* Older u-blox (6-series) may have ignored earlier requests
5818
             * for UBX-MON-VER.  Try again if needed. */
5819
150
            if ('\0' == session->subtype[0]) {
5820
                // request UBX-MON-VER, for  SW and HW Versions
5821
22
                (void)ubx_write(session, UBX_CLASS_MON, 0x04, NULL, 0);
5822
22
            }
5823
150
            break;
5824
92
        case 20:
5825
92
            if (!session->context->passive) {
5826
61
                unsigned i;
5827
5828
61
                msg[0] = 0x01;          // class, UBX-NAV
5829
61
                msg[2] = 0x01;          // rate, one
5830
61
                if (15 > session->driver.ubx.protver) {
5831
                    /* protver 14 or less, or unknown version,
5832
                     * We should have a version now.
5833
                     * Turn on pre-15 UBX-NAV stuff */
5834
5835
57
                    for (i = 0; i < ROWS(ubx_14_nav_on); i++) {
5836
                        // msg id to turn on
5837
38
                        msg[1] = ubx_14_nav_on[i];
5838
38
                        (void)ubx_write(session, UBX_CLASS_CFG, 0x01, msg, 3);
5839
38
                    }
5840
42
                } else {
5841
                    /* must be 15 <= session->driver.ubx.protver
5842
                     * turn on 15+ UBX-NAV */
5843
252
                    for (i = 0; i < ROWS(ubx_15_nav_on); i++) {
5844
                        // msg id to turn on
5845
210
                        msg[1] = ubx_15_nav_on[i];
5846
210
                        (void)ubx_write(session, UBX_CLASS_CFG, 0x01, msg, 3);
5847
210
                    }
5848
42
                }
5849
61
            }
5850
92
            break;
5851
16
        case 71:
5852
16
            if (!session->context->passive &&
5853
12
                15 <= session->driver.ubx.protver) {
5854
                // good cycle ender, except when it is not the ender...
5855
9
                msg[0] = 0x01;          // class
5856
9
                msg[1] = 0x61;          // msg id  = UBX-NAV-EOE
5857
9
                msg[2] = 0x01;          // every cycle
5858
9
                (void)ubx_write(session, UBX_CLASS_CFG, 0x01, msg, 3);
5859
9
            }
5860
16
            break;
5861
16
        case 75:
5862
16
            if (!session->context->passive &&
5863
12
                15 <= session->driver.ubx.protver) {
5864
9
                msg[0] = 0x01;          // class
5865
9
                msg[1] = 0x26;          // msg id  = UBX-NAV-TIMELS
5866
9
                msg[2] = 0xff;          // about every 4 mins if nav rate is 1Hz
5867
9
                (void)ubx_write(session, UBX_CLASS_CFG, 0x01, msg, 3);
5868
9
            }
5869
16
            break;
5870
12
        case 80:
5871
12
            if (18 <= session->driver.ubx.protver) {
5872
                // No UNIQ-ID before PROTVER 18
5873
                // UBX-SEC-UNIQID: query for uniq id
5874
7
                (void)ubx_write(session, UBX_CLASS_SEC, 0x03, NULL, 0);
5875
7
            }
5876
12
            break;
5877
11
        case 83:
5878
11
            if (session->context->passive) {
5879
                // do nothing
5880
8
            } else if (27 > session->driver.ubx.protver) {
5881
5
                msg[0] = 0x0a;          // class, UBX-MON
5882
5
                msg[1] = 0x09;          // MON-HW
5883
5
                msg[2] = 0x04;          // every 4
5884
5
                (void)ubx_write(session, UBX_CLASS_CFG, 0x01, msg, 3);
5885
5
            } else {
5886
3
                msg[0] = 0x0a;          // class, UBX-MON
5887
3
                msg[1] = 0x38;          // MON-RF
5888
3
                msg[2] = 0x04;          // every 4
5889
3
                (void)ubx_write(session, UBX_CLASS_CFG, 0x01, msg, 3);
5890
3
            }
5891
11
            break;
5892
8
        case 87:
5893
8
            if (!session->context->passive &&
5894
6
                15 > session->driver.ubx.protver) {
5895
2
                unsigned i;
5896
5897
                /* protver 14 or less
5898
                 * turn off 15 and above UBX-NAV.  Do we need to? */
5899
2
                msg[0] = 0x01;          // class, UBX-NAV
5900
2
                msg[2] = 0x00;          // rate, off
5901
12
                for (i = 0; i < ROWS(ubx_15_nav_on); i++) {
5902
10
                    msg[1] = ubx_15_nav_on[i];          // msg id to turn off
5903
10
                    (void)ubx_write(session, UBX_CLASS_CFG, 0x01, msg, 3);
5904
10
                }
5905
2
            }
5906
8
            break;
5907
6
        case 90:
5908
            // Turn off some clutter, no need to do it early
5909
6
            if (!session->context->passive &&
5910
5
                15 <= session->driver.ubx.protver &&
5911
4
                27 > session->driver.ubx.protver) {
5912
                /* protver 15 or more, and less than 27
5913
                 * Soturn off 14 and below UBX-NAV */
5914
2
                unsigned i;
5915
5916
2
                msg[0] = 0x01;          // class, UBX-NAV
5917
2
                msg[2] = 0x00;          // rate, off
5918
6
                for (i = 0; i < ROWS(ubx_14_nav_on); i++) {
5919
4
                    msg[1] = ubx_14_nav_on[i];          // msg id to turn off
5920
4
                    (void)ubx_write(session, UBX_CLASS_CFG, 0x01, msg, 3);
5921
4
                }
5922
2
            }
5923
6
            break;
5924
3
        case 93:
5925
            /* finish up by checking if we overflowed the input buffer
5926
             * request MON-RXBUF/TXBUFF, or MON-COMMS */
5927
3
            if (27 > session->driver.ubx.protver) {
5928
                // MON-RXBUF and MON-TXBUF
5929
1
                (void)ubx_write(session, UBX_CLASS_MON, 0x08, NULL, 0);
5930
1
                (void)ubx_write(session, UBX_CLASS_MON, 0x07, NULL, 0);
5931
2
            } else {
5932
                // MON-COMMS
5933
2
                (void)ubx_write(session, UBX_CLASS_MON, 0x36, NULL, 0);
5934
2
            }
5935
3
            break;
5936
4.92k
        default:
5937
4.92k
            break;
5938
5.81k
        }
5939
5.81k
        session->queue++;
5940
5.81k
    }
5941
19.3k
    return mask | ONLINE_SET;
5942
19.3k
}
5943
5944
static gps_mask_t parse_input(struct gps_device_t *session)
5945
20.6k
{
5946
20.6k
    if (UBX_PACKET == session->lexer.type) {
5947
19.3k
        return ubx_parse(session, session->lexer.outbuffer,
5948
19.3k
                         session->lexer.outbuflen);
5949
19.3k
    }
5950
1.28k
    return generic_parse_input(session);
5951
20.6k
}
5952
5953
// not used by gpsd, it's for gpsctl and friends
5954
static ssize_t ubx_control_send(struct gps_device_t *session, char *msg,
5955
                                size_t data_len)
5956
0
{
5957
0
    return ubx_write(session, (unsigned int)msg[0], (unsigned int)msg[1],
5958
0
                     (unsigned char *)msg + 2,
5959
0
                     (size_t)(data_len - 2)) ? ((ssize_t) (data_len + 7)) : -1;
5960
0
}
5961
5962
static void ubx_init_query(struct gps_device_t *session)
5963
263
{
5964
    // UBX-MON-VER: query for version information
5965
263
    (void)ubx_write(session, UBX_CLASS_MON, 0x04, NULL, 0);
5966
5967
    /* We can't get query for UBX-SEC-UNIQID as we need the protver first.
5968
     * Plus, we want to chain requests so as not to  overflow the receiver
5969
     * inbuffers. */
5970
263
}
5971
5972
static void ubx_event_hook(struct gps_device_t *session, event_t event)
5973
27.8k
{
5974
27.8k
    if (session->context->readonly) {
5975
9.57k
        return;
5976
9.57k
    }
5977
18.2k
    if (event == EVENT_IDENTIFIED) {
5978
204
        GPSD_LOG(LOG_PROG, &session->context->errout, "UBX identified\n");
5979
5980
        // no longer set UBX-CFG-SBAS here, u-blox 9 and 10 do not have it
5981
5982
204
        if (session->context->passive) {
5983
            /* passive mode, do no autoconfig
5984
             * but we really want MON-VER. */
5985
49
            (void)ubx_write(session, UBX_CLASS_MON, 0x04, NULL, 0);
5986
155
        } else if (O_OPTIMIZE == session->mode) {
5987
            // Turn off UBX output, turn on NMEA on this port.
5988
0
            ubx_mode(session, MODE_BINARY);
5989
155
        } else {
5990
            //* Turn off NMEA output, turn on UBX on this port.
5991
155
            ubx_mode(session, MODE_NMEA);
5992
155
        }
5993
18.0k
    } else if (event == EVENT_DEACTIVATE) {
5994
        /* There used to be a hotstart/reset here.
5995
         * That caused u-blox USB to re-enumerate.
5996
         * Sometimes to a new device name.
5997
         * Bad.  Don't do that anymore...
5998
         */
5999
0
    }
6000
18.2k
}
6001
6002
// generate and send a configuration block
6003
static gps_mask_t ubx_cfg_prt(struct gps_device_t *session, speed_t speed,
6004
                              const char parity,
6005
                              const int stopbits, const int mode)
6006
155
{
6007
155
    unsigned long usart_mode = 0;
6008
155
    unsigned char buf[UBX_CFG_LEN];
6009
155
    unsigned long i;
6010
6011
155
    memset(buf, '\0', UBX_CFG_LEN);
6012
6013
    /*
6014
     * When this is called from gpsd, the initial probe for UBX should
6015
     * have picked up the device's port number from the CFG_PRT response.
6016
     */
6017
    // FIXME!  Bad test, port_id == 0 is valid too.  DDC (I2X) = port 0
6018
155
    if (session->driver.ubx.port_id != 0) {
6019
70
        buf[0] = session->driver.ubx.port_id;
6020
70
    }
6021
    /*
6022
     * This default can be hit if we haven't sent a CFG_PRT query yet,
6023
     * which can happen in gpsmon because it doesn't autoprobe.
6024
     *
6025
     * What we'd like to do here is dispatch to USART1_ID or
6026
     * USB_ID intelligently based on whether this is a USB or RS232
6027
     * source.  Unfortunately the GR601-W screws that up by being
6028
     * a USB device with port_id 1.  So we bite the bullet and
6029
     * default to port 1.
6030
     *
6031
     * Without further logic, this means gpsmon wouldn't be able to
6032
     * change the speed on the EVK 6H's USB port.  But! To pick off
6033
     * the EVK 6H on Linux as a special case, we notice that its
6034
     * USB device name is /dev/ttyACMx - it presents as a USB modem.
6035
     *
6036
     * This logic will fail on any USB u-blox device that presents
6037
     * as an ordinary USB serial device (/dev/ttyUSB*) and actually
6038
     * has port ID 3 the way it "ought" to.
6039
     */
6040
85
    else if (strstr(session->gpsdata.dev.path, "/ttyACM") != NULL) {
6041
        // using the built in USB port
6042
        // FIXME!!  USB port has no speed!
6043
        // FIXME!!  maybe we know the portid already?
6044
0
        session->driver.ubx.port_id = buf[0] = USB_ID;
6045
85
    } else {
6046
        // A guess.  Could be UART2, or SPI, or DDC port
6047
85
        session->driver.ubx.port_id = buf[0] = USART1_ID;
6048
85
    }
6049
6050
155
    putle32(buf, 8, speed);
6051
6052
    /*
6053
     * u-blox tech support explains the default contents of the mode
6054
     * field as follows:
6055
     *
6056
     * D0 08 00 00     mode (LSB first)
6057
     *
6058
     * re-ordering bytes: 000008D0
6059
     * dividing into fields: 000000000000000000 00 100 0 11 0 1 0000
6060
     * nStopbits = 00 = 1
6061
     * parity = 100 = none
6062
     * charLen = 11 = 8-bit
6063
     * reserved1 = 1
6064
     *
6065
     * The protocol reference further gives the following subfield values:
6066
     * 01 = 1.5 stop bits (?)
6067
     * 10 = 2 stopbits
6068
     * 000 = even parity
6069
     * 001 = odd parity
6070
     * 10x = no parity
6071
     * 10 = 7 bits
6072
     *
6073
     * Some UBX reference code amplifies this with:
6074
     *
6075
     *   prtcfg.mode = (1<<4) | // compatibility with ANTARIS 4
6076
     *                 (1<<7) | // charLen = 11 = 8 bit
6077
     *                 (1<<6) | // charLen = 11 = 8 bit
6078
     *                 (1<<11); // parity = 10x = none
6079
     */
6080
155
    usart_mode |= (1<<4);       // reserved1 Antaris 4 compatibility bit
6081
155
    usart_mode |= (1<<7);       // high bit of charLen
6082
6083
    // u-blox 5+ binary only supports 8N1
6084
155
    switch (parity) {
6085
0
    case (int)'E':
6086
0
    case 2:
6087
0
        usart_mode |= (1<<7);           // 7E
6088
0
        break;
6089
0
    case (int)'O':
6090
0
    case 1:
6091
0
        usart_mode |= (1<<9) | (1<<7);  // 7O
6092
0
        break;
6093
155
    case (int)'N':
6094
155
    case 0:
6095
155
    default:
6096
155
        usart_mode |= (1<<11) | (3<<6); // 8N
6097
155
        break;
6098
155
    }
6099
6100
155
    if (2 == stopbits) {
6101
0
        usart_mode |= (1<<13);
6102
0
    }
6103
6104
155
    putle32(buf, 4, usart_mode);
6105
6106
    // enable all input protocols by default
6107
    // RTCM3 is protver 20+
6108
155
    buf[12] = NMEA_PROTOCOL_MASK | UBX_PROTOCOL_MASK | RTCM_PROTOCOL_MASK |
6109
155
              RTCM3_PROTOCOL_MASK;
6110
6111
    /* enable all input protocols by default
6112
     * no u-blox has RTCM2 out
6113
     * RTCM3 is protver 20+ */
6114
155
    buf[outProtoMask] = NMEA_PROTOCOL_MASK | UBX_PROTOCOL_MASK |
6115
155
                        RTCM3_PROTOCOL_MASK;
6116
    // FIXME: use VALGET if protver  24+
6117
155
    (void)ubx_write(session, UBX_CLASS_CFG, 0x00, buf, sizeof(buf));
6118
6119
155
    GPSD_LOG(LOG_PROG, &session->context->errout,
6120
155
             "UBX ubx_cfg_prt mode %d port %d PROTVER %d\n", mode, buf[0],
6121
155
             session->driver.ubx.protver);
6122
6123
    // selectively enable output protocols
6124
155
    if (mode == MODE_NMEA) {
6125
        /*
6126
         * We have to club the GR601-W over the head to make it stop emitting
6127
         * UBX after we've told it to start.  But do not mung the
6128
         * protocol out mask, that breaks things.
6129
         */
6130
6131
155
        unsigned char msg[3];
6132
        /* nmea to turn on at rate one (multiplier on measurement rate)
6133
         * u-blox 8 default: RMC, VTG, GGA, GSA GSV, GLL
6134
         * who wanted GST? */
6135
155
        const unsigned char nmea_on[] = {
6136
155
            0x00,          // msg id  = GGA
6137
            // 0x01,          // msg id  = GLL, only need RMC
6138
155
            0x02,          // msg id  = GSA
6139
155
            0x03,          // msg id  = GSV
6140
155
            0x04,          // msg id  = RMC
6141
155
            0x05,          // msg id  = VTG
6142
155
            0x07,          // msg id  = GST, GNSS pseudorange error statistics
6143
155
            0x08,          // msg id  = ZDA, for UTC year
6144
155
            0x09,          // msg id  = GBS, for RAIM errors
6145
155
        };
6146
6147
155
        const unsigned char ubx_nav_off[] = {
6148
155
            0x01,          // msg id = NAV-POSECEF
6149
155
            0x04,          // msg id = UBX-NAV-DOP
6150
155
            0x06,          // msg id = NAV-SOL, deprecated in 6, gone in 9
6151
155
            0x07,          // msg id = NAV-PVT, in u-blox 6 and on
6152
155
            0x11,          // msg id = NAV-VELECEF
6153
155
            0x20,          // msg id = UBX-NAV-TIMEGPS
6154
            // 0x26;       // msg id  = UBX-NAV-TIMELS, allow as low rate
6155
155
            0x30,          // msg id = NAV-SVINFO, in 4 to 8, not 9
6156
155
            0x32,          // msg id = NAV-SBAS, in u-blox 4 to 8, not all 9
6157
155
            0x35,          // msg id = NAV-SAT, in u-blox 8 and up
6158
155
            0x43,          // msg id = NAV-SIG, in u-blox 9 and up
6159
155
            0x61,          // msg id = NAV-EOE
6160
155
        };
6161
6162
        // turn off init queue
6163
155
        session->queue = 0;
6164
6165
        // enable NMEA first, in case we over-run receiver input buffer.
6166
6167
        // turn on rate one NMEA
6168
155
        msg[0] = 0xf0;          // class, NMEA
6169
155
        msg[2] = 0x01;          // rate, one
6170
1.39k
        for (i = 0; i < sizeof(nmea_on); i++) {
6171
1.24k
            msg[1] = nmea_on[i];          // msg id to turn on
6172
1.24k
            (void)ubx_write(session, UBX_CLASS_CFG, 0x01, msg, 3);
6173
1.24k
        }
6174
6175
        // Now turn off UBX-NAV, one at a time.
6176
155
        msg[0] = 0x01;          // class, UBX-NAV
6177
155
        msg[2] = 0x00;          // rate off
6178
1.86k
        for (i = 0; i < sizeof(ubx_nav_off); i++) {
6179
1.70k
            msg[1] = ubx_nav_off[i];          // msg id to turn on
6180
1.70k
            (void)ubx_write(session, UBX_CLASS_CFG, 0x01, msg, 3);
6181
1.70k
        }
6182
6183
155
    } else {    // MODE_BINARY
6184
6185
        /*
6186
         * Just enabling the UBX protocol for output is not enough to
6187
         * actually get UBX output; the sentence mix is initially empty.
6188
         * Fix that...
6189
         */
6190
6191
        /* Beware sending too many messages without waiting
6192
         * for u-blox ACK, over running its input buffer.
6193
         *
6194
         * For example, the UBX-MON-VER may fail here, but works in other
6195
         * contexts.
6196
         *
6197
         * Need UBX-MON-VER for protver.  Need protver to properly configure
6198
         * the message set.
6199
         */
6200
0
        unsigned char msg[3] = {0, 0, 0};
6201
        // request SW and HW Versions, prolly already requested at detection
6202
        // ask again as older u-blox are hard of hearing
6203
0
        (void)ubx_write(session, UBX_CLASS_MON, 0x04, msg, 0);
6204
6205
0
        GPSD_LOG(LOG_IO, &session->context->errout, "UBX: init protVer %u\n",
6206
0
                 session->driver.ubx.protver);
6207
6208
        // turn on init queue
6209
0
        session->queue = 1;
6210
0
    }
6211
155
    return 0;
6212
155
}
6213
6214
static void ubx_mode(struct gps_device_t *session, int mode)
6215
155
{
6216
155
    ubx_cfg_prt(session,
6217
155
                gpsd_get_speed(session),
6218
155
                gpsd_get_parity(session),
6219
155
                gpsd_get_stopbits(session),
6220
155
                mode);
6221
155
}
6222
6223
static bool ubx_speed(struct gps_device_t *session,
6224
                      speed_t speed, char parity, int stopbits)
6225
0
{
6226
0
    ubx_cfg_prt(session,
6227
0
                speed,
6228
0
                parity,
6229
0
                stopbits,
6230
0
                (session->lexer.type == UBX_PACKET) ? MODE_BINARY : MODE_NMEA);
6231
0
    return true;
6232
0
}
6233
6234
/* change the sample rate of the GPS */
6235
static bool ubx_rate(struct gps_device_t *session, double cycletime)
6236
0
{
6237
    /* Minimum measurement cycle time currently known from documentation
6238
     * for fastest devices, here in milli seconds. Maintained in
6239
     * struct gps_type_t driver_ubx.
6240
     */
6241
0
    const int64_t min_cycle = TSTOMS(&session->device_type->min_cycle);
6242
    // cycletime in milli seconds
6243
0
    int64_t measRate = (int64_t)(cycletime * MS_IN_SEC);
6244
    // Message to be sent to device
6245
0
    unsigned char msg[6] = {
6246
0
        0x00, 0x00,     // U2: Measurement rate (ms), will be set below
6247
0
        0x01, 0x00,     // U2: Navigation rate (cycles), set to 1
6248
0
        0x00, 0x00,     // U2: Alignment to reference time: 0 = UTC
6249
0
    };
6250
6251
    // check max
6252
0
    if (65535 < measRate) {
6253
0
        measRate = 65535;   // milli seconds
6254
0
    } else if (min_cycle > measRate) {
6255
        /* Clamp cycle time to lowest bound given in documentation.
6256
         * protVer >= 24 has 25 ms min.
6257
         * protVer < 24 has min of 50ms or more.
6258
         */
6259
0
        measRate = min_cycle;
6260
0
    }
6261
    // we now know measRate fits in a U2
6262
6263
0
    GPSD_LOG(LOG_PROG, &session->context->errout,
6264
0
             "UBX rate change, measRate %lld millisecs\n",
6265
0
             (long long) measRate);
6266
0
    msg[0] = (unsigned char)(measRate & 0xff);
6267
0
    msg[1] = (unsigned char)(measRate >> 8);
6268
6269
    // UBX-CFG-RATE deprecated in u-blox 10
6270
0
    return ubx_write(session, UBX_CLASS_CFG, 0x08, msg, 6); // CFG-RATE
6271
0
}
6272
6273
// This is everything we export
6274
// *INDENT-OFF*
6275
const struct gps_type_t driver_ubx = {
6276
    .type_name         = "u-blox",       // Full name of type
6277
    .packet_type       = UBX_PACKET,     // associated lexer packet type
6278
    .flags             = DRIVER_STICKY,  // remember this
6279
    .trigger           = NULL,
6280
    // Number of satellite channels supported by the device
6281
    // ZED-F0T supports 60, ZED-F0P supports 184
6282
    .channels          = 184,
6283
    .probe_detect      = NULL,           // Startup-time device detector
6284
    // Packet getter (using default routine)
6285
    .get_packet        = packet_get1,
6286
    .parse_packet      = parse_input,    // Parse message packets
6287
    // RTCM handler (using default routine)
6288
    .rtcm_writer       = gpsd_write,
6289
    .init_query        = ubx_init_query, // non-perturbing initial query
6290
    .event_hook        = ubx_event_hook, // Fire on various lifetime events
6291
    .speed_switcher    = ubx_speed,      // Speed (baudrate) switch
6292
    .mode_switcher     = ubx_mode,       // Mode switcher
6293
    .rate_switcher     = ubx_rate,       // Message delivery rate switcher
6294
    /* Minimum measurement cycle time currently known from documentation
6295
     * for fastest devices.
6296
     */
6297
    .min_cycle.tv_sec  = 0,
6298
    .min_cycle.tv_nsec = 25000000,          // Maximum 40Hz sample rate
6299
    .control_send      = ubx_control_send,  // how to send a control string
6300
    .time_offset       = NULL,              // no method for NTP fudge factor
6301
};
6302
// *INDENT-ON*
6303
6304
// vim: set expandtab shiftwidth=4