/src/gpsd/gpsd-3.27.6~dev/drivers/driver_rtcm2.c
Line | Count | Source |
1 | | /***************************************************************************** |
2 | | |
3 | | This is a decoder for RTCM-104 2.x, an obscure and complicated serial |
4 | | protocol used for broadcasting pseudorange corrections from |
5 | | differential-GPS reference stations. The applicable |
6 | | standard is |
7 | | |
8 | | RTCM RECOMMENDED STANDARDS FOR DIFFERENTIAL GNSS (GLOBAL NAVIGATION |
9 | | SATELLITE) SERVICE, VERSION 2.3 (RTCM PAPER 136-2001/SC104-STD) |
10 | | |
11 | | Ordering instructions are accessible from <http://www.rtcm.org/> |
12 | | under "Publications". This describes version 2.3 of the RTCM specification. |
13 | | RTCM-104 was later completely redesigned as level 3.0. |
14 | | |
15 | | Also applicable is ITU-R M.823: "Technical characteristics of |
16 | | differential transmissions for global navigation satellite systems |
17 | | from maritime radio beacons in the frequency band 283.5 - 315 kHz in |
18 | | region 1 and 285 - 325 kHz in regions 2 & 3." |
19 | | |
20 | | The RTCM 2.x protocol uses as a transport layer the GPS satellite downlink |
21 | | protocol described in IS-GPS-200, the Navstar GPS Interface |
22 | | Specification. This code relies on the lower-level packet-assembly |
23 | | code for that protocol in isgps.c. |
24 | | |
25 | | The lower layer's job is done when it has assembled a message of up to |
26 | | 33 30-bit words of clean parity-checked data. At this point this upper layer |
27 | | takes over. struct rtcm2_msg_t is overlaid on the buffer and the bitfields |
28 | | are used to extract pieces of it. Those pieces are copied and (where |
29 | | necessary) reassembled into a struct rtcm2_t. |
30 | | |
31 | | This code and the contents of isgps.c are evolved from code by |
32 | | Wolfgang Rupprecht. Wolfgang's decoder was loosely based on one |
33 | | written by John Sager in 1999. Here are John Sager's original notes: |
34 | | |
35 | | The RTCM decoder prints a legible representation of the input data. |
36 | | The RTCM SC-104 specification is copyrighted, so I cannot |
37 | | quote it - in fact, I have never read it! Most of the information |
38 | | used to develop the decoder came from publication ITU-R M.823. |
39 | | This is a specification of the data transmitted from LF DGPS |
40 | | beacons in the 300kHz band. M.823 contains most of those parts of |
41 | | RTCM SC-104 directly relevant to the air interface (there |
42 | | are one or two annoying and vital omissions!). Information |
43 | | about the serial interface format was gleaned from studying |
44 | | the output of a beacon receiver test program made available on |
45 | | Starlink's website. |
46 | | |
47 | | This code has been checked against ASCII dumps made by a proprietary |
48 | | decoder running under Windows and is known to be consistent with it |
49 | | with respect to message types 1, 3, 9, 14, 16, and 31. Decoding of |
50 | | message types 4, 5, 6, 7, and 13 has not been checked. Message types |
51 | | 8, 10-12, 15-27, 28-30 (undefined), 31-37, 38-58 (undefined), and |
52 | | 60-63 are not yet supported. |
53 | | |
54 | | This file is Copyright 2010 by the GPSD project |
55 | | SPDX-License-Identifier: BSD-2-clause |
56 | | |
57 | | *****************************************************************************/ |
58 | | |
59 | | #include "../include/gpsd_config.h" // must be before all includes |
60 | | |
61 | | #include <stdio.h> |
62 | | #include <string.h> |
63 | | |
64 | | #include "../include/compiler.h" // for FALLTHROUGH |
65 | | #include "../include/gpsd.h" |
66 | | |
67 | | /* |
68 | | __BYTE_ORDER__, __ORDER_BIG_ENDIAN__ and __ORDER_LITTLE_ENDIAN__ are |
69 | | defined in some gcc versions only, probably depending on the |
70 | | architecture. Try to use endian.h if the gcc way fails - endian.h also |
71 | | does not seem to be available on all platforms. |
72 | | */ |
73 | | |
74 | | #if defined(HAVE_BUILTIN_ENDIANNESS) && HAVE_BUILTIN_ENDIANNESS |
75 | | #if __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__ |
76 | | #define WORDS_BIGENDIAN 1 |
77 | | #elif __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ |
78 | | #undef WORDS_BIGENDIAN |
79 | | #else |
80 | | #error Unknown endianness! |
81 | | #endif |
82 | | |
83 | | #else // !HAVE_BUILTIN_ENDIANNESS |
84 | | |
85 | | #if defined(HAVE_ENDIAN_H) |
86 | | #include <endian.h> |
87 | | #elif defined(HAVE_SYS_ENDIAN_H) |
88 | | #include <sys/endian.h> |
89 | | #elif defined(HAVE_MACHINE_ENDIAN_H) |
90 | | #include <machine/endian.h> |
91 | | #endif |
92 | | |
93 | | /* |
94 | | * Darwin (Mac OS X) uses special defines. |
95 | | * This must precede the BSD case, since _BIG_ENDIAN may be incorrectly defined |
96 | | */ |
97 | | #if !defined( __BYTE_ORDER) && defined(__DARWIN_BYTE_ORDER) |
98 | | #define __BYTE_ORDER __DARWIN_BYTE_ORDER |
99 | | #endif |
100 | | #if !defined( __BIG_ENDIAN) && defined(__DARWIN_BIG_ENDIAN) |
101 | | #define __BIG_ENDIAN __DARWIN_BIG_ENDIAN |
102 | | #endif |
103 | | #if !defined( __LITTLE_ENDIAN) && defined(__DARWIN_LITTLE_ENDIAN) |
104 | | #define __LITTLE_ENDIAN __DARWIN_LITTLE_ENDIAN |
105 | | #endif |
106 | | |
107 | | /* |
108 | | * BSD uses _BYTE_ORDER, and Linux uses __BYTE_ORDER. |
109 | | */ |
110 | | #if !defined( __BYTE_ORDER) && defined(_BYTE_ORDER) |
111 | | #define __BYTE_ORDER _BYTE_ORDER |
112 | | #endif |
113 | | #if !defined( __BIG_ENDIAN) && defined(_BIG_ENDIAN) |
114 | | #define __BIG_ENDIAN _BIG_ENDIAN |
115 | | #endif |
116 | | #if !defined( __LITTLE_ENDIAN) && defined(_LITTLE_ENDIAN) |
117 | | #define __LITTLE_ENDIAN _LITTLE_ENDIAN |
118 | | #endif |
119 | | |
120 | | #if !defined(__BYTE_ORDER) || !defined(__BIG_ENDIAN) || \ |
121 | | !defined(__LITTLE_ENDIAN) |
122 | | #error endianness macros are not defined |
123 | | #endif |
124 | | |
125 | | #if __BYTE_ORDER == __BIG_ENDIAN |
126 | | #define WORDS_BIGENDIAN 1 |
127 | | #elif __BYTE_ORDER == __LITTLE_ENDIAN |
128 | | #undef WORDS_BIGENDIAN |
129 | | #else |
130 | | #error Unknown endianness! |
131 | | #endif //__BYTE_ORDER |
132 | | |
133 | | #endif // !HAVE_BUILTIN_ENDIANNESS |
134 | | |
135 | | /* |
136 | | * Structures for interpreting words in an RTCM-104 2.x message (after |
137 | | * parity checking and removing inversion). Note, these structures |
138 | | * are overlaid on the raw data in order to decode them into |
139 | | * bitfields; this will fail horribly if your C compiler introduces |
140 | | * padding between or before bit fields, or between 8-bit-aligned |
141 | | * bitfields and character arrays despite #pragma pack(1). The right |
142 | | * things happen under gcc 4.x on amd64, i386, ia64, all arm and mips |
143 | | * variants, m68k, and powerpc) |
144 | | * |
145 | | * (In practice, the only class of machines on which this is likely |
146 | | * to fail are word-aligned architectures without barrel shifters. |
147 | | * Very few of these are left in 2012. By test, we know of s390, s390x, |
148 | | * and sparc.) |
149 | | * |
150 | | * The RTCM 2.1 standard is less explicit than it should be about |
151 | | * signed-integer representations. Two's complement is specified for |
152 | | * some but not all. |
153 | | */ |
154 | | |
155 | 11.4k | #define ZCOUNT_SCALE 0.6 // sec |
156 | 2.13k | #define PRCSMALL 0.02 // meters |
157 | 484 | #define PRCLARGE 0.32 // meters |
158 | 2.13k | #define RRSMALL 0.002 // meters/sec |
159 | 484 | #define RRLARGE 0.032 // meters/sec |
160 | | |
161 | | #define MAXPCSMALL (0x7FFF * PCSMALL) // 16-bits signed |
162 | | #define MAXRRSMALL (0x7F * RRSMALL) // 8-bits signed |
163 | | |
164 | 396 | #define XYZ_SCALE 0.01 // meters |
165 | | // extended ECEF scale |
166 | 801 | #define EXYZ_SCALE 0.0001 // meters |
167 | | // ECEF delta scale |
168 | 402 | #define DXYZ_SCALE 0.1 // meters |
169 | | // extended ECEF delta scale: 1/256 cm |
170 | 1.61k | #define EDXYZ_SCALE (1.0/256.0) // centimeter |
171 | | // L2 extended ECEF delta scale: 1/16 cm |
172 | 582 | #define EDXYZ2_SCALE (1.0/16.0) // centimeter |
173 | 1.20k | #define LA_SCALE (90.0/32767.0) // degrees |
174 | 1.20k | #define LO_SCALE (180.0/32767.0) // degrees |
175 | 1.20k | #define FREQ_SCALE 0.1 // kHz |
176 | 1.20k | #define FREQ_OFFSET 190.0 // kHz |
177 | 512 | #define CNR_OFFSET 24 // dB |
178 | 1.07k | #define TU_SCALE 5 // minutes |
179 | | |
180 | 808 | #define LATLON_SCALE 0.01 // degrees |
181 | 404 | #define RANGE_SCALE 4 // kilometers |
182 | | |
183 | | #pragma pack(1) |
184 | | |
185 | | /* |
186 | | * Reminder: Emacs reverse-region is useful... |
187 | | */ |
188 | | |
189 | | #ifndef WORDS_BIGENDIAN // little-endian, like x86, amd64 |
190 | | |
191 | | struct rtcm2_msg_t { |
192 | | struct rtcm2_msghw1 { // header word 1 |
193 | | unsigned int parity:6; |
194 | | unsigned int refstaid:10; // reference station ID |
195 | | unsigned int msgtype:6; // RTCM message type |
196 | | unsigned int preamble:8; // fixed at 01100110 |
197 | | unsigned int _pad:2; |
198 | | } w1; |
199 | | |
200 | | struct rtcm2_msghw2 { // header word 2 |
201 | | unsigned int parity:6; |
202 | | unsigned int stathlth:3; // station health |
203 | | unsigned int frmlen:5; |
204 | | unsigned int sqnum:3; |
205 | | unsigned int zcnt:13; |
206 | | unsigned int _pad:2; |
207 | | } w2; |
208 | | |
209 | | union { |
210 | | // msg 1 - differential gps corrections |
211 | | struct rtcm2_msg1 { |
212 | | struct gps_correction_t { |
213 | | struct { // msg 1 word 3 |
214 | | unsigned int parity:6; |
215 | | int prc1:16; |
216 | | unsigned int satident1:5; // satellite ID |
217 | | unsigned int udre1:2; |
218 | | unsigned int scale1:1; |
219 | | unsigned int _pad:2; |
220 | | } w3; |
221 | | struct { // msg 1 word 4 |
222 | | unsigned int parity:6; |
223 | | unsigned int satident2:5; // satellite ID |
224 | | unsigned int udre2:2; |
225 | | unsigned int scale2:1; |
226 | | unsigned int iod1:8; |
227 | | int rrc1:8; |
228 | | unsigned int _pad:2; |
229 | | } w4; |
230 | | struct { // msg 1 word 5 |
231 | | unsigned int parity:6; |
232 | | int rrc2:8; |
233 | | int prc2:16; |
234 | | unsigned int _pad:2; |
235 | | } w5; |
236 | | struct { // msg 1 word |
237 | | unsigned int parity:6; |
238 | | int prc3_h:8; |
239 | | unsigned int satident3:5; // satellite ID |
240 | | unsigned int udre3:2; |
241 | | unsigned int scale3:1; |
242 | | unsigned int iod2:8; |
243 | | unsigned int _pad:2; |
244 | | } w6; |
245 | | struct { // msg 1 word 7 |
246 | | unsigned int parity:6; |
247 | | unsigned int iod3:8; |
248 | | int rrc3:8; |
249 | | // NOTE: unsigned int for low byte |
250 | | unsigned int prc3_l:8; |
251 | | unsigned int _pad:2; |
252 | | } w7; |
253 | | } corrections[(RTCM2_WORDS_MAX - 2) / 5]; |
254 | | } type1; |
255 | | |
256 | | /* msg 2 - Pseudo-range corrections, referring to previous orbit |
257 | | * data records (maximum 12 satellites) */ |
258 | | |
259 | | // msg 3 - reference station parameters |
260 | | struct rtcm2_msg3 { |
261 | | struct { |
262 | | unsigned int parity:6; |
263 | | unsigned int x_h:24; |
264 | | unsigned int _pad:2; |
265 | | } w3; |
266 | | struct { |
267 | | unsigned int parity:6; |
268 | | unsigned int y_h:16; |
269 | | unsigned int x_l:8; |
270 | | unsigned int _pad:2; |
271 | | } w4; |
272 | | struct { |
273 | | unsigned int parity:6; |
274 | | unsigned int z_h:8; |
275 | | unsigned int y_l:16; |
276 | | unsigned int _pad:2; |
277 | | } w5; |
278 | | struct { |
279 | | unsigned int parity:6; |
280 | | unsigned int z_l:24; |
281 | | unsigned int _pad:2; |
282 | | } w6; |
283 | | } type3; |
284 | | |
285 | | // msg 4 - reference station datum |
286 | | struct rtcm2_msg4 { |
287 | | struct { |
288 | | unsigned int parity:6; |
289 | | unsigned int datum_alpha_char2:8; |
290 | | unsigned int datum_alpha_char1:8; |
291 | | unsigned int spare:4; |
292 | | unsigned int dat:1; |
293 | | unsigned int dgnss:3; |
294 | | unsigned int _pad:2; |
295 | | } w3; |
296 | | struct { |
297 | | unsigned int parity:6; |
298 | | unsigned int datum_sub_div_char2:8; |
299 | | unsigned int datum_sub_div_char1:8; |
300 | | unsigned int datum_sub_div_char3:8; |
301 | | unsigned int _pad:2; |
302 | | } w4; |
303 | | struct { |
304 | | unsigned int parity:6; |
305 | | unsigned int dy_h:8; |
306 | | unsigned int dx:16; |
307 | | unsigned int _pad:2; |
308 | | } w5; |
309 | | struct { |
310 | | unsigned int parity:6; |
311 | | unsigned int dz:24; |
312 | | unsigned int dy_l:8; |
313 | | unsigned int _pad:2; |
314 | | } w6; |
315 | | } type4; |
316 | | |
317 | | // msg 5 - constellation health |
318 | | struct rtcm2_msg5 { |
319 | | struct b_health_t { |
320 | | unsigned int parity:6; |
321 | | unsigned int unassigned:2; |
322 | | unsigned int time_unhealthy:4; |
323 | | unsigned int loss_warn:1; |
324 | | unsigned int new_nav_data:1; |
325 | | unsigned int health_enable:1; |
326 | | unsigned int cn0:5; |
327 | | unsigned int data_health:3; |
328 | | unsigned int issue_of_data_link:1; |
329 | | unsigned int sat_id:5; |
330 | | unsigned int reserved:1; |
331 | | unsigned int _pad:2; |
332 | | } health[MAXHEALTH]; |
333 | | } type5; |
334 | | |
335 | | // msg 6 - null message |
336 | | |
337 | | // msg 7 - beacon almanac |
338 | | struct rtcm2_msg7 { |
339 | | struct b_station_t { |
340 | | struct { |
341 | | unsigned int parity:6; |
342 | | int lon_h:8; |
343 | | int lat:16; |
344 | | unsigned int _pad:2; |
345 | | } w3; |
346 | | struct { |
347 | | unsigned int parity:6; |
348 | | unsigned int freq_h:6; |
349 | | unsigned int range:10; |
350 | | unsigned int lon_l:8; |
351 | | unsigned int _pad:2; |
352 | | } w4; |
353 | | struct { |
354 | | unsigned int parity:6; |
355 | | unsigned int encoding:1; |
356 | | unsigned int sync_type:1; |
357 | | unsigned int mod_mode:1; |
358 | | unsigned int bit_rate:3; |
359 | | /* |
360 | | * ITU-R M.823-2 page 9 and RTCM-SC104 v2.1 pages |
361 | | * 4-21 and 4-22 are in conflict over the next two |
362 | | * field sizes. ITU says 9+3, RTCM says 10+2. |
363 | | * The latter correctly decodes the USCG station |
364 | | * id's so I'll use that one here. -wsr |
365 | | */ |
366 | | unsigned int station_id:10; |
367 | | unsigned int health:2; |
368 | | unsigned int freq_l:6; |
369 | | unsigned int _pad:2; |
370 | | } w5; |
371 | | } almanac[32]; |
372 | | } type7; |
373 | | |
374 | | // msg 8 - Pseudolite almanac |
375 | | |
376 | | // msg 9 - GPS Partial correction set |
377 | | |
378 | | // msg 10 - P-code differential corrections |
379 | | |
380 | | // msg 11 - C/A-code L1, L2 delta corrections |
381 | | |
382 | | // msg 12 - Pseudolite station parameters. |
383 | | |
384 | | // msg 13 - Ground Transmitter Parameters (RTCM2.3 only) |
385 | | struct rtcm2_msg13 { |
386 | | struct { |
387 | | unsigned int parity:6; |
388 | | int lat:16; |
389 | | unsigned int reserved:6; |
390 | | unsigned int rangeflag:1; |
391 | | unsigned int status:1; |
392 | | unsigned int _pad:2; |
393 | | } w1; |
394 | | struct { |
395 | | unsigned int parity:6; |
396 | | unsigned int range:8; |
397 | | int lon:16; |
398 | | unsigned int _pad:2; |
399 | | } w2; |
400 | | } type13; |
401 | | |
402 | | // msg 14 - GPS Time of Week (RTCM2.3 only) |
403 | | struct rtcm2_msg14 { |
404 | | struct { |
405 | | unsigned int parity:6; |
406 | | unsigned int leapsecs:6; |
407 | | unsigned int hour:8; |
408 | | unsigned int week:10; |
409 | | unsigned int _pad:2; |
410 | | } w1; |
411 | | } type14; |
412 | | |
413 | | // msg 15 - Ionospheric delay message |
414 | | |
415 | | // msg 16 - text msg |
416 | | struct rtcm2_msg16 { |
417 | | struct { |
418 | | unsigned int parity:6; |
419 | | unsigned int byte3:8; |
420 | | unsigned int byte2:8; |
421 | | unsigned int byte1:8; |
422 | | unsigned int _pad:2; |
423 | | } txt[RTCM2_WORDS_MAX-2]; |
424 | | } type16; |
425 | | |
426 | | // msg 17 - GPS ephmerides. RTCM 2.1 |
427 | | |
428 | | // msg 18 - RTK uncorrected carrier phases. RTCM 2.1 |
429 | | struct rtcm2_msg18 { |
430 | | unsigned int parity:6; |
431 | | unsigned int tom:20; |
432 | | unsigned int r:2; |
433 | | unsigned int f:2; |
434 | | unsigned int _pad:2; |
435 | | struct { |
436 | | unsigned int parity:6; |
437 | | unsigned int cp_h:8; |
438 | | unsigned int clc:5; |
439 | | unsigned int dq:3; |
440 | | unsigned int ident:5; |
441 | | unsigned int g:1; |
442 | | unsigned int pc:1; |
443 | | unsigned int m:1; |
444 | | unsigned int _pad:2; |
445 | | unsigned int parity1:6; |
446 | | unsigned int cp_l:24; |
447 | | unsigned int _pad1:2; |
448 | | } sat[15]; |
449 | | } type18; |
450 | | |
451 | | // msg 19 - RTK uncorrected pseudoranges. RTCM 2.1 |
452 | | struct rtcm2_msg19 { |
453 | | unsigned int parity:6; |
454 | | unsigned int tom:20; |
455 | | unsigned int sm:2; |
456 | | unsigned int f:2; |
457 | | unsigned int _pad:2; |
458 | | struct { |
459 | | unsigned int parity:6; |
460 | | unsigned int pr_h:8; |
461 | | unsigned int me:4; |
462 | | unsigned int dq:4; |
463 | | unsigned int ident:5; |
464 | | unsigned int g:1; |
465 | | unsigned int pc:1; |
466 | | unsigned int m:1; |
467 | | unsigned int _pad:2; |
468 | | unsigned int parity1:6; |
469 | | |
470 | | unsigned int pr_l:24; |
471 | | unsigned int _pad1:2; |
472 | | } sat[15]; |
473 | | } type19; |
474 | | |
475 | | // msg 20 - RTK carrier phase corrections. RTCM 2.1 |
476 | | struct rtcm2_msg20 { |
477 | | unsigned int parity:6; |
478 | | unsigned int tom:20; |
479 | | unsigned int r:2; |
480 | | unsigned int f:2; |
481 | | unsigned int _pad:2; |
482 | | struct { |
483 | | unsigned int parity:6; |
484 | | unsigned int iod:8; |
485 | | unsigned int clc:5; |
486 | | unsigned int dq:3; |
487 | | unsigned int ident:5; |
488 | | unsigned int g:1; |
489 | | unsigned int pc:1; |
490 | | unsigned int m:1; |
491 | | unsigned int _pad:2; |
492 | | unsigned int parity1:6; |
493 | | unsigned int cpc:24; |
494 | | unsigned int _pad1:2; |
495 | | } sat[RTCM2_WORDS_MAX / 2]; |
496 | | } type20; |
497 | | |
498 | | // msg 21 - RTK/high accuracy pseudorange corrections. RTCM 2.1 |
499 | | struct rtcm2_msg21 { |
500 | | unsigned int parity:6; |
501 | | unsigned int tom:20; |
502 | | unsigned int sm:2; |
503 | | unsigned int f:2; |
504 | | unsigned int _pad:2; |
505 | | } type21; |
506 | | |
507 | | // msg 22 - Extended reference station parameters |
508 | | struct rtcm2_msg22 { |
509 | | unsigned int parity:6; |
510 | | int ecef_dz:8; |
511 | | int ecef_dy:8; |
512 | | int ecef_dx:8; |
513 | | unsigned int _pad:2; |
514 | | // word 4 |
515 | | unsigned int parity1:6; |
516 | | unsigned int ah:18; |
517 | | unsigned int nh:1; |
518 | | unsigned int ap:1; |
519 | | unsigned int at:1; |
520 | | unsigned int gs:1; |
521 | | unsigned int res:2; |
522 | | unsigned int _pad1:2; |
523 | | // word 5 |
524 | | unsigned int parity2:6; |
525 | | int ecef_dz2:8; |
526 | | int ecef_dy2:8; |
527 | | int ecef_dx2:8; |
528 | | unsigned int _pad2:2; |
529 | | } type22; |
530 | | |
531 | | // msg 23 - Type of Antenna. RTCM 2.3 |
532 | | struct rtcm2_msg23 { |
533 | | struct { |
534 | | unsigned int parity:6; |
535 | | unsigned char byte2:8; |
536 | | unsigned char byte1:8; |
537 | | unsigned char byte0:8; |
538 | | unsigned int _pad:2; |
539 | | } words[RTCM2_WORDS_MAX - 2]; |
540 | | } type23; |
541 | | |
542 | | // msg 24 - Reference station ARP.. RTCM 2.3 |
543 | | struct rtcm2_msg24 { |
544 | | struct { |
545 | | unsigned int parity:6; |
546 | | unsigned int x_h:24; |
547 | | unsigned int _pad:2; |
548 | | } w3; |
549 | | struct { |
550 | | unsigned int parity:6; |
551 | | unsigned int y_h:8; |
552 | | unsigned int r:2; |
553 | | unsigned int x_l:14; |
554 | | unsigned int _pad:2; |
555 | | } w4; |
556 | | struct { |
557 | | unsigned int parity:6; |
558 | | unsigned int y_m:24; |
559 | | unsigned int _pad:2; |
560 | | } w5; |
561 | | struct { |
562 | | unsigned int parity:6; |
563 | | unsigned int z_h:16; |
564 | | unsigned int r:2; |
565 | | unsigned int y_l:6; |
566 | | unsigned int _pad:2; |
567 | | } w6; |
568 | | struct { |
569 | | unsigned int parity:6; |
570 | | unsigned int ah:1; |
571 | | unsigned int gs:1; |
572 | | unsigned int z_l:22; |
573 | | unsigned int _pad:2; |
574 | | } w7; |
575 | | struct { |
576 | | unsigned int parity:6; |
577 | | unsigned int res:6; |
578 | | unsigned int ah:18; |
579 | | unsigned int _pad:2; |
580 | | } w8; |
581 | | } type24; |
582 | | |
583 | | // msg 27 - Extended almanac of DGPS. |
584 | | |
585 | | // msg 31 - differential GLONASS corrections |
586 | | struct rtcm2_msg31 { |
587 | | struct glonass_correction_t { |
588 | | struct { // msg 1 word 3 |
589 | | unsigned int parity:6; |
590 | | int prc1:16; |
591 | | unsigned int satident1:5; // satellite ID |
592 | | unsigned int udre1:2; |
593 | | unsigned int scale1:1; |
594 | | unsigned int _pad:2; |
595 | | } w3; |
596 | | |
597 | | struct { // msg 1 word 4 |
598 | | unsigned int parity:6; |
599 | | unsigned int satident2:5; // satellite ID |
600 | | unsigned int udre2:2; |
601 | | unsigned int scale2:1; |
602 | | unsigned int tod1:7; |
603 | | unsigned int change1:1; |
604 | | int rrc1:8; |
605 | | unsigned int _pad:2; |
606 | | } w4; |
607 | | |
608 | | struct { // msg 1 word 5 |
609 | | unsigned int parity:6; |
610 | | int rrc2:8; |
611 | | int prc2:16; |
612 | | unsigned int _pad:2; |
613 | | } w5; |
614 | | |
615 | | struct { // msg 1 word 6 |
616 | | unsigned int parity:6; |
617 | | int prc3_h:8; |
618 | | unsigned int satident3:5; // satellite ID |
619 | | unsigned int udre3:2; |
620 | | unsigned int scale3:1; |
621 | | unsigned int tod2:7; |
622 | | unsigned int change2:1; |
623 | | unsigned int _pad:2; |
624 | | } w6; |
625 | | |
626 | | struct { // msg 1 word 7 |
627 | | unsigned int parity:6; |
628 | | unsigned int tod3:7; |
629 | | unsigned int change3:1; |
630 | | int rrc3:8; |
631 | | // NOTE: unsigned int for low byte |
632 | | unsigned int prc3_l:8; |
633 | | unsigned int _pad:2; |
634 | | } w7; |
635 | | } corrections[(RTCM2_WORDS_MAX - 2) / 5]; |
636 | | } type31; |
637 | | |
638 | | // msg 32 - Differential GLONASS reference station parameters, RTCM 2.3 |
639 | | |
640 | | // msg 33 - GLONASS constellation health, RTCM 2.3 |
641 | | |
642 | | // msg 34 - DGLONASS corrections, RTCM 2.3 |
643 | | |
644 | | // msg 35 - GLONASS radiobeacon almanac, RTCM 2.3 |
645 | | |
646 | | // msg 36 - GLONASS special message, RTCM 2.3 |
647 | | |
648 | | // msg 37 - GNSS system time offset, RTCM 2.3 |
649 | | |
650 | | /* msg 59 - Proprietary messages, for transmission of any |
651 | | * required data */ |
652 | | |
653 | | // msg 60-63 - Multipurpose usage |
654 | | |
655 | | // unknown message |
656 | | isgps30bits_t rtcm2_msgunk[RTCM2_WORDS_MAX - 2]; |
657 | | } msg_type; |
658 | | } __attribute__((__packed__)); |
659 | | |
660 | | #endif // LITTLE_ENDIAN |
661 | | |
662 | | #ifdef WORDS_BIGENDIAN |
663 | | |
664 | | struct rtcm2_msg_t { |
665 | | struct rtcm2_msghw1 { // header word 1 |
666 | | unsigned int _pad:2; |
667 | | unsigned int preamble:8; // fixed at 01100110 |
668 | | unsigned int msgtype:6; // RTCM message type |
669 | | unsigned int refstaid:10; // reference station ID |
670 | | unsigned int parity:6; |
671 | | } w1; |
672 | | |
673 | | struct rtcm2_msghw2 { // header word 2 |
674 | | unsigned int _pad:2; |
675 | | unsigned int zcnt:13; |
676 | | unsigned int sqnum:3; |
677 | | unsigned int frmlen:5; |
678 | | unsigned int stathlth:3; // station health |
679 | | unsigned int parity:6; |
680 | | } w2; |
681 | | |
682 | | union { |
683 | | // msg 1 - differential GPS corrections |
684 | | struct rtcm2_msg1 { |
685 | | struct gps_correction_t { |
686 | | struct { // msg 1 word 3 |
687 | | unsigned int _pad:2; |
688 | | unsigned int scale1:1; |
689 | | unsigned int udre1:2; |
690 | | unsigned int satident1:5; // satellite ID |
691 | | int prc1:16; |
692 | | unsigned int parity:6; |
693 | | } w3; |
694 | | |
695 | | struct { // msg 1 word 4 |
696 | | unsigned int _pad:2; |
697 | | int rrc1:8; |
698 | | unsigned int iod1:8; |
699 | | unsigned int scale2:1; |
700 | | unsigned int udre2:2; |
701 | | unsigned int satident2:5; // satellite ID |
702 | | unsigned int parity:6; |
703 | | } w4; |
704 | | |
705 | | struct { // msg 1 word 5 |
706 | | unsigned int _pad:2; |
707 | | int prc2:16; |
708 | | int rrc2:8; |
709 | | unsigned int parity:6; |
710 | | } w5; |
711 | | |
712 | | struct { // msg 1 word 6 |
713 | | unsigned int _pad:2; |
714 | | unsigned int iod2:8; |
715 | | unsigned int scale3:1; |
716 | | unsigned int udre3:2; |
717 | | unsigned int satident3:5; // satellite ID |
718 | | int prc3_h:8; |
719 | | unsigned int parity:6; |
720 | | } w6; |
721 | | |
722 | | struct { // msg 1 word 7 |
723 | | unsigned int _pad:2; |
724 | | unsigned int prc3_l:8; // NOTE: unsigned for low byte |
725 | | int rrc3:8; |
726 | | unsigned int iod3:8; |
727 | | unsigned int parity:6; |
728 | | } w7; |
729 | | } corrections[(RTCM2_WORDS_MAX - 2) / 5]; |
730 | | } type1; |
731 | | |
732 | | /* msg 2 - Pseudo-range corrections, referring to previous orbit |
733 | | * data records (maximum 12 satellites) */ |
734 | | |
735 | | // msg 3 - reference station parameters |
736 | | struct rtcm2_msg3 { |
737 | | struct { |
738 | | unsigned int _pad:2; |
739 | | unsigned int x_h:24; |
740 | | unsigned int parity:6; |
741 | | } w3; |
742 | | struct { |
743 | | unsigned int _pad:2; |
744 | | unsigned int x_l:8; |
745 | | unsigned int y_h:16; |
746 | | unsigned int parity:6; |
747 | | } w4; |
748 | | struct { |
749 | | unsigned int _pad:2; |
750 | | unsigned int y_l:16; |
751 | | unsigned int z_h:8; |
752 | | unsigned int parity:6; |
753 | | } w5; |
754 | | struct { |
755 | | unsigned int _pad:2; |
756 | | unsigned int z_l:24; |
757 | | unsigned int parity:6; |
758 | | } w6; |
759 | | } type3; |
760 | | |
761 | | // msg 4 - reference station datum |
762 | | struct rtcm2_msg4 { |
763 | | struct { |
764 | | unsigned int _pad:2; |
765 | | unsigned int dgnss:3; |
766 | | unsigned int dat:1; |
767 | | unsigned int spare:4; |
768 | | unsigned int datum_alpha_char1:8; |
769 | | unsigned int datum_alpha_char2:8; |
770 | | unsigned int parity:6; |
771 | | } w3; |
772 | | struct { |
773 | | unsigned int _pad:2; |
774 | | unsigned int datum_sub_div_char3:8; |
775 | | unsigned int datum_sub_div_char1:8; |
776 | | unsigned int datum_sub_div_char2:8; |
777 | | unsigned int parity:6; |
778 | | } w4; |
779 | | struct { |
780 | | unsigned int _pad:2; |
781 | | unsigned int dx:16; |
782 | | unsigned int dy_h:8; |
783 | | unsigned int parity:6; |
784 | | } w5; |
785 | | struct { |
786 | | unsigned int _pad:2; |
787 | | unsigned int dy_l:8; |
788 | | unsigned int dz:24; |
789 | | unsigned int parity:6; |
790 | | } w6; |
791 | | } type4; |
792 | | |
793 | | // msg 5 - constellation health |
794 | | struct rtcm2_msg5 { |
795 | | struct b_health_t { |
796 | | unsigned int _pad:2; |
797 | | unsigned int reserved:1; |
798 | | unsigned int sat_id:5; |
799 | | unsigned int issue_of_data_link:1; |
800 | | unsigned int data_health:3; |
801 | | unsigned int cn0:5; |
802 | | unsigned int health_enable:1; |
803 | | unsigned int new_nav_data:1; |
804 | | unsigned int loss_warn:1; |
805 | | unsigned int time_unhealthy:4; |
806 | | unsigned int unassigned:2; |
807 | | unsigned int parity:6; |
808 | | } health[MAXHEALTH]; |
809 | | } type5; |
810 | | |
811 | | // msg - 6 - Null message, used as filler record during time-outs |
812 | | |
813 | | // msg 7 - beacon almanac |
814 | | struct rtcm2_msg7 { |
815 | | struct b_station_t { |
816 | | struct { |
817 | | unsigned int _pad:2; |
818 | | int lat:16; |
819 | | int lon_h:8; |
820 | | unsigned int parity:6; |
821 | | } w3; |
822 | | struct { |
823 | | unsigned int _pad:2; |
824 | | unsigned int lon_l:8; |
825 | | unsigned int range:10; |
826 | | unsigned int freq_h:6; |
827 | | unsigned int parity:6; |
828 | | } w4; |
829 | | struct { |
830 | | unsigned int _pad:2; |
831 | | unsigned int freq_l:6; |
832 | | unsigned int health:2; |
833 | | unsigned int station_id:10; |
834 | | // see comments in LE struct above. |
835 | | unsigned int bit_rate:3; |
836 | | unsigned int mod_mode:1; |
837 | | unsigned int sync_type:1; |
838 | | unsigned int encoding:1; |
839 | | unsigned int parity:6; |
840 | | } w5; |
841 | | } almanac[32]; |
842 | | } type7; |
843 | | |
844 | | // msg 8 - Pseudolite almanac |
845 | | |
846 | | // msg 9 - GPS Partial correction set |
847 | | |
848 | | // msg 10 - P-code differential corrections |
849 | | |
850 | | // msg 11 - C/A-code L1, L2 delta corrections |
851 | | |
852 | | // msg 12 - Pseudolite station parameters. |
853 | | |
854 | | // msg 13 - Ground Transmitter Parameters (RTCM2.3 only) |
855 | | struct rtcm2_msg13 { |
856 | | struct { |
857 | | unsigned int _pad:2; |
858 | | unsigned int status:1; |
859 | | unsigned int rangeflag:1; |
860 | | unsigned int reserved:6; |
861 | | int lat:16; |
862 | | unsigned int parity:6; |
863 | | } w1; |
864 | | struct { |
865 | | unsigned int _pad:2; |
866 | | int lon:16; |
867 | | unsigned int range:8; |
868 | | unsigned int parity:6; |
869 | | } w2; |
870 | | } type13; |
871 | | |
872 | | // msg 14 - GPS Time of Week (RTCM2.3 only) |
873 | | struct rtcm2_msg14 { |
874 | | struct { |
875 | | unsigned int _pad:2; |
876 | | unsigned int week:10; |
877 | | unsigned int hour:8; |
878 | | unsigned int leapsecs:6; |
879 | | unsigned int parity:6; |
880 | | } w1; |
881 | | } type14; |
882 | | |
883 | | // msg 15 - Ionospheric delay message |
884 | | |
885 | | // msg 16 - text msg |
886 | | struct rtcm2_msg16 { |
887 | | struct { |
888 | | unsigned int _pad:2; |
889 | | unsigned int byte1:8; |
890 | | unsigned int byte2:8; |
891 | | unsigned int byte3:8; |
892 | | unsigned int parity:6; |
893 | | } txt[RTCM2_WORDS_MAX-2]; |
894 | | } type16; |
895 | | |
896 | | // msg 17 - GPS ephmerides. RTCM 2.1 |
897 | | |
898 | | // msg 18 - RTK uncorrected carrier phases. RTCM 2.1 |
899 | | struct rtcm2_msg18 { |
900 | | unsigned int _pad:2; |
901 | | unsigned int f:2; |
902 | | unsigned int r:2; |
903 | | unsigned int tom:20; |
904 | | unsigned int parity:6; |
905 | | struct { |
906 | | unsigned int _pad:2; |
907 | | unsigned int m:1; |
908 | | unsigned int pc:1; |
909 | | unsigned int g:1; |
910 | | unsigned int ident:5; |
911 | | unsigned int dq:3; |
912 | | unsigned int clc:5; |
913 | | unsigned int cp_h:8; |
914 | | unsigned int parity:6; |
915 | | unsigned int _pad1:2; |
916 | | unsigned int cp_l:24; |
917 | | unsigned int parity1:6; |
918 | | } sat[15]; |
919 | | } type18; |
920 | | |
921 | | // msg 19 - RTK uncorrected pseudoranges. RTCM 2.1 |
922 | | struct rtcm2_msg19 { |
923 | | unsigned int _pad:2; |
924 | | unsigned int f:2; |
925 | | unsigned int sm:2; |
926 | | unsigned int tom:20; |
927 | | unsigned int parity:6; |
928 | | struct { |
929 | | unsigned int _pad:2; |
930 | | unsigned int m:1; |
931 | | unsigned int pc:1; |
932 | | unsigned int g:1; |
933 | | unsigned int ident:5; |
934 | | unsigned int dq:4; |
935 | | unsigned int me:4; |
936 | | unsigned int pr_h:8; |
937 | | unsigned int parity:6; |
938 | | |
939 | | unsigned int _pad1:2; |
940 | | unsigned int pr_l:24; |
941 | | unsigned int parity1:6; |
942 | | } sat[15]; |
943 | | } type19; |
944 | | |
945 | | // msg 20 - RTK carrier phase corrections. RTCM 2.1 |
946 | | struct rtcm2_msg20 { |
947 | | unsigned int _pad:2; |
948 | | unsigned int f:2; |
949 | | unsigned int r:2; |
950 | | unsigned int tom:20; |
951 | | unsigned int parity:6; |
952 | | struct { |
953 | | unsigned int _pad:2; |
954 | | unsigned int m:1; |
955 | | unsigned int pc:1; |
956 | | unsigned int g:1; |
957 | | unsigned int dq:3; |
958 | | unsigned int ident:5; |
959 | | unsigned int clc:5; |
960 | | unsigned int iod:8; |
961 | | unsigned int parity:6; |
962 | | unsigned int _pad1:2; |
963 | | unsigned int cpc:24; |
964 | | unsigned int parity1:6; |
965 | | } sat[RTCM2_WORDS_MAX / 2]; |
966 | | } type20; |
967 | | |
968 | | // msg 21 - RTK/high accuracy pseudorange corrections. RTCM 2.1 |
969 | | struct rtcm2_msg21 { |
970 | | unsigned int _pad:2; |
971 | | unsigned int f:2; |
972 | | unsigned int sm:2; |
973 | | unsigned int tom:20; |
974 | | unsigned int parity:6; |
975 | | } type21; |
976 | | |
977 | | // msg 22 - Extended reference station parameters |
978 | | struct rtcm2_msg22 { |
979 | | unsigned int _pad:2; |
980 | | int ecef_dx:8; |
981 | | int ecef_dy:8; |
982 | | int ecef_dz:8; |
983 | | unsigned int parity:6; |
984 | | // word 4 |
985 | | unsigned int _pad1:2; |
986 | | unsigned int res:2; |
987 | | unsigned int gs:1; |
988 | | unsigned int at:1; |
989 | | unsigned int ap:1; |
990 | | unsigned int nh:1; |
991 | | unsigned int ah:18; |
992 | | unsigned int parity1:6; |
993 | | // word 5 |
994 | | unsigned int _pad2:2; |
995 | | int ecef_dx2:8; |
996 | | int ecef_dy2:8; |
997 | | int ecef_dz2:8; |
998 | | unsigned int parity2:6; |
999 | | } type22; |
1000 | | |
1001 | | // msg 23 - Type of Antenna. RTCM 2.3 |
1002 | | struct rtcm2_msg23 { |
1003 | | struct { |
1004 | | unsigned int _pad:2; |
1005 | | unsigned char byte0:8; |
1006 | | unsigned char byte1:8; |
1007 | | unsigned char byte2:8; |
1008 | | unsigned int parity:6; |
1009 | | } words[RTCM2_WORDS_MAX - 2]; |
1010 | | } type23; |
1011 | | |
1012 | | // msg 24 - Reference station ARP.. RTCM 2.3 |
1013 | | struct rtcm2_msg24 { |
1014 | | struct { |
1015 | | unsigned int _pad:2; |
1016 | | unsigned int x_h:24; |
1017 | | unsigned int parity:6; |
1018 | | } w3; |
1019 | | struct { |
1020 | | unsigned int _pad:2; |
1021 | | unsigned int x_l:14; |
1022 | | unsigned int r:2; |
1023 | | unsigned int y_h:8; |
1024 | | unsigned int parity:6; |
1025 | | } w4; |
1026 | | struct { |
1027 | | unsigned int _pad:2; |
1028 | | unsigned int y_m:24; |
1029 | | unsigned int parity:6; |
1030 | | } w5; |
1031 | | struct { |
1032 | | unsigned int _pad:2; |
1033 | | unsigned int y_l:6; |
1034 | | unsigned int r:2; |
1035 | | unsigned int z_h:16; |
1036 | | unsigned int parity:6; |
1037 | | } w6; |
1038 | | struct { |
1039 | | unsigned int _pad:2; |
1040 | | unsigned int z_l:22; |
1041 | | unsigned int gs:1; |
1042 | | unsigned int ah:1; |
1043 | | unsigned int parity:6; |
1044 | | } w7; |
1045 | | struct { |
1046 | | unsigned int _pad:2; |
1047 | | unsigned int ah:18; |
1048 | | unsigned int res:6; |
1049 | | unsigned int parity:6; |
1050 | | } w8; |
1051 | | } type24; |
1052 | | |
1053 | | // msg 27 - Extended almanac of DGPS. |
1054 | | |
1055 | | // msg 31 - differential GLONASS corrections |
1056 | | struct rtcm2_msg31 { |
1057 | | struct glonass_correction_t { |
1058 | | struct { // msg 1 word 3 |
1059 | | unsigned int _pad:2; |
1060 | | unsigned int scale1:1; |
1061 | | unsigned int udre1:2; |
1062 | | unsigned int satident1:5; // satellite ID |
1063 | | int prc1:16; |
1064 | | unsigned int parity:6; |
1065 | | } w3; |
1066 | | |
1067 | | struct { // msg 1 word 4 |
1068 | | unsigned int _pad:2; |
1069 | | int rrc1:8; |
1070 | | unsigned int change1:1; |
1071 | | unsigned int tod1:7; |
1072 | | unsigned int scale2:1; |
1073 | | unsigned int udre2:2; |
1074 | | unsigned int satident2:5; // satellite ID |
1075 | | unsigned int parity:6; |
1076 | | } w4; |
1077 | | |
1078 | | struct { // msg 1 word 5 |
1079 | | unsigned int _pad:2; |
1080 | | int prc2:16; |
1081 | | int rrc2:8; |
1082 | | unsigned int parity:6; |
1083 | | } w5; |
1084 | | |
1085 | | struct { // msg 1 word 6 |
1086 | | unsigned int _pad:2; |
1087 | | unsigned int change2:1; |
1088 | | unsigned int tod2:7; |
1089 | | unsigned int scale3:1; |
1090 | | unsigned int udre3:2; |
1091 | | unsigned int satident3:5; // satellite ID |
1092 | | int prc3_h:8; |
1093 | | unsigned int parity:6; |
1094 | | } w6; |
1095 | | |
1096 | | struct { // msg 1 word 7 |
1097 | | unsigned int _pad:2; |
1098 | | unsigned int prc3_l:8; // NOTE: unsigned for low byte |
1099 | | int rrc3:8; |
1100 | | unsigned int change3:1; |
1101 | | unsigned int tod3:7; |
1102 | | unsigned int parity:6; |
1103 | | } w7; |
1104 | | } corrections[(RTCM2_WORDS_MAX - 2) / 5]; |
1105 | | } type31; |
1106 | | |
1107 | | // msg 32 - Differential GLONASS reference station parameters, RTCM 2.3 |
1108 | | |
1109 | | // msg 33 - GLONASS constellation health, RTCM 2.3 |
1110 | | |
1111 | | // msg 34 - DGLONASS corrections, RTCM 2.3 |
1112 | | |
1113 | | // msg 35 - GLONASS radiobeacon almanac, RTCM 2.3 |
1114 | | |
1115 | | // msg 36 - GLONASS special message, RTCM 2.3 |
1116 | | |
1117 | | // msg 37 - GNSS system time offset, RTCM 2.3 |
1118 | | |
1119 | | /* msg 59 - Proprietary messages, for transmission of any |
1120 | | * required data */ |
1121 | | |
1122 | | // msg 60-63 - Multipurpsoe usage |
1123 | | |
1124 | | // unknown message |
1125 | | isgps30bits_t rtcm2_msgunk[RTCM2_WORDS_MAX-2]; |
1126 | | } msg_type; |
1127 | | } __attribute__((__packed__)); |
1128 | | |
1129 | | #endif // BIG ENDIAN |
1130 | | |
1131 | 466M | #define PREAMBLE_PATTERN 0x66 |
1132 | | |
1133 | | static unsigned int tx_speed[] = { 25, 50, 100, 110, 150, 200, 250, 300 }; |
1134 | | |
1135 | | #define DIMENSION(a) (unsigned)(sizeof(a)/sizeof(a[0])) |
1136 | | |
1137 | | // break out the raw bits into the content fields |
1138 | | void rtcm2_unpack(struct gps_device_t *session, struct rtcm2_t *tp, char *buf) |
1139 | 11.4k | { |
1140 | 11.4k | unsigned len; |
1141 | 11.4k | unsigned n, w; |
1142 | 11.4k | struct rtcm2_msg_t *msg = (struct rtcm2_msg_t *)buf; |
1143 | 11.4k | bool unknown = true; // we don't know how to decode |
1144 | 11.4k | const char *msg_name = NULL; // no decode, but maybe we know the name |
1145 | | |
1146 | 11.4k | tp->type = msg->w1.msgtype; // 1 to 64. Zero means 64 |
1147 | 11.4k | tp->refstaid = msg->w1.refstaid; // 0 to 1023 |
1148 | 11.4k | tp->length = msg->w2.frmlen; // 0 to 31 |
1149 | 11.4k | tp->zcount = msg->w2.zcnt * ZCOUNT_SCALE; // 0 to 3599.4 sec |
1150 | 11.4k | tp->seqnum = msg->w2.sqnum; // 0 to 7 |
1151 | 11.4k | tp->stathlth = msg->w2.stathlth; |
1152 | | |
1153 | 11.4k | if (31 < tp->length) { |
1154 | 0 | GPSD_LOG(LOG_ERROR, &session->context->errout, |
1155 | 0 | "RTCM2: tp->tplength too long %dd\n", |
1156 | 0 | tp->length); |
1157 | 0 | return; |
1158 | 0 | } |
1159 | | // clear rtk struct |
1160 | 11.4k | memset(&tp->rtk, 0, sizeof(tp->rtk)); |
1161 | | |
1162 | | // clear ref_sta struct |
1163 | 11.4k | memset(&tp->ref_sta, 0, sizeof(tp->ref_sta)); |
1164 | 11.4k | tp->ref_sta.x = NAN; |
1165 | 11.4k | tp->ref_sta.y = NAN; |
1166 | 11.4k | tp->ref_sta.z = NAN; |
1167 | 11.4k | tp->ref_sta.dx = NAN; |
1168 | 11.4k | tp->ref_sta.dy = NAN; |
1169 | 11.4k | tp->ref_sta.dz = NAN; |
1170 | 11.4k | tp->ref_sta.ah = NAN; |
1171 | 11.4k | tp->ref_sta.dx2 = NAN; |
1172 | 11.4k | tp->ref_sta.dy2 = NAN; |
1173 | 11.4k | tp->ref_sta.dz2 = NAN; |
1174 | | |
1175 | | // FIXME: test tp->length + 2, against session->lexer.isgps.buflen, |
1176 | | |
1177 | | // len does not include 2 byte header |
1178 | 11.4k | len = tp->length; |
1179 | 11.4k | n = 0; |
1180 | 11.4k | switch (tp->type) { |
1181 | 505 | case 1: |
1182 | 505 | FALLTHROUGH |
1183 | 782 | case 9: |
1184 | 782 | { |
1185 | 782 | int lens = (int)len; |
1186 | 782 | struct gps_correction_t *m = &msg->msg_type.type1.corrections[0]; |
1187 | 782 | msg_name = "Differential GPS Corrections"; |
1188 | 782 | unknown = false; |
1189 | | |
1190 | 1.90k | while (0 <= lens) { |
1191 | 1.12k | if (2 <= lens) { |
1192 | 614 | tp->gps_ranges.sat[n].ident = m->w3.satident1; |
1193 | 614 | tp->gps_ranges.sat[n].udre = m->w3.udre1; |
1194 | 614 | tp->gps_ranges.sat[n].iod = m->w4.iod1; |
1195 | 614 | tp->gps_ranges.sat[n].prc = m->w3.prc1 * |
1196 | 614 | (m->w3.scale1 ? PRCLARGE : PRCSMALL); |
1197 | 614 | tp->gps_ranges.sat[n].rrc = m->w4.rrc1 * |
1198 | 614 | (m->w3.scale1 ? RRLARGE : RRSMALL); |
1199 | 614 | n++; |
1200 | 614 | } |
1201 | 1.12k | if (4 <= lens) { |
1202 | 481 | tp->gps_ranges.sat[n].ident = m->w4.satident2; |
1203 | 481 | tp->gps_ranges.sat[n].udre = m->w4.udre2; |
1204 | 481 | tp->gps_ranges.sat[n].iod = m->w6.iod2; |
1205 | 481 | tp->gps_ranges.sat[n].prc = m->w5.prc2 * |
1206 | 481 | (m->w4.scale2 ? PRCLARGE : PRCSMALL); |
1207 | 481 | tp->gps_ranges.sat[n].rrc = m->w5.rrc2 * |
1208 | 481 | (m->w4.scale2 ? RRLARGE : RRSMALL); |
1209 | 481 | n++; |
1210 | 481 | } |
1211 | 1.12k | if (5 <= lens) { |
1212 | 338 | tp->gps_ranges.sat[n].ident = m->w6.satident3; |
1213 | 338 | tp->gps_ranges.sat[n].udre = m->w6.udre3; |
1214 | 338 | tp->gps_ranges.sat[n].iod = m->w7.iod3; |
1215 | 338 | tp->gps_ranges.sat[n].prc = |
1216 | 338 | ((m->w6.prc3_h << 8) | (m->w7.prc3_l)) * |
1217 | 338 | (m->w6.scale3 ? PRCLARGE : PRCSMALL); |
1218 | 338 | tp->gps_ranges.sat[n].rrc = |
1219 | 338 | m->w7.rrc3 * (m->w6.scale3 ? RRLARGE : RRSMALL); |
1220 | 338 | n++; |
1221 | 338 | } |
1222 | 1.12k | lens -= 5; |
1223 | 1.12k | m++; |
1224 | 1.12k | } |
1225 | 782 | tp->gps_ranges.nentries = n; |
1226 | 782 | } |
1227 | 782 | break; |
1228 | | |
1229 | 134 | case 2: |
1230 | 134 | msg_name = "Delta Differential GPS Corrections"; |
1231 | 134 | break; |
1232 | | |
1233 | 264 | case 3: |
1234 | 264 | { |
1235 | 264 | struct rtcm2_msg3 *m = &msg->msg_type.type3; |
1236 | 264 | msg_name = "Reference Station Parameters (GPS)"; |
1237 | 264 | unknown = false; |
1238 | | |
1239 | 264 | if (4 <= len) { |
1240 | 132 | tp->ref_sta.valid = true; |
1241 | 132 | tp->ref_sta.x = ((m->w3.x_h << 8) | (m->w4.x_l)) * XYZ_SCALE; |
1242 | 132 | tp->ref_sta.y = ((m->w4.y_h << 16) | (m->w5.y_l)) * XYZ_SCALE; |
1243 | 132 | tp->ref_sta.z = ((m->w5.z_h << 24) | (m->w6.z_l)) * XYZ_SCALE; |
1244 | 132 | } |
1245 | 264 | } |
1246 | 264 | break; |
1247 | | |
1248 | 785 | case 4: |
1249 | 785 | msg_name = "Reference Station Datum"; |
1250 | 785 | unknown = false; |
1251 | 785 | if (2 <= len) { |
1252 | 648 | struct rtcm2_msg4 *m = &msg->msg_type.type4; |
1253 | 648 | tp->reference.valid = true; |
1254 | | |
1255 | 648 | tp->reference.system = |
1256 | 648 | (m->w3.dgnss == 0) ? NAVSYSTEM_GPS : |
1257 | 648 | ((m->w3.dgnss == 1) ? NAVSYSTEM_GLONASS : NAVSYSTEM_UNKNOWN); |
1258 | 648 | tp->reference.sense = |
1259 | 648 | (m->w3.dat != 0) ? SENSE_GLOBAL : SENSE_LOCAL; |
1260 | 648 | if (m->w3.datum_alpha_char1) { |
1261 | 317 | tp->reference.datum[n++] = (char)(m->w3.datum_alpha_char1); |
1262 | 317 | } |
1263 | 648 | if (m->w3.datum_alpha_char2) { |
1264 | 240 | tp->reference.datum[n++] = (char)(m->w3.datum_alpha_char2); |
1265 | 240 | } |
1266 | 648 | if (m->w4.datum_sub_div_char1) { |
1267 | 266 | tp->reference.datum[n++] = (char)(m->w4.datum_sub_div_char1); |
1268 | 266 | } |
1269 | 648 | if (m->w4.datum_sub_div_char2) { |
1270 | 241 | tp->reference.datum[n++] = (char)(m->w4.datum_sub_div_char2); |
1271 | 241 | } |
1272 | 648 | if (m->w4.datum_sub_div_char3) { |
1273 | 152 | tp->reference.datum[n++] = (char)(m->w4.datum_sub_div_char3); |
1274 | 152 | } |
1275 | | // we used to say n++ here, but scan-build complains |
1276 | 648 | tp->reference.datum[n] = '\0'; |
1277 | 648 | if (4 <= len) { |
1278 | 134 | tp->reference.dx = m->w5.dx * DXYZ_SCALE; |
1279 | 134 | tp->reference.dy = |
1280 | 134 | ((m->w5.dy_h << 8) | m->w6.dy_l) * DXYZ_SCALE; |
1281 | 134 | tp->reference.dz = m->w6.dz * DXYZ_SCALE; |
1282 | 134 | } else |
1283 | 514 | tp->reference.sense = SENSE_INVALID; |
1284 | 648 | } |
1285 | 785 | break; |
1286 | | |
1287 | 489 | case 5: |
1288 | 489 | msg_name = "Constellation health (GPS)"; |
1289 | 489 | unknown = false; |
1290 | 1.55k | for (n = 0; n < len; n++) { |
1291 | 1.07k | struct consat_t *csp = &tp->conhealth.sat[n]; |
1292 | 1.07k | struct b_health_t *m = &msg->msg_type.type5.health[n]; |
1293 | | |
1294 | 1.07k | csp->ident = m->sat_id; |
1295 | 1.07k | csp->iodl = m->issue_of_data_link != 0; |
1296 | 1.07k | csp->health = m->data_health; |
1297 | 1.07k | csp->snr = (int)(m->cn0 ? (m->cn0 + CNR_OFFSET) : SNR_BAD); |
1298 | 1.07k | csp->health_en = m->health_enable != 0; |
1299 | 1.07k | csp->new_data = m->new_nav_data != 0; |
1300 | 1.07k | csp->los_warning = m->loss_warn != 0; |
1301 | 1.07k | csp->tou = m->time_unhealthy * TU_SCALE; |
1302 | 1.07k | } |
1303 | 489 | tp->conhealth.nentries = n; |
1304 | 489 | break; |
1305 | | |
1306 | 158 | case 6: |
1307 | 158 | msg_name = "Null frame (GPS)"; |
1308 | 158 | unknown = false; |
1309 | 158 | break; |
1310 | | |
1311 | 611 | case 7: |
1312 | 611 | msg_name = "Beacon Almanac (GPS)"; |
1313 | 611 | unknown = false; |
1314 | 611 | if (ROWS(msg->msg_type.type7.almanac) <= len) { |
1315 | | // Coverity 645086 Out-of-bounds write |
1316 | 0 | GPSD_LOG(LOG_PROG, &session->context->errout, |
1317 | 0 | "RTCM2: type 7 bad len %u >= %zu\n", |
1318 | 0 | len, ROWS(msg->msg_type.type7.almanac)); |
1319 | 0 | break; |
1320 | 0 | } |
1321 | 1.81k | for (w = 0; w < len; w++) { |
1322 | 1.20k | struct station_t *np = &tp->almanac.station[w]; |
1323 | 1.20k | struct b_station_t *mp = &msg->msg_type.type7.almanac[w]; |
1324 | | |
1325 | 1.20k | np->latitude = mp->w3.lat * LA_SCALE; |
1326 | 1.20k | np->longitude = ((mp->w3.lon_h << 8) | mp->w4.lon_l) * LO_SCALE; |
1327 | 1.20k | np->range = mp->w4.range; |
1328 | 1.20k | np->frequency = |
1329 | 1.20k | (((mp->w4.freq_h << 6) | mp->w5.freq_l) * FREQ_SCALE) + |
1330 | 1.20k | FREQ_OFFSET; |
1331 | 1.20k | np->health = mp->w5.health; |
1332 | 1.20k | np->station_id = mp->w5.station_id, |
1333 | 1.20k | np->bitrate = tx_speed[mp->w5.bit_rate]; |
1334 | 1.20k | } |
1335 | 611 | tp->almanac.nentries = len / 3; |
1336 | 611 | break; |
1337 | | |
1338 | 142 | case 8: |
1339 | 142 | msg_name = "Pseudolite Almanac"; |
1340 | 142 | break; |
1341 | | |
1342 | | // case 9 is handled above with case 1 |
1343 | | |
1344 | 141 | case 10: |
1345 | 141 | msg_name = "P-Code Differential Corrections"; |
1346 | 141 | break; |
1347 | | |
1348 | 224 | case 11: |
1349 | 224 | msg_name = "C/A Code L2 Corrections"; |
1350 | 224 | break; |
1351 | | |
1352 | 132 | case 12: |
1353 | 132 | msg_name = "Pseudolite Station Parameters"; |
1354 | 132 | break; |
1355 | | |
1356 | 404 | case 13: |
1357 | 404 | msg_name = "Ground Transmitter Parameters"; |
1358 | 404 | unknown = false; |
1359 | 404 | tp->xmitter.status = (bool)msg->msg_type.type13.w1.status; |
1360 | 404 | tp->xmitter.rangeflag = (bool)msg->msg_type.type13.w1.rangeflag; |
1361 | 404 | tp->xmitter.lat = msg->msg_type.type13.w1.lat * LATLON_SCALE; |
1362 | 404 | tp->xmitter.lon = msg->msg_type.type13.w2.lon * LATLON_SCALE; |
1363 | 404 | tp->xmitter.range = msg->msg_type.type13.w2.range * RANGE_SCALE; |
1364 | 404 | if (0 == tp->xmitter.range) { |
1365 | 272 | tp->xmitter.range = 1024; |
1366 | 272 | } |
1367 | 404 | break; |
1368 | | |
1369 | 133 | case 14: |
1370 | 133 | msg_name = "GPS Time Of Week"; |
1371 | 133 | unknown = false; |
1372 | 133 | tp->gpstime.week = msg->msg_type.type14.w1.week; |
1373 | 133 | tp->gpstime.hour = msg->msg_type.type14.w1.hour; |
1374 | 133 | tp->gpstime.leapsecs = msg->msg_type.type14.w1.leapsecs; |
1375 | 133 | break; |
1376 | | |
1377 | 194 | case 15: |
1378 | 194 | msg_name = "Ionospheric Delay Message"; |
1379 | 194 | break; |
1380 | | |
1381 | 909 | case 16: |
1382 | 909 | msg_name = "Special Message (GPS)"; |
1383 | 909 | unknown = false; |
1384 | 1.38k | for (w = 0; w < len; w++) { |
1385 | 891 | if (!msg->msg_type.type16.txt[w].byte1) { |
1386 | 138 | break; |
1387 | 138 | } |
1388 | 753 | tp->message[n++] = (char)(msg->msg_type.type16.txt[w].byte1); |
1389 | 753 | if (!msg->msg_type.type16.txt[w].byte2) { |
1390 | 132 | break; |
1391 | 132 | } |
1392 | 621 | tp->message[n++] = (char)(msg->msg_type.type16.txt[w].byte2); |
1393 | 621 | if (!msg->msg_type.type16.txt[w].byte3) { |
1394 | 141 | break; |
1395 | 141 | } |
1396 | 480 | tp->message[n++] = (char)(msg->msg_type.type16.txt[w].byte3); |
1397 | 480 | } |
1398 | 909 | tp->message[n] = '\0'; |
1399 | 909 | break; |
1400 | | |
1401 | 149 | case 17: |
1402 | 149 | msg_name = "GPS Ephemeris"; |
1403 | 149 | break; |
1404 | | |
1405 | 494 | case 18: |
1406 | 494 | msg_name = "RTK Uncorrected Carrier-phase"; |
1407 | 494 | unknown = false; |
1408 | | // WIP: partial decode |
1409 | 494 | if (1 > len) { |
1410 | | // too short |
1411 | 138 | break; |
1412 | 138 | } |
1413 | 356 | { |
1414 | 356 | struct rtcm2_msg18 *m = &msg->msg_type.type18; |
1415 | 356 | tp->rtk.nentries = (len - 1) / 2; |
1416 | 356 | unsigned i; |
1417 | | |
1418 | 356 | tp->rtk.tom = m->tom; |
1419 | 356 | tp->rtk.f = m->f; |
1420 | 1.25k | for (i = 0; i < tp->rtk.nentries; i++) { |
1421 | 896 | tp->rtk.sat[i].m = m->sat[i].m; |
1422 | 896 | tp->rtk.sat[i].pc = m->sat[i].pc; |
1423 | 896 | tp->rtk.sat[i].g = m->sat[i].g; |
1424 | 896 | tp->rtk.sat[i].ident = m->sat[i].ident; |
1425 | 896 | tp->rtk.sat[i].dq = m->sat[i].dq; |
1426 | 896 | tp->rtk.sat[i].carrier_phase = m->sat[i].cp_l; |
1427 | 896 | tp->rtk.sat[i].carrier_phase |= m->sat[i].cp_h << 24; |
1428 | 896 | tp->rtk.sat[i].clc = m->sat[i].clc; |
1429 | 896 | } |
1430 | 356 | } |
1431 | 356 | break; |
1432 | | |
1433 | 489 | case 19: |
1434 | 489 | msg_name = "RTK Corrected Pseudorange"; |
1435 | | // WIP: partial decode |
1436 | 489 | if (1 > len) { |
1437 | | // too short |
1438 | 133 | break; |
1439 | 133 | } |
1440 | 356 | { |
1441 | 356 | struct rtcm2_msg19 *m = &msg->msg_type.type19; |
1442 | 356 | unsigned i; |
1443 | 356 | tp->rtk.tom = m->tom; |
1444 | 356 | tp->rtk.f = m->f; |
1445 | 356 | tp->rtk.sm = m->sm; |
1446 | 356 | tp->rtk.nentries = (len - 1) / 2; |
1447 | | |
1448 | 917 | for (i = 0; i < tp->rtk.nentries; i++) { |
1449 | 561 | tp->rtk.sat[i].m = m->sat[i].m; |
1450 | 561 | tp->rtk.sat[i].pc = m->sat[i].pc; |
1451 | 561 | tp->rtk.sat[i].g = m->sat[i].g; |
1452 | 561 | tp->rtk.sat[i].ident = m->sat[i].ident; |
1453 | 561 | tp->rtk.sat[i].dq = m->sat[i].dq; |
1454 | 561 | tp->rtk.sat[i].pseudorange = m->sat[i].pr_l; |
1455 | 561 | tp->rtk.sat[i].pseudorange |= m->sat[i].pr_h << 24; |
1456 | 561 | } |
1457 | 356 | } |
1458 | 356 | break; |
1459 | | |
1460 | 434 | case 20: |
1461 | 434 | msg_name = "RTK Carrier Phase Corrections"; |
1462 | | // WIP: partial decode |
1463 | 434 | if (3 > len) { |
1464 | | // too short |
1465 | 297 | break; |
1466 | 297 | } |
1467 | 137 | { |
1468 | 137 | struct rtcm2_msg20 *m = &msg->msg_type.type20; |
1469 | 137 | tp->rtk.tom = m->tom; |
1470 | 137 | tp->rtk.f = m->f; |
1471 | 137 | } |
1472 | 137 | break; |
1473 | | |
1474 | 269 | case 21: |
1475 | 269 | msg_name = "High-Accuracy Pseudorange Corrections"; |
1476 | | // WIP: partial decode |
1477 | 269 | if (3 > len) { |
1478 | | // too short |
1479 | 137 | break; |
1480 | 137 | } |
1481 | 132 | { |
1482 | 132 | struct rtcm2_msg21 *m = &msg->msg_type.type21; |
1483 | 132 | tp->rtk.tom = m->tom; |
1484 | 132 | tp->rtk.f = m->f; |
1485 | 132 | tp->rtk.sm = m->sm; |
1486 | 132 | } |
1487 | 132 | break; |
1488 | | |
1489 | 639 | case 22: |
1490 | | // Extends types 3 and 34 |
1491 | 639 | msg_name = "Extended Reference Station Parameters"; |
1492 | 639 | unknown = false; |
1493 | | // WIP: partial decode |
1494 | 639 | if (3 > len) { |
1495 | | // too short |
1496 | 211 | break; |
1497 | 211 | } |
1498 | | // May be length 3, 4 or 5! |
1499 | 428 | { |
1500 | 428 | struct rtcm2_msg22 *m = &msg->msg_type.type22; |
1501 | | |
1502 | 428 | tp->ref_sta.dx = m->ecef_dx * EDXYZ_SCALE; |
1503 | 428 | tp->ref_sta.dy = m->ecef_dy * EDXYZ_SCALE; |
1504 | 428 | tp->ref_sta.dz = m->ecef_dz * EDXYZ_SCALE; |
1505 | 428 | if (3 < len) { |
1506 | | // word 4 present |
1507 | 344 | tp->ref_sta.gs = m->gs; |
1508 | 344 | if (0 == m->nh) { |
1509 | 192 | tp->ref_sta.ah = m->ah * EDXYZ_SCALE; |
1510 | 192 | } |
1511 | 344 | } |
1512 | 428 | if (4 < len) { |
1513 | | // word 5 present |
1514 | 194 | tp->ref_sta.dx2 = m->ecef_dx2 * EDXYZ2_SCALE; |
1515 | 194 | tp->ref_sta.dy2 = m->ecef_dy2 * EDXYZ2_SCALE; |
1516 | 194 | tp->ref_sta.dz2 = m->ecef_dz2 * EDXYZ2_SCALE; |
1517 | 194 | } |
1518 | 428 | } |
1519 | 428 | break; |
1520 | | |
1521 | 457 | case 23: |
1522 | 457 | msg_name = "Antenna Type Definition"; |
1523 | | // WIP |
1524 | 457 | unknown = false; |
1525 | 457 | if (1 > len) { |
1526 | | // too short |
1527 | 158 | break; |
1528 | 158 | } |
1529 | 299 | { |
1530 | 299 | unsigned sf; |
1531 | 299 | unsigned nad = 0, nas = 0; |
1532 | 299 | unsigned char tbuf[RTCM2_WORDS_MAX * 3]; |
1533 | 299 | struct rtcm2_msg23 *m = &msg->msg_type.type23; |
1534 | 299 | unsigned i = 0, j = 0; |
1535 | | |
1536 | 299 | tp->ref_sta.ar = (m->words[0].byte0 >> 6) & 1; |
1537 | | // sf, serial flag. serial number to follow? |
1538 | 299 | sf = (m->words[0].byte0 >> 5) & 1; |
1539 | | // nad, number of characters for antenna description, is 0 to 31 |
1540 | 299 | nad = m->words[0].byte0 & 0x1f; |
1541 | | |
1542 | 299 | if (sizeof(tbuf) < ((size_t)len * 3)) { |
1543 | 0 | GPSD_LOG(LOG_ERROR, &session->context->errout, |
1544 | 0 | "RTCM2: len too long, len %u nad %d\n", |
1545 | 0 | len, nad); |
1546 | 0 | break; |
1547 | 0 | } |
1548 | | // crazy packing... move to simple array first |
1549 | 299 | memset(tbuf, 0, sizeof(tbuf)); // avoid a CVE |
1550 | 881 | for (i = 0; i < len; i++) { |
1551 | 582 | tbuf[j++] = m->words[i].byte0; |
1552 | 582 | tbuf[j++] = m->words[i].byte1; |
1553 | 582 | tbuf[j++] = m->words[i].byte2; |
1554 | 582 | } |
1555 | | #ifdef __UNUSED__ |
1556 | | // debug code |
1557 | | { |
1558 | | char tmpbuf[100]; |
1559 | | GPSD_LOG(LOG_SHOUT, &session->context->errout, |
1560 | | "RTCM2: len %u nad %d tbuf %s\n", |
1561 | | len, nad, |
1562 | | gps_hexdump(tmpbuf, sizeof(tmpbuf), |
1563 | | (char *)tbuf, len * 3)); |
1564 | | } |
1565 | | #endif // __UNUSED__ |
1566 | | // skip first byte (AR, SF, NAD) |
1567 | 299 | memcpy(tp->ref_sta.ant_desc, &tbuf[1], nad); |
1568 | 299 | tp->ref_sta.ant_desc[nad] = '\0'; |
1569 | | |
1570 | 299 | tp->ref_sta.setup_id = tbuf[nad + 1]; |
1571 | | |
1572 | | // get serial number |
1573 | 299 | if (1 == sf) { |
1574 | | // nas is 0 to 31. just after last ant desc byte |
1575 | 133 | nas = tbuf[nad + 2] & 0x1f; |
1576 | 133 | memcpy(tp->ref_sta.ant_serial, &tbuf[nad + 3], nas); |
1577 | 133 | } |
1578 | 299 | tp->ref_sta.ant_serial[nas] = '\0'; |
1579 | 299 | } |
1580 | 0 | break; |
1581 | | |
1582 | 267 | case 24: |
1583 | 267 | msg_name = "Antenna Reference Point (arp)"; |
1584 | | // WIP |
1585 | 267 | unknown = false; |
1586 | 267 | { |
1587 | 267 | struct rtcm2_msg24 *m = &msg->msg_type.type24; |
1588 | | // try to prevent int overflow |
1589 | 267 | long long temp; |
1590 | | |
1591 | 267 | temp = ((long long)m->w3.x_h << 8) | (m->w4.x_l); |
1592 | 267 | tp->ref_sta.x = temp * EXYZ_SCALE; |
1593 | 267 | temp = ((long long)m->w4.y_h << 32) | |
1594 | 267 | (m->w5.y_m << 6) | |
1595 | 267 | (m->w6.y_l); |
1596 | 267 | tp->ref_sta.y = temp * EXYZ_SCALE; |
1597 | 267 | temp = ((long long)m->w6.z_h << 22) | (m->w7.z_l); |
1598 | 267 | tp->ref_sta.z = temp * EXYZ_SCALE; |
1599 | 267 | if (0 == m->w7.ah) { |
1600 | 135 | tp->ref_sta.ah = m->w8.ah * EDXYZ_SCALE; |
1601 | 135 | } |
1602 | 267 | } |
1603 | 267 | break; |
1604 | | |
1605 | 202 | case 25: |
1606 | 202 | FALLTHROUGH |
1607 | 334 | case 26: |
1608 | 334 | msg_name = "Undefined"; |
1609 | 334 | break; |
1610 | | |
1611 | 132 | case 27: |
1612 | 132 | msg_name = "Radio Beacon Almanac"; |
1613 | 132 | break; |
1614 | | |
1615 | 203 | case 28: |
1616 | 203 | FALLTHROUGH |
1617 | 364 | case 29: |
1618 | 364 | FALLTHROUGH |
1619 | 499 | case 30: |
1620 | 499 | msg_name = "Undefined"; |
1621 | 499 | break; |
1622 | | |
1623 | 135 | case 31: |
1624 | 135 | FALLTHROUGH |
1625 | 593 | case 34: |
1626 | 593 | { |
1627 | 593 | int lens = (int)len; |
1628 | | // 32 and 34 are the same, except 31 is all sats, 34 is some sats |
1629 | 593 | struct glonass_correction_t *m = &msg->msg_type.type31.corrections[0]; |
1630 | 593 | msg_name = "Differential GLONASS corrections"; |
1631 | 593 | unknown = false; |
1632 | | |
1633 | 1.44k | while (0 <= lens) { |
1634 | 854 | if (2 <= lens) { |
1635 | 560 | tp->glonass_ranges.sat[n].ident = m->w3.satident1; |
1636 | 560 | tp->glonass_ranges.sat[n].udre = m->w3.udre1; |
1637 | 560 | tp->glonass_ranges.sat[n].change = (bool)m->w4.change1; |
1638 | 560 | tp->glonass_ranges.sat[n].tod = m->w4.tod1; |
1639 | 560 | tp->glonass_ranges.sat[n].prc = m->w3.prc1 * |
1640 | 560 | (m->w3.scale1 ? PRCLARGE : PRCSMALL); |
1641 | 560 | tp->glonass_ranges.sat[n].rrc = m->w4.rrc1 * |
1642 | 560 | (m->w3.scale1 ? RRLARGE : RRSMALL); |
1643 | 560 | n++; |
1644 | 560 | } |
1645 | 854 | if (4 <= lens) { |
1646 | 361 | tp->glonass_ranges.sat[n].ident = m->w4.satident2; |
1647 | 361 | tp->glonass_ranges.sat[n].udre = m->w4.udre2; |
1648 | 361 | tp->glonass_ranges.sat[n].change = (bool)m->w6.change2; |
1649 | 361 | tp->glonass_ranges.sat[n].tod = m->w6.tod2; |
1650 | 361 | tp->glonass_ranges.sat[n].prc = m->w5.prc2 * |
1651 | 361 | (m->w4.scale2 ? PRCLARGE : PRCSMALL); |
1652 | 361 | tp->glonass_ranges.sat[n].rrc = m->w5.rrc2 * |
1653 | 361 | (m->w4.scale2 ? RRLARGE : RRSMALL); |
1654 | 361 | n++; |
1655 | 361 | } |
1656 | 854 | if (5 <= lens) { |
1657 | 261 | tp->glonass_ranges.sat[n].ident = m->w6.satident3; |
1658 | 261 | tp->glonass_ranges.sat[n].udre = m->w6.udre3; |
1659 | 261 | tp->glonass_ranges.sat[n].change = (bool)m->w7.change3; |
1660 | 261 | tp->glonass_ranges.sat[n].tod = m->w7.tod3; |
1661 | 261 | tp->glonass_ranges.sat[n].prc = |
1662 | 261 | ((m->w6.prc3_h << 8) | (m->w7.prc3_l)) * |
1663 | 261 | (m->w6.scale3 ? PRCLARGE : PRCSMALL); |
1664 | 261 | tp->glonass_ranges.sat[n].rrc = |
1665 | 261 | m->w7.rrc3 * (m->w6.scale3 ? RRLARGE : RRSMALL); |
1666 | 261 | n++; |
1667 | 261 | } |
1668 | 854 | lens -= 5; |
1669 | 854 | m++; |
1670 | 854 | } |
1671 | 593 | tp->glonass_ranges.nentries = n; |
1672 | 593 | } |
1673 | 593 | break; |
1674 | | |
1675 | 132 | case 32: |
1676 | 132 | msg_name = "Reference Station Parameters (GLONASS)"; |
1677 | 132 | break; |
1678 | | |
1679 | 217 | case 33: |
1680 | 217 | msg_name = "Constellation health (GLONASS)"; |
1681 | 217 | break; |
1682 | | |
1683 | | // case 34 is above with 31 |
1684 | | |
1685 | 270 | case 35: |
1686 | 270 | msg_name = "Beacon Almanac (GLONASS)"; |
1687 | 270 | break; |
1688 | | |
1689 | 205 | case 36: |
1690 | 205 | msg_name = "Special Message (GLONASS)"; |
1691 | 205 | break; |
1692 | | |
1693 | 133 | case 37: |
1694 | 133 | msg_name = "GNSS System Time Offset"; |
1695 | 133 | break; |
1696 | | |
1697 | 135 | case 59: |
1698 | 135 | msg_name = "Proprietary Message"; |
1699 | 135 | break; |
1700 | | |
1701 | 135 | default: |
1702 | 135 | msg_name = "Huh?"; |
1703 | 135 | break; |
1704 | 11.4k | } |
1705 | | |
1706 | 11.4k | if (unknown) { |
1707 | 4.50k | memcpy(tp->words, msg->msg_type.rtcm2_msgunk, |
1708 | 4.50k | tp->length * sizeof(isgps30bits_t)); |
1709 | 4.50k | if (NULL == msg_name) { |
1710 | 0 | GPSD_LOG(LOG_PROG, &session->context->errout, |
1711 | 0 | "RTCM2: type %d (unknown), length %d\n", |
1712 | 0 | tp->type, tp->length + 2); |
1713 | 4.50k | } else { |
1714 | 4.50k | GPSD_LOG(LOG_PROG, &session->context->errout, |
1715 | 4.50k | "RTCM2: type %d (%s), length %d\n", |
1716 | 4.50k | tp->type, msg_name,tp->length + 2); |
1717 | 4.50k | } |
1718 | 6.98k | } else { |
1719 | 6.98k | GPSD_LOG(LOG_PROG, &session->context->errout, |
1720 | 6.98k | "RTCM2: type %d (%s), length %d\n", |
1721 | 6.98k | tp->type, msg_name, tp->length + 2); |
1722 | 6.98k | } |
1723 | 11.4k | GPSD_LOG(LOG_RAW, &session->context->errout, |
1724 | 11.4k | "RTCM 2: type %d (%s) length %d words " |
1725 | 11.4k | "from %zd bytes: %s\n", |
1726 | 11.4k | tp->type, |
1727 | 11.4k | msg_name, |
1728 | 11.4k | tp->length + 2, |
1729 | 11.4k | session->lexer.isgps.buflen, |
1730 | 11.4k | gps_hexdump(session->msgbuf, sizeof(session->msgbuf), |
1731 | 11.4k | (unsigned char *)session->lexer.isgps.buf, |
1732 | 11.4k | (tp->length + 2) * sizeof(isgps30bits_t))); |
1733 | 11.4k | } |
1734 | | |
1735 | | static bool preamble_match(isgps30bits_t * w) |
1736 | 466M | { |
1737 | 466M | return (((struct rtcm2_msghw1 *)w)->preamble == PREAMBLE_PATTERN); |
1738 | 466M | } |
1739 | | |
1740 | | static bool length_check(struct gps_lexer_t *lexer) |
1741 | 62.6k | { |
1742 | 62.6k | return lexer->isgps.bufindex >= 2 |
1743 | 42.2k | && lexer->isgps.bufindex >= |
1744 | 42.2k | ((struct rtcm2_msg_t *)lexer->isgps.buf)->w2.frmlen + 2u; |
1745 | 62.6k | } |
1746 | | |
1747 | | enum isgpsstat_t rtcm2_decode(struct gps_lexer_t *lexer, unsigned int c) |
1748 | 82.3M | { |
1749 | 82.3M | return isgps_decode(lexer, |
1750 | 82.3M | preamble_match, length_check, RTCM2_WORDS_MAX, c); |
1751 | 82.3M | } |
1752 | | |
1753 | | // vim: set expandtab shiftwidth=4 |