source: ntrip/trunk/BNC/RTCM/RTCM.cpp@ 203

Last change on this file since 203 was 154, checked in by mervart, 18 years ago

* empty log message *
File size: 22.1 KB
Line 
1// -*- C++ -*-
2// RTCM.C
3// $Id: RTCM.cpp,v 1.3 2006/09/08 12:14:11 mervart Exp $
4// 2005/04/11: counter 'iPCode' for indicating CA or P Code on L1 (BKG)
5
6#include "RTCM.h"
7
8#include <cmath>
9
10#if !defined(__GNUC__)
11double rint(double val) {
12 return ((val < 0.0) ? -floor(-val+0.5) : floor(val+0.5));
13}
14#endif
15
16/* Org-ID: rtcm.c,v 1.11 1999/08/28 00:06:20 wolfgang Exp */
17
18/* rtcm.c - decode RTCM SC-104 data from a DGPS beacon receiver */
19
20/*
21 written by John Sager john.sager@btinternet.com
22 Copyright (C) 1999 John C Sager
23
24 This program is free software; you can redistribute it and/or modify
25 it under the terms of the GNU General Public License as published by
26 the Free Software Foundation; either version 2 of the License, or
27 (at your option) any later version.
28
29 This program is distributed in the hope that it will be useful,
30 but WITHOUT ANY WARRANTY; without even the implied warranty of
31 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
32 GNU General Public License for more details.
33
34 You should have received a copy of the GNU General Public License
35 along with this program; if not, write to the Free Software
36 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
37
38*/
39
40/* History:
41
42 v0.3 22nd February 1999
43 Incorrect scale factor on range error field fixed.
44 dx,dy,dz fields in Datum message are signed.
45
46 v0.2 9th February 1999
47 Sundry improvements to the code by Wolfgang Rupprecht so
48 he could incorporate it in his dgpsip client.
49
50 Addition of decode capability for message types 4 and 5.
51
52 v0.1 4th February 1999
53 First version - reads RTCM data from standard input & sends
54 decoded output to standard output. Implements decoding of
55 types 1,3,6,7,9,16. This is original software and not
56 a modification of any other program.
57*/
58
59/* TODO
60 Add facility to decode from tty port input & write to a file
61*/
62
63/*
64 * The data comes across as bytes where the top 2 bits indicate the type
65 * of data and the bottom 6 bits are the data itself. Bytes 01xxxxxx
66 * are received RTCM data. Other bytes are other kinds of data/commands
67 * etc. The data framing can be at any bit position. Also the data is
68 * lsb first, so it needs to be bit-reversed.
69 *
70 * This program calculates parity over a 32-bit field starting at each
71 * bit position until a parity match is found. Then parity is checked
72 * at successive 30-bit boundaries until sufficient error-free words
73 * have been found. This gives word sync. Then frame sync is acquired
74 * by searching for the preamble and checking at subsequent frame boundaries
75 * until sufficient frames have been found. Then data is decoded.
76 *
77 * Parity errors are flagged in frames so that data before the parity
78 * error can be decoded, if possible. A parity error in the preamble
79 * word or the next word causes frame sync to be lost. Sufficient
80 * successive parity errors causes sync to be lost totally.
81 */
82
83#if 0
84#include <stdio.h>
85#include <stdlib.h>
86#endif
87#include <sys/types.h>
88
89#define DEBUG
90
91
92/* state machine state */
93
94#define NO_SYNC 0
95#define WORD_SYNCING 1
96#define WORD_SYNC 2
97#define FRAME_SYNCING 3
98#define FULL_SYNC 4
99
100#define W_SYNC_TEST 6
101#define F_SYNC_TEST 3
102#define P_FAIL_TEST 10
103
104/* message types */
105
106#define MSG_FULLCOR 1
107#define MSG_REFPARM 3
108#define MSG_DATUM 4
109#define MSG_CONHLTH 5
110#define MSG_NULL 6
111#define MSG_BEACALM 7
112#define MSG_SUBSCOR 9
113#define MSG_SPECIAL 16
114
115/* field scale factors */
116
117#define ZCOUNT_SCALE 0.6 /* sec */
118#define RANGE_SMALL 0.02 /* metres */
119#define RANGE_LARGE 0.32 /* metres */
120#define RANGERATE_SMALL 0.002 /* metres/sec */
121#define RANGERATE_LARGE 0.032 /* metres/sec */
122#define XYZ_SCALE 0.01 /* metres */
123#define DXYZ_SCALE 0.1 /* metres */
124#define LA_SCALE 90.0/32767.0 /* degrees */
125#define LO_SCALE 180.0/32767.0 /* degrees */
126#define FREQ_SCALE 0.1 /* kHz */
127#define FREQ_OFFSET 190.0 /* kHz */
128#define CNR_OFFSET 24 /* dB */
129#define TU_SCALE 5 /* minutes */
130
131char * RTCM::state_name[] = {
132 "NO_SYNC",
133 "WORD_SYNCING",
134 "WORD_SYNC",
135 "FRAME_SYNCING",
136 "FULL_SYNC"};
137
138u_int RTCM::tx_speed[] = { 25, 50, 100, 110, 150, 200, 250, 300 };
139
140/* parity stuff */
141
142#define P_30_MASK 0x40000000
143#define P_31_MASK 0x80000000
144
145#define PARITY_25 0xbb1f3480
146#define PARITY_26 0x5d8f9a40
147#define PARITY_27 0xaec7cd00
148#define PARITY_28 0x5763e680
149#define PARITY_29 0x6bb1f340
150#define PARITY_30 0x8b7a89c0
151
152#define W_DATA_MASK 0x3fffffc0
153
154u_char RTCM::parity_of[] = {
1550,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,
1561,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,
1571,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,
1580,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,
1591,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,
1600,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,
1610,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,
1621,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0};
163
164u_char RTCM::reverse_bits[] = {
1650,32,16,48,8,40,24,56,4,36,20,52,12,44,28,60,
1662,34,18,50,10,42,26,58,6,38,22,54,14,46,30,62,
1671,33,17,49,9,41,25,57,5,37,21,53,13,45,29,61,
1683,35,19,51,11,43,27,59,7,39,23,55,15,47,31,63};
169
170
171#define DATA_SHIFT 6
172#define B_DATA_MASK 0x3f
173#define FILL_BASE 24
174
175#define PREAMBLE 0x19800000
176#define PREAMBLE_MASK 0x3fc00000
177
178int RTCM::preamble() {
179 return ((data_word & PREAMBLE_MASK) == PREAMBLE);
180}
181
182/* state_change - change state & print a useful message */
183
184void RTCM::state_change(u_int s) {
185#if 0
186 printf("M\tstate change: %s -> %s\n",
187 state_name[rtcm_state], state_name[s]);
188 fflush(stdout);
189#endif
190 rtcm_state = s;
191}
192
193/* check the parity on this_word. bits 31,30 are parity on previous word.
194 * bits 29-6 are data bits. Bits 5-0 are parity bits.
195 */
196
197u_int RTCM::parity_ok() {
198 u_int t, th, p;
199
200 th = this_word;
201 if (th & P_30_MASK)
202 th ^= W_DATA_MASK;
203
204 t = th & PARITY_25;
205 p = parity_of[t & 0xff] ^ parity_of[(t>>8)&0xff] ^
206 parity_of[(t>>16)&0xff] ^ parity_of[(t>>24)];
207 t = th & PARITY_26;
208 p = (p<<1) | (parity_of[t & 0xff] ^ parity_of[(t>>8)&0xff] ^
209 parity_of[(t>>16)&0xff] ^ parity_of[(t>>24)]);
210 t = th & PARITY_27;
211 p = (p<<1) | (parity_of[t & 0xff] ^ parity_of[(t>>8)&0xff] ^
212 parity_of[(t>>16)&0xff] ^ parity_of[(t>>24)]);
213 t = th & PARITY_28;
214 p = (p<<1) | (parity_of[t & 0xff] ^ parity_of[(t>>8)&0xff] ^
215 parity_of[(t>>16)&0xff] ^ parity_of[(t>>24)]);
216 t = th & PARITY_29;
217 p = (p<<1) | (parity_of[t & 0xff] ^ parity_of[(t>>8)&0xff] ^
218 parity_of[(t>>16)&0xff] ^ parity_of[(t>>24)]);
219 t = th & PARITY_30;
220 p = (p<<1) | (parity_of[t & 0xff] ^ parity_of[(t>>8)&0xff] ^
221 parity_of[(t>>16)&0xff] ^ parity_of[(t>>24)]);
222
223
224 if (!this_word || ((this_word &0x3f) != p)) {
225 if (rtcm_state > WORD_SYNCING)
226 pf_count++;
227 return 0;
228 }
229
230 return th;
231}
232
233/* find word sync by a successful parity check */
234
235int RTCM::find_sync(u_char b) {
236 int i;
237
238 b <<= 2;
239 i = 1;
240
241 while (i <= DATA_SHIFT) {
242 this_word <<= 1;
243 this_word |= (b & 0x80) ? 1 : 0; /* next bit into 32 bits */
244 b <<= 1;
245 if ((data_word = parity_ok())) {
246 sync_bit = (i==DATA_SHIFT) ? 0 : i;
247 fill_shift = FILL_BASE - DATA_SHIFT + i;
248 next_bits = b >> 2;
249 return 1;
250 }
251 i++;
252 }
253 return 0;
254}
255
256void RTCM::next_word() {
257 this_word = (this_word << 30) | (next_bits << 24);
258}
259
260/* fill bits into this_word and indicate when filled. Put the residue bits in
261 next_bits.
262 */
263
264int RTCM::filled_word(u_char b) {
265
266 if (fill_shift <= 0) { /* can complete fill */
267 if (fill_shift) {
268 next_bits = b << (DATA_SHIFT + fill_shift);
269 this_word |= b >> (-fill_shift);
270 }
271 else {
272 next_bits = 0;
273 this_word |= b;
274 }
275 fill_shift += FILL_BASE;
276 word_count++;
277 return 1;
278 }
279 this_word |= b << fill_shift;
280 fill_shift -= DATA_SHIFT;
281 return 0;
282}
283
284
285//
286//
287//
288void RTCM::msgRTKUncorrectedCarrierPhases() {
289
290 // const static char *freqIndicatorS[] = { "L1" , "?1" , "L2" , "?3" };
291 const u_int freqIndicator = (message[2]>>28)&0x3;
292
293 const u_int gnssTimeofMeasurement = (message[2]>>6)&0xfffff;
294 const double expandedTimeOfMeasurement = z_count*ZCOUNT_SCALE + gnssTimeofMeasurement * .000001;
295
296 int m;
297 for(m= 3 ; m < msg_len+2 ; m+=2 ) {
298 const int multipleMessageIndicator = (message[m] >> 29) & 0x1;
299 const int pCodeIndicator = (message[m] >> 28) & 0x1;
300 const int glonassIndicator = (message[m] >> 27) & 0x1;
301
302#if 0
303 const u_int svprn = (message[m] >> 22) & 0x1f
304 + (glonassIndicator ? glonass_svid : 0);
305#else
306 const u_int svprn = (message[m] >> 22) & 0x1f;
307#endif
308 if(!glonassIndicator) {
309 dumpOnNewTimeTag(expandedTimeOfMeasurement);
310
311 const u_int dataQuality = (message[m] >> 18) & 0x7;
312 const u_int cumuLossOfCont = (message[m] >> 14) & 0x1f;
313
314 obsMap[svprn].set_cumuLossOfCont(cumuLossOfCont);
315 obsMap[svprn].set_pCodeIndicator(pCodeIndicator);
316
317 const int carrierPhase = (int)( (((message[m] >> 6) & 0xff) << 24)
318 | ((message[m+1]>> 6) & 0xffffff));
319 switch(freqIndicator) {
320 case 0: /* L1 */
321 // TBD unroll L1
322 obsMap[svprn].set_l1(carrierPhase);
323 if(pCodeIndicator > 0) {
324 iPCode++;
325 }
326 break;
327 case 2: /* L2 */
328 // TBD unroll L2
329 obsMap[svprn].set_l2(carrierPhase);
330 break;
331 default:
332 /* unknown frequency */;
333 }
334 }
335
336#if 0
337 printf( "RTKPhase: %s prn:%2d %s %s %f %f %f %11.3f\n"
338 , glonassIndicator ? "GLONASS" : "GPS "
339 , svprn
340 , freqIndicatorS[freqIndicator]
341 , pCodeIndicator ? "P-Code " : "CA-Code"
342 , expandedTimeOfMeasurement
343 , r.timeTag
344 , r.clockError
345 , carrierPhase/256.
346 );
347#endif
348 }
349}
350
351
352//
353//
354//
355void RTCM::msgRTKUncorrectedPseudoranges() {
356 // const static char *freqIndicatorS[] = { "L1" , "?1" , "L2" , "?3" };
357 const u_int freqIndicator = (message[2]>>28)&0x3;
358 const u_int smoothingIntervall = (message[2]>>26)&0x3;
359 const u_int gnssTimeofMeasurement = (message[2]>>6)&0xfffff;
360 const double expandedTimeOfMeasurement = z_count*ZCOUNT_SCALE + gnssTimeofMeasurement * .000001;
361
362 int m;
363 for(m= 3 ; m < msg_len+2 ; m+=2 ) {
364 const int multipleMessageIndicator = (message[m] >> 29) & 0x1;
365 const int pCodeIndicator = (message[m] >> 28) & 0x1;
366 const int glonassIndicator = (message[m] >> 27) & 0x1;
367 if(!glonassIndicator) {
368 dumpOnNewTimeTag(expandedTimeOfMeasurement);
369 const u_int svprn = (message[m] >> 22) & 0x1f
370 + (glonassIndicator ? glonass_svid : 0) ;
371 const u_int dataQual = (message[m] >> 18) & 0xf;
372 const u_int multiPathError = (message[m] >> 14) &0xf;
373 const u_int pseudoRange = ( (((message[m] >> 6) & 0xff) << 24)
374 | ((message[m+1]>> 6) & 0xffffff));
375 switch(freqIndicator) {
376 case 0: /* L1 */
377 obsMap[svprn].set_C1(pseudoRange*0.02);
378 if(pCodeIndicator > 0) {
379 iPCode++;
380 }
381 break;
382 case 2: /* L2 */
383 obsMap[svprn].set_P2(pseudoRange*0.02);
384 break;
385 default:
386 /* unknown frequency */;
387 }
388 }
389
390#if 0
391 printf( "RTKRange: %s prn:%2d %s %s %f %f %f %11.3f\n"
392 , glonassIndicator ? "GLONASS" : "GPS "
393 , svprn
394 , freqIndicatorS[freqIndicator]
395 , pCodeIndicator ? "P-Code " : "CA-Code"
396 , expandedTimeOfMeasurement
397 , r.timeTag
398 , r.clockError
399 , pseudoRange*0.02
400 );
401#endif
402 }
403}
404
405
406/* printcor - print differential corrections - full or subset */
407
408void RTCM::printcor() {
409 int i, w;
410 u_int m, n;
411 int scale, udre, sat, range, rangerate, iod;
412
413 i = 0;
414 w = 2;
415 m = 0;
416 if (pfptr) {
417 msg_len = (pfptr-message) - 2;
418 n = msg_len % 5;
419 if (n == 1 || n == 3) msg_len--;
420 if (msg_len < 2)
421 return;
422 }
423 while (w < msg_len+2) {
424 if ((i & 0x3) == 0){
425 m = message[w++] & W_DATA_MASK;
426 scale = m >> 29 & 0x1;
427 udre = (m >>27) & 0x3;
428 sat = (m >>22) & 0x1f;
429 range = (m >>6) & 0xffff;
430 if (range > 32767) range -= 65536;
431 m = message[w++] & W_DATA_MASK;
432 rangerate = (m >>22) & 0xff;
433 if (rangerate > 127) rangerate -= 256;
434 iod = (m >>14) & 0xff;
435 i++;
436 }
437 else if ((i & 0x3) == 1){
438 scale = m >> 13 & 0x1;
439 udre = (m >>11) & 0x3;
440 sat = (m >>6) & 0x1f;
441 m = message[w++] & W_DATA_MASK;
442 range = (m >>14) & 0xffff;
443 if (range > 32767) range -= 65536;
444 rangerate = (m >>6) & 0xff;
445 if (rangerate > 127) rangerate -= 256;
446 m = message[w++] & W_DATA_MASK;
447 iod = (m >>22) & 0xff;
448 i++;
449 }
450 else {
451 scale = m >> 21 & 0x1;
452 udre = (m >>19) & 0x3;
453 sat = (m >>14) & 0x1f;
454 range = (m <<2) & 0xff00;
455 m = message[w++] & W_DATA_MASK;
456 range |= (m >>22) & 0xff;
457 if (range > 32767) range -= 65536;
458 rangerate = (m >>14) & 0xff;
459 if (rangerate > 127) rangerate -= 256;
460 iod = (m >>6) & 0xff;
461 i+= 2;
462 }
463#if 0
464 printf("S\t%d\t%d\t%d\t%.1f\t%.3f\t%.3f\n", sat, udre, iod,
465 z_count*ZCOUNT_SCALE, range*(scale?RANGE_LARGE:RANGE_SMALL),
466 rangerate*(scale?RANGERATE_LARGE:RANGERATE_SMALL));
467#endif
468 }
469}
470
471/* printref - print reference position. The transmitted xyz quantities are
472 integers scaled in units of 0.01m
473*/
474
475void RTCM::printref(void) {
476 int x, y, z;
477
478 if (pfptr)
479 return;
480 x = ((message[2] & W_DATA_MASK) << 2) | ((message[3] & W_DATA_MASK) >> 22);
481 y = ((message[3] & W_DATA_MASK) << 10) | ((message[4] & W_DATA_MASK) >> 14);
482 z = ((message[4] & W_DATA_MASK) << 18) | ((message[5] & W_DATA_MASK) >> 6);
483
484 onPosition(x*XYZ_SCALE, y*XYZ_SCALE, z*XYZ_SCALE);
485
486#if 0
487 printf("R\t%.2f\t%.2f\t%.2f\n", x*XYZ_SCALE, y*XYZ_SCALE, z*XYZ_SCALE);
488#endif
489}
490
491/* printba - print beacon almanac
492*/
493
494void RTCM::printba() {
495 int la, lo, range, freq, hlth, id, bitrate;
496
497 if (pfptr)
498 return;
499 la = ((message[2] >> 14) & 0xffff);
500 if (la > 32767) la -= 65536;
501 lo = ((message[2] << 2) & 0xff00) | ((message[3] >> 22) & 0xff);
502 if (lo > 32767) lo -= 65536;
503 range = ((message[3] >> 12) & 0x3ff);
504 freq = ((message[3]) & 0xfc0) | ((message[4] >> 24) & 0x3f);
505 hlth = ((message[4] >> 22) & 0x3);
506 id = ((message[4] >> 12) & 0x3ff);
507 bitrate = ((message[4] >> 9) & 0x7);
508
509#if 0
510 printf("A\t%.4f\t%.4f\t%d\t%.1f\t%d\t%d\t%d\n", (la*LA_SCALE), (lo*LO_SCALE),
511 range, (freq*FREQ_SCALE+FREQ_OFFSET), hlth, id, tx_speed[bitrate]);
512#endif
513}
514
515/* printspec - print text of special message
516*/
517
518void RTCM::printspec() {
519 u_int i, d, c;
520#if 0
521 if (pfptr)
522 msg_len = (pfptr-message)>>2;
523 printf("T\t");
524 for (i=0; i< msg_len; i++) {
525 d = message[i+2] & W_DATA_MASK;
526 if ((c=d>>22)) putchar(c); else break;
527 if ((c=((d>>14) & 0xff))) putchar(c); else break;
528 if ((c=((d>>6) & 0xff))) putchar(c); else break;
529 }
530 printf("\n");
531#endif
532}
533
534/* printnull - print a marker for a null message
535*/
536
537void RTCM::printnull() {
538#if 0
539 printf("N\n");
540#endif
541}
542
543/* printdatum - print datum message
544*/
545
546void RTCM::printdatum() {
547 char dname[6];
548 char *dn;
549 u_int d, dgnss, dat;
550 int dx, dy, dz;
551
552 if (pfptr)
553 return;
554 d = message[2] & W_DATA_MASK;
555 dgnss = d>>27;
556 dat = (d>>26) & 1;
557 dname[0] = (d>>14) & 0xff;
558 if (dname[0]) { /* not null */
559 dname[1] = (d>>6) & 0xff;
560 d = message[3] & W_DATA_MASK;
561 dname[2] = (d>>22) & 0xff;
562 dname[3] = (d>>14) & 0xff;
563 dname[4] = (d>>6) & 0xff;
564 dname[5] = '\0';
565 dn = dname;
566 }
567 else
568 dn = "NUL";
569#if 0
570 printf("D\t%s\t%1d\t%s", (dgnss==0)?"GPS":((dgnss==1)?"GLONASS":"???"),
571 dat, dn);
572#endif
573 if (msg_len > 2) {
574 d = message[4] & W_DATA_MASK;
575 dx = (d>>14) & 0xffff;
576 if (dx > 32767) dx -= 65536;
577 dy = (d<<2) & 0xff00;
578 d = message[5] & W_DATA_MASK;
579 dy |= (d>>22) & 0xff;
580 if (dy > 32767) dy -= 65536;
581 dz = (d>>6) & 0xffff;
582 if (dz > 32767) dz -= 65536;
583#if 0
584 printf("\t%.1f\t%.1f\t%.1f", dx*DXYZ_SCALE, dy*DXYZ_SCALE, dz*DXYZ_SCALE);
585#endif
586 }
587#if 0
588 printf("\n");
589#endif
590}
591
592/* printconh - print constellation health message
593*/
594
595void RTCM::printconh() {
596 u_int i, d, sat, iodl, hlth, cnr, he, nd, lw, tu;
597
598 if (pfptr) {
599 msg_len = (pfptr-message) - 2;
600 if (!msg_len)
601 return;
602 }
603 for (i=0; i< msg_len; i++) {
604 d = message[i+2] & W_DATA_MASK;
605 sat = (d>>24) & 0x1f;
606 if (!sat) sat = 32;
607 iodl = (d>>23) & 1;
608 hlth = (d>>20) & 0x7;
609 cnr = (d>>15) & 0x1f;
610 he = (d>>14) & 1;
611 nd = (d>>13) & 1;
612 lw = (d>>12) & 1;
613 tu = (d>>8) & 0x0f;
614#if 0
615 printf("C\t%2d\t%1d %1d\t%2d\t%1d %1d %1d\t%2d\n",
616 sat, iodl, hlth, (cnr?(cnr+CNR_OFFSET):-1), he, nd, lw, tu*TU_SCALE);
617#endif
618 }
619}
620
621/* new_frame - called when a new frame is complete */
622
623void RTCM::new_frame() {
624 char s[8];
625
626 frame_count++;
627 if (pfptr == message) /* dud frame */
628 return;
629
630 msg_type = (message[0]>>16)&0x3f;
631 station_id = (message[0]>>6)&0x3ff;
632 z_count = (message[1]>>17)&0x1fff;
633 health = (message[1]>>6)&0x7;
634#if 0
635 if (pfptr)
636 sprintf(s, "\tT\t%d", (pfptr-message)-2);
637
638 printf("H\t%d\t%d\t%.1f\t%d\t%d\t%d%s\n", msg_type, station_id,
639 (z_count*ZCOUNT_SCALE), seqno, msg_len, health,
640 (pfptr)?s:"");
641#endif
642 switch (msg_type) {
643
644 case MSG_FULLCOR:
645 case MSG_SUBSCOR:
646 printcor();
647 break;
648
649 case MSG_REFPARM:
650 printref();
651 break;
652
653 case MSG_DATUM:
654 printdatum();
655 break;
656
657 case MSG_CONHLTH:
658 printconh();
659 break;
660
661 case MSG_NULL:
662 printnull();
663 break;
664
665 case MSG_BEACALM:
666 printba();
667 break;
668
669 case MSG_SPECIAL:
670 printspec();
671 break;
672
673 case 18: /* RTK Uncorrected Carrier Phases */
674 msgRTKUncorrectedCarrierPhases();
675 break;
676
677 case 19: /* RTK Uncorrected Pseudoranges */
678 msgRTKUncorrectedPseudoranges();
679 break;
680
681 default:
682#if 0
683 printf("?\t%d\n", msg_type);
684#endif
685 break;
686 }
687}
688
689void RTCM::buffer(u_int w) {
690 *fillptr++ = w;
691}
692
693void RTCM::frame_start() {
694 frame_fill = -1;
695 fillptr = message;
696 pfptr = NULL;
697 buffer(data_word);
698}
699
700void RTCM::find_start() {
701 if ((data_word = parity_ok()))
702 p_fail = 0;
703 else if (++p_fail >= P_FAIL_TEST) { /* too many consecutive parity fails */
704 state_change(NO_SYNC);
705 return;
706 }
707 next_word();
708 if (preamble()) {
709 seqno = -1; /* resync at next word */
710 frame_start();
711 state_change(FRAME_SYNCING);
712 }
713}
714
715void RTCM::fill_frame() {
716 int seq;
717
718 if ((data_word = parity_ok()))
719 p_fail=0;
720 else if (++p_fail >= P_FAIL_TEST) {
721 state_change(NO_SYNC);
722 return;
723 }
724 next_word();
725 frame_fill++; /* another word */
726 if (frame_fill == 0) { /* this is second header word */
727 if (!data_word) { /* parity fail - bad news! */
728 state_change(WORD_SYNC); /* lost frame sync */
729 frame_sync = F_SYNC_TEST-1; /* resync rapidly */
730 return;
731 }
732 buffer(data_word);
733 seq = (data_word>>14)&0x7;
734 msg_len = (data_word>>9)&0x1f;
735#if 0
736#ifdef DEBUG
737 if (debug)
738 fprintf(stderr, "ff=%d: %08x %08x %d %d %d %d %s\n", frame_fill,
739 (u_int)fillptr,
740 data_word, msg_len, word_count, pf_count, seqno, state_name[rtcm_state]);
741#endif
742#endif
743 if ((seqno < 0) || (((seqno +1) & 0x7) == seq))
744 seqno = seq; /* resync */
745 else { /* sequence error */
746 state_change(WORD_SYNC); /* to be on the safe side */
747#if 0
748fprintf(stderr,"2\n");
749#endif
750 return;
751 }
752 }
753 else if (frame_fill > msg_len) { /* should be next preamble */
754#if 0
755#ifdef DEBUG
756 if (debug)
757 fprintf(stderr, "ff=%d: %08x %08x %d %d %d %d %s\n", frame_fill,
758 (u_int)fillptr,
759 data_word, msg_len, word_count, pf_count, seqno, state_name[rtcm_state]);
760#endif
761#endif
762 if (rtcm_state == FRAME_SYNCING) { /* be very tough */
763 if (!(data_word && preamble())) {
764 state_change(WORD_SYNC); /* start again */
765 }
766 else if (++frame_sync >= F_SYNC_TEST) {
767/* frame_count = 0; */
768 state_change(FULL_SYNC);
769 new_frame(); /* output the last frame acquired before we start a new one */
770 }
771 frame_start(); /* new frame here */
772 }
773 else {
774 frame_start(); /* new frame here */
775 if (!data_word) /* parity error on preamble - keep sync but lose message */
776 pfptr = message; /* indicates dud message */
777 else if (!preamble()) { /* good word but no preamble! */
778 state_change(WORD_SYNC); /* can't carry on */
779 }
780 }
781 }
782 else { /* other message words */
783#if 0
784#ifdef DEBUG
785 if (debug)
786 fprintf(stderr, "ff=%d: %08x %08x %d %d %d %d %s\n", frame_fill,
787 (u_int)fillptr,
788 data_word, msg_len, word_count, pf_count, seqno, state_name[rtcm_state]);
789#endif
790#endif
791 if (!data_word && !pfptr)
792 pfptr = fillptr; /* mark the (first) error */
793 buffer(data_word);
794 if ((frame_fill == msg_len) && (rtcm_state == FULL_SYNC)) /* frame completed */
795 new_frame();
796 }
797}
798
799void RTCM::status_byte(u_char b) {
800#if 0
801#if defined(PRINTSTATUS)
802 printf("-\tstatus\t-\t%d 0x%x\n", b, b);
803#endif
804#endif
805}
806
807
808/* take a data byte and process it. This will change state according to
809 * the data, place parity-checked data in a buffer and call a function to
810 * process completed frames.
811 */
812
813void RTCM::data_byte(u_char b) {
814
815 b = reverse_bits[b&0x3f];
816
817 if (rtcm_state == NO_SYNC) {
818 if(find_sync(b)) {
819 state_change(WORD_SYNCING);
820#if 0
821 printf("M\tsync_bit: %d\n", sync_bit);
822#endif
823 word_sync = 1;
824 next_word();
825 }
826 }
827 else if (filled_word(b)) {
828 switch (rtcm_state) {
829
830 case WORD_SYNCING:
831 data_word = parity_ok();
832 next_word();
833 if (data_word) {
834 if (++word_sync >= W_SYNC_TEST) {
835 state_change(WORD_SYNC);
836 p_fail = 0;
837 frame_sync = 1;
838 if (preamble()) { /* just in case we hit one immediately */
839 frame_start();
840 state_change(FRAME_SYNCING);
841 }
842 }
843 }
844 else {
845 if (--word_sync <= 0) {
846 state_change(NO_SYNC);
847 return;
848 }
849 }
850 break;
851
852 case WORD_SYNC: /* look for frame start */
853 find_start();
854 break;
855
856 case FRAME_SYNCING:
857 fill_frame();
858 break;
859
860 case FULL_SYNC:
861 fill_frame();
862 break;
863 }
864 }
865}
866
867
868void RTCM::new_byte(u_char b) {
869
870 switch (b >> DATA_SHIFT) {
871
872 case 0:
873 case 2:
874#if 0
875#ifdef DEBUG
876 fprintf(stderr, "RTCM 2.x: unknown byte type %d (%d 0x%0x)\n", b >> DATA_SHIFT,
877 b, b);
878#endif
879#endif
880 return;
881
882 case 3: /* status */
883 status_byte(b);
884 return;
885
886 case 1: /* data */
887 data_byte(b);
888 return;
889 }
890}
891
892#if 0
893main()
894{
895 int b;
896
897 initialise();
898 while ((b=getchar()) != EOF)
899 new_byte(b);
900
901 printf("M\tword count: %d\tparity failures: %d\tframe count: %d\n",
902 word_count, pf_count, frame_count);
903}
904#endif
905
906
Note: See TracBrowser for help on using the repository browser.