/src/gpsd/gpsd-3.25.1~dev/drivers/driver_rtcm2.c
Line | Count | Source (jump to first uncovered line) |
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 | 0 | #define ZCOUNT_SCALE 0.6 /* sec */ |
156 | 0 | #define PRCSMALL 0.02 /* meters */ |
157 | 0 | #define PRCLARGE 0.32 /* meters */ |
158 | 0 | #define RRSMALL 0.002 /* meters/sec */ |
159 | 0 | #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 | 0 | #define XYZ_SCALE 0.01 /* meters */ |
165 | | // extended ECEF scale |
166 | 0 | #define EXYZ_SCALE 0.0001 // meters |
167 | | // ECEF delta scale |
168 | 0 | #define DXYZ_SCALE 0.1 /* meters */ |
169 | | // extended ECEF delta scale: 1/256 cm |
170 | 0 | #define EDXYZ_SCALE (1.0/256.0) // centimeter |
171 | | // L2 extended ECEF delta scale: 1/16 cm |
172 | 0 | #define EDXYZ2_SCALE (1.0/16.0) // centimeter |
173 | 0 | #define LA_SCALE (90.0/32767.0) /* degrees */ |
174 | 0 | #define LO_SCALE (180.0/32767.0) /* degrees */ |
175 | 0 | #define FREQ_SCALE 0.1 /* kHz */ |
176 | 0 | #define FREQ_OFFSET 190.0 /* kHz */ |
177 | 0 | #define CNR_OFFSET 24 /* dB */ |
178 | 0 | #define TU_SCALE 5 /* minutes */ |
179 | | |
180 | 0 | #define LATLON_SCALE 0.01 /* degrees */ |
181 | 0 | #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 6 */ |
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[(RTCM2_WORDS_MAX - 2)/3]; |
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[(RTCM2_WORDS_MAX - 2)/3]; |
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 | 957k | #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 | 0 | { |
1140 | 0 | int len; |
1141 | 0 | unsigned int n, w; |
1142 | 0 | struct rtcm2_msg_t *msg = (struct rtcm2_msg_t *)buf; |
1143 | 0 | bool unknown = true; // we don't know how to decode |
1144 | 0 | const char *msg_name = NULL; // no decode, but maybe we know the name |
1145 | |
|
1146 | 0 | tp->type = msg->w1.msgtype; // 1 to 64. Zero means 64 |
1147 | 0 | tp->refstaid = msg->w1.refstaid; // 0 to 1023 |
1148 | 0 | tp->length = msg->w2.frmlen; // 0 to 31 |
1149 | 0 | tp->zcount = msg->w2.zcnt * ZCOUNT_SCALE; // 0 to 3599.4 sec |
1150 | 0 | tp->seqnum = msg->w2.sqnum; // 0 to 7 |
1151 | 0 | tp->stathlth = msg->w2.stathlth; |
1152 | | |
1153 | | // clear rtk struct |
1154 | 0 | memset(&tp->rtk, 0, sizeof(tp->rtk)); |
1155 | | |
1156 | | // clear ref_sta struct |
1157 | 0 | memset(&tp->ref_sta, 0, sizeof(tp->ref_sta)); |
1158 | 0 | tp->ref_sta.x = NAN; |
1159 | 0 | tp->ref_sta.y = NAN; |
1160 | 0 | tp->ref_sta.z = NAN; |
1161 | 0 | tp->ref_sta.dx = NAN; |
1162 | 0 | tp->ref_sta.dy = NAN; |
1163 | 0 | tp->ref_sta.dz = NAN; |
1164 | 0 | tp->ref_sta.ah = NAN; |
1165 | 0 | tp->ref_sta.dx2 = NAN; |
1166 | 0 | tp->ref_sta.dy2 = NAN; |
1167 | 0 | tp->ref_sta.dz2 = NAN; |
1168 | | |
1169 | | /* FIXME: test tp->length + 2, against session->lexer.isgps.buflen, */ |
1170 | | |
1171 | | // len does not include 2 byte header |
1172 | 0 | len = (int)tp->length; |
1173 | 0 | n = 0; |
1174 | 0 | switch (tp->type) { |
1175 | 0 | case 1: |
1176 | 0 | FALLTHROUGH |
1177 | 0 | case 9: |
1178 | 0 | { |
1179 | 0 | struct gps_correction_t *m = &msg->msg_type.type1.corrections[0]; |
1180 | 0 | msg_name = "Differential GPS Corrections"; |
1181 | 0 | unknown = false; |
1182 | |
|
1183 | 0 | while (len >= 0) { |
1184 | 0 | if (len >= 2) { |
1185 | 0 | tp->gps_ranges.sat[n].ident = m->w3.satident1; |
1186 | 0 | tp->gps_ranges.sat[n].udre = m->w3.udre1; |
1187 | 0 | tp->gps_ranges.sat[n].iod = m->w4.iod1; |
1188 | 0 | tp->gps_ranges.sat[n].prc = m->w3.prc1 * |
1189 | 0 | (m->w3.scale1 ? PRCLARGE : PRCSMALL); |
1190 | 0 | tp->gps_ranges.sat[n].rrc = m->w4.rrc1 * |
1191 | 0 | (m->w3.scale1 ? RRLARGE : RRSMALL); |
1192 | 0 | n++; |
1193 | 0 | } |
1194 | 0 | if (len >= 4) { |
1195 | 0 | tp->gps_ranges.sat[n].ident = m->w4.satident2; |
1196 | 0 | tp->gps_ranges.sat[n].udre = m->w4.udre2; |
1197 | 0 | tp->gps_ranges.sat[n].iod = m->w6.iod2; |
1198 | 0 | tp->gps_ranges.sat[n].prc = m->w5.prc2 * |
1199 | 0 | (m->w4.scale2 ? PRCLARGE : PRCSMALL); |
1200 | 0 | tp->gps_ranges.sat[n].rrc = m->w5.rrc2 * |
1201 | 0 | (m->w4.scale2 ? RRLARGE : RRSMALL); |
1202 | 0 | n++; |
1203 | 0 | } |
1204 | 0 | if (len >= 5) { |
1205 | 0 | tp->gps_ranges.sat[n].ident = m->w6.satident3; |
1206 | 0 | tp->gps_ranges.sat[n].udre = m->w6.udre3; |
1207 | 0 | tp->gps_ranges.sat[n].iod = m->w7.iod3; |
1208 | 0 | tp->gps_ranges.sat[n].prc = |
1209 | 0 | ((m->w6.prc3_h << 8) | (m->w7.prc3_l)) * |
1210 | 0 | (m->w6.scale3 ? PRCLARGE : PRCSMALL); |
1211 | 0 | tp->gps_ranges.sat[n].rrc = |
1212 | 0 | m->w7.rrc3 * (m->w6.scale3 ? RRLARGE : RRSMALL); |
1213 | 0 | n++; |
1214 | 0 | } |
1215 | 0 | len -= 5; |
1216 | 0 | m++; |
1217 | 0 | } |
1218 | 0 | tp->gps_ranges.nentries = n; |
1219 | 0 | } |
1220 | 0 | break; |
1221 | | |
1222 | 0 | case 2: |
1223 | 0 | msg_name = "Delta Differential GPS Corrections"; |
1224 | 0 | break; |
1225 | | |
1226 | 0 | case 3: |
1227 | 0 | { |
1228 | 0 | struct rtcm2_msg3 *m = &msg->msg_type.type3; |
1229 | 0 | msg_name = "Reference Station Parameters (GPS)"; |
1230 | 0 | unknown = false; |
1231 | |
|
1232 | 0 | if ((tp->ref_sta.valid = len >= 4)) { |
1233 | 0 | tp->ref_sta.x = ((m->w3.x_h << 8) | (m->w4.x_l)) * XYZ_SCALE; |
1234 | 0 | tp->ref_sta.y = ((m->w4.y_h << 16) | (m->w5.y_l)) * XYZ_SCALE; |
1235 | 0 | tp->ref_sta.z = ((m->w5.z_h << 24) | (m->w6.z_l)) * XYZ_SCALE; |
1236 | 0 | } |
1237 | 0 | } |
1238 | 0 | break; |
1239 | | |
1240 | 0 | case 4: |
1241 | 0 | msg_name = "Reference Station Datum"; |
1242 | 0 | unknown = false; |
1243 | 0 | if ((tp->reference.valid = len >= 2)) { |
1244 | 0 | struct rtcm2_msg4 *m = &msg->msg_type.type4; |
1245 | |
|
1246 | 0 | tp->reference.system = |
1247 | 0 | (m->w3.dgnss == 0) ? NAVSYSTEM_GPS : |
1248 | 0 | ((m->w3.dgnss == 1) ? NAVSYSTEM_GLONASS : NAVSYSTEM_UNKNOWN); |
1249 | 0 | tp->reference.sense = |
1250 | 0 | (m->w3.dat != 0) ? SENSE_GLOBAL : SENSE_LOCAL; |
1251 | 0 | if (m->w3.datum_alpha_char1) { |
1252 | 0 | tp->reference.datum[n++] = (char)(m->w3.datum_alpha_char1); |
1253 | 0 | } |
1254 | 0 | if (m->w3.datum_alpha_char2) { |
1255 | 0 | tp->reference.datum[n++] = (char)(m->w3.datum_alpha_char2); |
1256 | 0 | } |
1257 | 0 | if (m->w4.datum_sub_div_char1) { |
1258 | 0 | tp->reference.datum[n++] = (char)(m->w4.datum_sub_div_char1); |
1259 | 0 | } |
1260 | 0 | if (m->w4.datum_sub_div_char2) { |
1261 | 0 | tp->reference.datum[n++] = (char)(m->w4.datum_sub_div_char2); |
1262 | 0 | } |
1263 | 0 | if (m->w4.datum_sub_div_char3) { |
1264 | 0 | tp->reference.datum[n++] = (char)(m->w4.datum_sub_div_char3); |
1265 | 0 | } |
1266 | | /* we used to say n++ here, but scan-build complains */ |
1267 | 0 | tp->reference.datum[n] = '\0'; |
1268 | 0 | if (len >= 4) { |
1269 | 0 | tp->reference.dx = m->w5.dx * DXYZ_SCALE; |
1270 | 0 | tp->reference.dy = |
1271 | 0 | ((m->w5.dy_h << 8) | m->w6.dy_l) * DXYZ_SCALE; |
1272 | 0 | tp->reference.dz = m->w6.dz * DXYZ_SCALE; |
1273 | 0 | } else |
1274 | 0 | tp->reference.sense = SENSE_INVALID; |
1275 | 0 | } |
1276 | 0 | break; |
1277 | | |
1278 | 0 | case 5: |
1279 | 0 | msg_name = "Constellation health (GPS)"; |
1280 | 0 | unknown = false; |
1281 | 0 | for (n = 0; n < (unsigned)len; n++) { |
1282 | 0 | struct consat_t *csp = &tp->conhealth.sat[n]; |
1283 | 0 | struct b_health_t *m = &msg->msg_type.type5.health[n]; |
1284 | |
|
1285 | 0 | csp->ident = m->sat_id; |
1286 | 0 | csp->iodl = m->issue_of_data_link != 0; |
1287 | 0 | csp->health = m->data_health; |
1288 | 0 | csp->snr = (int)(m->cn0 ? (m->cn0 + CNR_OFFSET) : SNR_BAD); |
1289 | 0 | csp->health_en = m->health_enable != 0; |
1290 | 0 | csp->new_data = m->new_nav_data != 0; |
1291 | 0 | csp->los_warning = m->loss_warn != 0; |
1292 | 0 | csp->tou = m->time_unhealthy * TU_SCALE; |
1293 | 0 | } |
1294 | 0 | tp->conhealth.nentries = n; |
1295 | 0 | break; |
1296 | | |
1297 | 0 | case 6: |
1298 | 0 | msg_name = "Null frame (GPS)"; |
1299 | 0 | unknown = false; |
1300 | 0 | break; |
1301 | | |
1302 | 0 | case 7: |
1303 | 0 | msg_name = "Beacon Almanac (GPS)"; |
1304 | 0 | unknown = false; |
1305 | 0 | for (w = 0; w < (unsigned)len; w++) { |
1306 | 0 | struct station_t *np = &tp->almanac.station[n]; |
1307 | 0 | struct b_station_t *mp = &msg->msg_type.type7.almanac[w]; |
1308 | |
|
1309 | 0 | np->latitude = mp->w3.lat * LA_SCALE; |
1310 | 0 | np->longitude = ((mp->w3.lon_h << 8) | mp->w4.lon_l) * LO_SCALE; |
1311 | 0 | np->range = mp->w4.range; |
1312 | 0 | np->frequency = |
1313 | 0 | (((mp->w4.freq_h << 6) | mp->w5.freq_l) * FREQ_SCALE) + |
1314 | 0 | FREQ_OFFSET; |
1315 | 0 | np->health = mp->w5.health; |
1316 | 0 | np->station_id = mp->w5.station_id, |
1317 | 0 | np->bitrate = tx_speed[mp->w5.bit_rate]; |
1318 | 0 | n++; |
1319 | 0 | } |
1320 | 0 | tp->almanac.nentries = (unsigned)(len / 3); |
1321 | 0 | break; |
1322 | | |
1323 | 0 | case 8: |
1324 | 0 | msg_name = "Pseudolite Almanac"; |
1325 | 0 | break; |
1326 | | |
1327 | | // case 9 is handled above with case 1 |
1328 | | |
1329 | 0 | case 10: |
1330 | 0 | msg_name = "P-Code Differential Corrections"; |
1331 | 0 | break; |
1332 | | |
1333 | 0 | case 11: |
1334 | 0 | msg_name = "C/A Code L2 Corrections"; |
1335 | 0 | break; |
1336 | | |
1337 | 0 | case 12: |
1338 | 0 | msg_name = "Pseudolite Station Parameters"; |
1339 | 0 | break; |
1340 | | |
1341 | 0 | case 13: |
1342 | 0 | msg_name = "Ground Transmitter Parameters"; |
1343 | 0 | unknown = false; |
1344 | 0 | tp->xmitter.status = (bool)msg->msg_type.type13.w1.status; |
1345 | 0 | tp->xmitter.rangeflag = (bool)msg->msg_type.type13.w1.rangeflag; |
1346 | 0 | tp->xmitter.lat = msg->msg_type.type13.w1.lat * LATLON_SCALE; |
1347 | 0 | tp->xmitter.lon = msg->msg_type.type13.w2.lon * LATLON_SCALE; |
1348 | 0 | tp->xmitter.range = msg->msg_type.type13.w2.range * RANGE_SCALE; |
1349 | 0 | if (tp->xmitter.range == 0) |
1350 | 0 | tp->xmitter.range = 1024; |
1351 | 0 | break; |
1352 | | |
1353 | 0 | case 14: |
1354 | 0 | msg_name = "GPS Time Of Week"; |
1355 | 0 | unknown = false; |
1356 | 0 | tp->gpstime.week = msg->msg_type.type14.w1.week; |
1357 | 0 | tp->gpstime.hour = msg->msg_type.type14.w1.hour; |
1358 | 0 | tp->gpstime.leapsecs = msg->msg_type.type14.w1.leapsecs; |
1359 | 0 | break; |
1360 | | |
1361 | 0 | case 15: |
1362 | 0 | msg_name = "Ionospheric Delay Message"; |
1363 | 0 | break; |
1364 | | |
1365 | 0 | case 16: |
1366 | 0 | msg_name = "Special Message (GPS)"; |
1367 | 0 | unknown = false; |
1368 | 0 | for (w = 0; w < (unsigned)len; w++) { |
1369 | 0 | if (!msg->msg_type.type16.txt[w].byte1) { |
1370 | 0 | break; |
1371 | 0 | } |
1372 | 0 | tp->message[n++] = (char)(msg->msg_type.type16.txt[w].byte1); |
1373 | 0 | if (!msg->msg_type.type16.txt[w].byte2) { |
1374 | 0 | break; |
1375 | 0 | } |
1376 | 0 | tp->message[n++] = (char)(msg->msg_type.type16.txt[w].byte2); |
1377 | 0 | if (!msg->msg_type.type16.txt[w].byte3) { |
1378 | 0 | break; |
1379 | 0 | } |
1380 | 0 | tp->message[n++] = (char)(msg->msg_type.type16.txt[w].byte3); |
1381 | 0 | } |
1382 | 0 | tp->message[n] = '\0'; |
1383 | 0 | break; |
1384 | | |
1385 | 0 | case 17: |
1386 | 0 | msg_name = "GPS Ephemeris"; |
1387 | 0 | break; |
1388 | | |
1389 | 0 | case 18: |
1390 | 0 | msg_name = "RTK Uncorrected Carrier-phase"; |
1391 | 0 | unknown = false; |
1392 | | // WIP: partial decode |
1393 | 0 | if (1 > len) { |
1394 | | // too short |
1395 | 0 | break; |
1396 | 0 | } |
1397 | 0 | { |
1398 | 0 | struct rtcm2_msg18 *m = &msg->msg_type.type18; |
1399 | 0 | tp->rtk.nentries = (len - 1) / 2; |
1400 | 0 | unsigned i; |
1401 | |
|
1402 | 0 | tp->rtk.tom = m->tom; |
1403 | 0 | tp->rtk.f = m->f; |
1404 | 0 | for (i = 0; i < tp->rtk.nentries; i++) { |
1405 | 0 | tp->rtk.sat[i].m = m->sat[i].m; |
1406 | 0 | tp->rtk.sat[i].pc = m->sat[i].pc; |
1407 | 0 | tp->rtk.sat[i].g = m->sat[i].g; |
1408 | 0 | tp->rtk.sat[i].ident = m->sat[i].ident; |
1409 | 0 | tp->rtk.sat[i].dq = m->sat[i].dq; |
1410 | 0 | tp->rtk.sat[i].carrier_phase = m->sat[i].cp_l; |
1411 | 0 | tp->rtk.sat[i].carrier_phase |= m->sat[i].cp_h << 24; |
1412 | 0 | tp->rtk.sat[i].clc = m->sat[i].clc; |
1413 | 0 | } |
1414 | 0 | } |
1415 | 0 | break; |
1416 | | |
1417 | 0 | case 19: |
1418 | 0 | msg_name = "RTK Corrected Pseudorange"; |
1419 | | // WIP: partial decode |
1420 | 0 | if (1 > len) { |
1421 | | // too short |
1422 | 0 | break; |
1423 | 0 | } |
1424 | 0 | { |
1425 | 0 | struct rtcm2_msg19 *m = &msg->msg_type.type19; |
1426 | 0 | unsigned i; |
1427 | 0 | tp->rtk.tom = m->tom; |
1428 | 0 | tp->rtk.f = m->f; |
1429 | 0 | tp->rtk.sm = m->sm; |
1430 | 0 | tp->rtk.nentries = (len - 1) / 2; |
1431 | |
|
1432 | 0 | for (i = 0; i < tp->rtk.nentries; i++) { |
1433 | 0 | tp->rtk.sat[i].m = m->sat[i].m; |
1434 | 0 | tp->rtk.sat[i].pc = m->sat[i].pc; |
1435 | 0 | tp->rtk.sat[i].g = m->sat[i].g; |
1436 | 0 | tp->rtk.sat[i].ident = m->sat[i].ident; |
1437 | 0 | tp->rtk.sat[i].dq = m->sat[i].dq; |
1438 | 0 | tp->rtk.sat[i].pseudorange = m->sat[i].pr_l; |
1439 | 0 | tp->rtk.sat[i].pseudorange |= m->sat[i].pr_h << 24; |
1440 | 0 | } |
1441 | 0 | } |
1442 | 0 | break; |
1443 | | |
1444 | 0 | case 20: |
1445 | 0 | msg_name = "RTK Carrier Phase Corrections"; |
1446 | | // WIP: partial decode |
1447 | 0 | if (3 > len) { |
1448 | | // too short |
1449 | 0 | break; |
1450 | 0 | } |
1451 | 0 | { |
1452 | 0 | struct rtcm2_msg20 *m = &msg->msg_type.type20; |
1453 | 0 | tp->rtk.tom = m->tom; |
1454 | 0 | tp->rtk.f = m->f; |
1455 | 0 | } |
1456 | 0 | break; |
1457 | | |
1458 | 0 | case 21: |
1459 | 0 | msg_name = "High-Accuracy Pseudorange Corrections"; |
1460 | | // WIP: partial decode |
1461 | 0 | if (3 > len) { |
1462 | | // too short |
1463 | 0 | break; |
1464 | 0 | } |
1465 | 0 | { |
1466 | 0 | struct rtcm2_msg21 *m = &msg->msg_type.type21; |
1467 | 0 | tp->rtk.tom = m->tom; |
1468 | 0 | tp->rtk.f = m->f; |
1469 | 0 | tp->rtk.sm = m->sm; |
1470 | 0 | } |
1471 | 0 | break; |
1472 | | |
1473 | 0 | case 22: |
1474 | | // Extends types 3 and 34 |
1475 | 0 | msg_name = "Extended Reference Station Parameters"; |
1476 | 0 | unknown = false; |
1477 | | // WIP: partial decode |
1478 | 0 | if (3 > len) { |
1479 | | // too short |
1480 | 0 | break; |
1481 | 0 | } |
1482 | | // May be length 3, 4 or 5! |
1483 | 0 | { |
1484 | 0 | struct rtcm2_msg22 *m = &msg->msg_type.type22; |
1485 | |
|
1486 | 0 | tp->ref_sta.dx = m->ecef_dx * EDXYZ_SCALE; |
1487 | 0 | tp->ref_sta.dy = m->ecef_dy * EDXYZ_SCALE; |
1488 | 0 | tp->ref_sta.dz = m->ecef_dz * EDXYZ_SCALE; |
1489 | 0 | if (3 < len) { |
1490 | | // word 4 present |
1491 | 0 | tp->ref_sta.gs = m->gs; |
1492 | 0 | if (0 == m->nh) { |
1493 | 0 | tp->ref_sta.ah = m->ah * EDXYZ_SCALE; |
1494 | 0 | } |
1495 | 0 | } |
1496 | 0 | if (4 < len) { |
1497 | | // word 5 present |
1498 | 0 | tp->ref_sta.dx2 = m->ecef_dx2 * EDXYZ2_SCALE; |
1499 | 0 | tp->ref_sta.dy2 = m->ecef_dy2 * EDXYZ2_SCALE; |
1500 | 0 | tp->ref_sta.dz2 = m->ecef_dz2 * EDXYZ2_SCALE; |
1501 | 0 | } |
1502 | 0 | } |
1503 | 0 | break; |
1504 | | |
1505 | 0 | case 23: |
1506 | 0 | msg_name = "Antenna Type Definition"; |
1507 | | // WIP |
1508 | 0 | unknown = false; |
1509 | 0 | if (1 > len) { |
1510 | | // too short |
1511 | 0 | break; |
1512 | 0 | } |
1513 | 0 | { |
1514 | 0 | unsigned sf; |
1515 | 0 | unsigned nad = 0, nas = 0; |
1516 | 0 | unsigned char tbuf[RTCM2_WORDS_MAX * 3]; |
1517 | 0 | struct rtcm2_msg23 *m = &msg->msg_type.type23; |
1518 | 0 | int i = 0, j = 0; |
1519 | |
|
1520 | 0 | tp->ref_sta.ar = (m->words[0].byte0 >> 6) & 1; |
1521 | 0 | sf = (m->words[0].byte0 >> 5) & 1; |
1522 | | // nad is 0 to 31 |
1523 | 0 | nad = m->words[0].byte0 & 0x1f; |
1524 | | |
1525 | | // crazy packing... move to simple array first |
1526 | 0 | for (i = 0; i < len; i++) { |
1527 | 0 | tbuf[j++] = m->words[i].byte0; |
1528 | 0 | tbuf[j++] = m->words[i].byte1; |
1529 | 0 | tbuf[j++] = m->words[i].byte2; |
1530 | 0 | } |
1531 | | #ifdef __UNUSED__ |
1532 | | // debug code |
1533 | | { |
1534 | | char tmpbuf[100]; |
1535 | | GPSD_LOG(LOG_SHOUT, &session->context->errout, |
1536 | | "RTCM2: len %d nad %d tbuf %s\n", |
1537 | | len, nad, |
1538 | | gps_hexdump(tmpbuf, sizeof(tmpbuf), |
1539 | | (char *)tbuf, len * 3)); |
1540 | | } |
1541 | | #endif // __UNUSED__ |
1542 | | // skip first byte (AR, SF, NAD) |
1543 | 0 | memcpy(tp->ref_sta.ant_desc, &tbuf[1], nad); |
1544 | 0 | tp->ref_sta.ant_desc[nad] = '\0'; |
1545 | |
|
1546 | 0 | tp->ref_sta.setup_id = tbuf[nad + 1]; |
1547 | | |
1548 | | // get serial number |
1549 | 0 | if (1 == sf) { |
1550 | | // nas is 0 to 31. just after last ant desc byte |
1551 | 0 | nas = tbuf[nad + 2] & 0x1f; |
1552 | 0 | memcpy(tp->ref_sta.ant_serial, &tbuf[nad + 3], nas); |
1553 | 0 | } |
1554 | 0 | tp->ref_sta.ant_serial[nas] = '\0'; |
1555 | 0 | } |
1556 | 0 | break; |
1557 | | |
1558 | 0 | case 24: |
1559 | 0 | msg_name = "Antenna Reference Point (arp)"; |
1560 | | // WIP |
1561 | 0 | unknown = false; |
1562 | 0 | { |
1563 | 0 | struct rtcm2_msg24 *m = &msg->msg_type.type24; |
1564 | | // try to prevent int overflow |
1565 | 0 | long long temp; |
1566 | |
|
1567 | 0 | temp = ((long long)m->w3.x_h << 8) | (m->w4.x_l); |
1568 | 0 | tp->ref_sta.x = temp * EXYZ_SCALE; |
1569 | 0 | temp = ((long long)m->w4.y_h << 32) | |
1570 | 0 | (m->w5.y_m << 6) | |
1571 | 0 | (m->w6.y_l); |
1572 | 0 | tp->ref_sta.y = temp * EXYZ_SCALE; |
1573 | 0 | temp = ((long long)m->w6.z_h << 22) | (m->w7.z_l); |
1574 | 0 | tp->ref_sta.z = temp * EXYZ_SCALE; |
1575 | 0 | if (0 == m->w7.ah) { |
1576 | 0 | tp->ref_sta.ah = m->w8.ah * EDXYZ_SCALE; |
1577 | 0 | } |
1578 | 0 | } |
1579 | 0 | break; |
1580 | | |
1581 | 0 | case 25: |
1582 | 0 | FALLTHROUGH |
1583 | 0 | case 26: |
1584 | 0 | msg_name = "Undefined"; |
1585 | 0 | break; |
1586 | | |
1587 | 0 | case 27: |
1588 | 0 | msg_name = "Radio Beacon Almanac"; |
1589 | 0 | break; |
1590 | | |
1591 | 0 | case 28: |
1592 | 0 | FALLTHROUGH |
1593 | 0 | case 29: |
1594 | 0 | FALLTHROUGH |
1595 | 0 | case 30: |
1596 | 0 | msg_name = "Undefined"; |
1597 | 0 | break; |
1598 | | |
1599 | 0 | case 31: |
1600 | 0 | FALLTHROUGH |
1601 | 0 | case 34: |
1602 | 0 | { |
1603 | | // 32 and 34 are the same, except 31 is all sats, 34 is some sats |
1604 | 0 | struct glonass_correction_t *m = &msg->msg_type.type31.corrections[0]; |
1605 | 0 | msg_name = "Differential GLONASS corrections"; |
1606 | 0 | unknown = false; |
1607 | |
|
1608 | 0 | while (len >= 0) { |
1609 | 0 | if (len >= 2) { |
1610 | 0 | tp->glonass_ranges.sat[n].ident = m->w3.satident1; |
1611 | 0 | tp->glonass_ranges.sat[n].udre = m->w3.udre1; |
1612 | 0 | tp->glonass_ranges.sat[n].change = (bool)m->w4.change1; |
1613 | 0 | tp->glonass_ranges.sat[n].tod = m->w4.tod1; |
1614 | 0 | tp->glonass_ranges.sat[n].prc = m->w3.prc1 * |
1615 | 0 | (m->w3.scale1 ? PRCLARGE : PRCSMALL); |
1616 | 0 | tp->glonass_ranges.sat[n].rrc = m->w4.rrc1 * |
1617 | 0 | (m->w3.scale1 ? RRLARGE : RRSMALL); |
1618 | 0 | n++; |
1619 | 0 | } |
1620 | 0 | if (len >= 4) { |
1621 | 0 | tp->glonass_ranges.sat[n].ident = m->w4.satident2; |
1622 | 0 | tp->glonass_ranges.sat[n].udre = m->w4.udre2; |
1623 | 0 | tp->glonass_ranges.sat[n].change = (bool)m->w6.change2; |
1624 | 0 | tp->glonass_ranges.sat[n].tod = m->w6.tod2; |
1625 | 0 | tp->glonass_ranges.sat[n].prc = m->w5.prc2 * |
1626 | 0 | (m->w4.scale2 ? PRCLARGE : PRCSMALL); |
1627 | 0 | tp->glonass_ranges.sat[n].rrc = m->w5.rrc2 * |
1628 | 0 | (m->w4.scale2 ? RRLARGE : RRSMALL); |
1629 | 0 | n++; |
1630 | 0 | } |
1631 | 0 | if (len >= 5) { |
1632 | 0 | tp->glonass_ranges.sat[n].ident = m->w6.satident3; |
1633 | 0 | tp->glonass_ranges.sat[n].udre = m->w6.udre3; |
1634 | 0 | tp->glonass_ranges.sat[n].change = (bool)m->w7.change3; |
1635 | 0 | tp->glonass_ranges.sat[n].tod = m->w7.tod3; |
1636 | 0 | tp->glonass_ranges.sat[n].prc = |
1637 | 0 | ((m->w6.prc3_h << 8) | (m->w7.prc3_l)) * |
1638 | 0 | (m->w6.scale3 ? PRCLARGE : PRCSMALL); |
1639 | 0 | tp->glonass_ranges.sat[n].rrc = |
1640 | 0 | m->w7.rrc3 * (m->w6.scale3 ? RRLARGE : RRSMALL); |
1641 | 0 | n++; |
1642 | 0 | } |
1643 | 0 | len -= 5; |
1644 | 0 | m++; |
1645 | 0 | } |
1646 | 0 | tp->glonass_ranges.nentries = n; |
1647 | 0 | } |
1648 | 0 | break; |
1649 | | |
1650 | 0 | case 32: |
1651 | 0 | msg_name = "Reference Station Parameters (GLONASS)"; |
1652 | 0 | break; |
1653 | | |
1654 | 0 | case 33: |
1655 | 0 | msg_name = "Constellation health (GLONASS)"; |
1656 | 0 | break; |
1657 | | |
1658 | | // case 34 is above with 31 |
1659 | | |
1660 | 0 | case 35: |
1661 | 0 | msg_name = "Beacon Almanac (GLONASS)"; |
1662 | 0 | break; |
1663 | | |
1664 | 0 | case 36: |
1665 | 0 | msg_name = "Special Message (GLONASS)"; |
1666 | 0 | break; |
1667 | | |
1668 | 0 | case 37: |
1669 | 0 | msg_name = "GNSS System Time Offset"; |
1670 | 0 | break; |
1671 | | |
1672 | 0 | case 59: |
1673 | 0 | msg_name = "Proprietary Message"; |
1674 | 0 | break; |
1675 | | |
1676 | 0 | default: |
1677 | 0 | msg_name = "Huh?"; |
1678 | 0 | break; |
1679 | 0 | } |
1680 | | |
1681 | 0 | if (unknown) { |
1682 | 0 | memcpy(tp->words, msg->msg_type.rtcm2_msgunk, |
1683 | 0 | tp->length * sizeof(isgps30bits_t)); |
1684 | 0 | if (NULL == msg_name) { |
1685 | 0 | GPSD_LOG(LOG_PROG, &session->context->errout, |
1686 | 0 | "RTCM2: type %d (unknown), length %d\n", |
1687 | 0 | tp->type, tp->length + 2); |
1688 | 0 | } else { |
1689 | 0 | GPSD_LOG(LOG_PROG, &session->context->errout, |
1690 | 0 | "RTCM2: type %d (%s), length %d\n", |
1691 | 0 | tp->type, msg_name,tp->length + 2); |
1692 | 0 | } |
1693 | 0 | } else { |
1694 | 0 | GPSD_LOG(LOG_PROG, &session->context->errout, |
1695 | 0 | "RTCM2: type %d (%s), length %d\n", |
1696 | 0 | tp->type, msg_name, tp->length + 2); |
1697 | 0 | } |
1698 | 0 | GPSD_LOG(LOG_RAW, &session->context->errout, |
1699 | 0 | "RTCM 2: type %d (%s) length %d words " |
1700 | 0 | "from %zd bytes: %s\n", |
1701 | 0 | tp->type, |
1702 | 0 | msg_name, |
1703 | 0 | tp->length + 2, |
1704 | 0 | session->lexer.isgps.buflen, |
1705 | 0 | gps_hexdump(session->msgbuf, sizeof(session->msgbuf), |
1706 | 0 | (unsigned char *)session->lexer.isgps.buf, |
1707 | 0 | (tp->length + 2) * sizeof(isgps30bits_t))); |
1708 | 0 | } |
1709 | | |
1710 | | static bool preamble_match(isgps30bits_t * w) |
1711 | 957k | { |
1712 | 957k | return (((struct rtcm2_msghw1 *)w)->preamble == PREAMBLE_PATTERN); |
1713 | 957k | } |
1714 | | |
1715 | | static bool length_check(struct gps_lexer_t *lexer) |
1716 | 10.8k | { |
1717 | 10.8k | return lexer->isgps.bufindex >= 2 |
1718 | 10.8k | && lexer->isgps.bufindex >= |
1719 | 6.87k | ((struct rtcm2_msg_t *)lexer->isgps.buf)->w2.frmlen + 2u; |
1720 | 10.8k | } |
1721 | | |
1722 | | enum isgpsstat_t rtcm2_decode(struct gps_lexer_t *lexer, unsigned int c) |
1723 | 327k | { |
1724 | 327k | return isgps_decode(lexer, |
1725 | 327k | preamble_match, length_check, RTCM2_WORDS_MAX, c); |
1726 | 327k | } |
1727 | | |
1728 | | // vim: set expandtab shiftwidth=4 |