// -*- C++ -*-
// RTCM.C
// $Id: RTCM.cpp,v 1.3 2006/09/08 12:14:11 mervart Exp $
// 2005/04/11: counter 'iPCode' for indicating CA or P Code on L1 (BKG)
#include "RTCM.h"
#include <cmath>
#if !defined(__GNUC__)
double rint(double val) {
return ((val < 0.0) ? -floor(-val+0.5) : floor(val+0.5));
}
#endif
/* Org-ID: rtcm.c,v 1.11 1999/08/28 00:06:20 wolfgang Exp */
/* rtcm.c - decode RTCM SC-104 data from a DGPS beacon receiver */
/*
written by John Sager john.sager@btinternet.com
Copyright (C) 1999 John C Sager
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/* History:
v0.3 22nd February 1999
Incorrect scale factor on range error field fixed.
dx,dy,dz fields in Datum message are signed.
v0.2 9th February 1999
Sundry improvements to the code by Wolfgang Rupprecht so
he could incorporate it in his dgpsip client.
Addition of decode capability for message types 4 and 5.
v0.1 4th February 1999
First version - reads RTCM data from standard input & sends
decoded output to standard output. Implements decoding of
types 1,3,6,7,9,16. This is original software and not
a modification of any other program.
*/
/* TODO
Add facility to decode from tty port input & write to a file
*/
/*
* The data comes across as bytes where the top 2 bits indicate the type
* of data and the bottom 6 bits are the data itself. Bytes 01xxxxxx
* are received RTCM data. Other bytes are other kinds of data/commands
* etc. The data framing can be at any bit position. Also the data is
* lsb first, so it needs to be bit-reversed.
*
* This program calculates parity over a 32-bit field starting at each
* bit position until a parity match is found. Then parity is checked
* at successive 30-bit boundaries until sufficient error-free words
* have been found. This gives word sync. Then frame sync is acquired
* by searching for the preamble and checking at subsequent frame boundaries
* until sufficient frames have been found. Then data is decoded.
*
* Parity errors are flagged in frames so that data before the parity
* error can be decoded, if possible. A parity error in the preamble
* word or the next word causes frame sync to be lost. Sufficient
* successive parity errors causes sync to be lost totally.
*/
#if 0
#include <stdio.h>
#include <stdlib.h>
#endif
#include <sys/types.h>
#define DEBUG
/* state machine state */
#define NO_SYNC 0
#define WORD_SYNCING 1
#define WORD_SYNC 2
#define FRAME_SYNCING 3
#define FULL_SYNC 4
#define W_SYNC_TEST 6
#define F_SYNC_TEST 3
#define P_FAIL_TEST 10
/* message types */
#define MSG_FULLCOR 1
#define MSG_REFPARM 3
#define MSG_DATUM 4
#define MSG_CONHLTH 5
#define MSG_NULL 6
#define MSG_BEACALM 7
#define MSG_SUBSCOR 9
#define MSG_SPECIAL 16
/* field scale factors */
#define ZCOUNT_SCALE 0.6 /* sec */
#define RANGE_SMALL 0.02 /* metres */
#define RANGE_LARGE 0.32 /* metres */
#define RANGERATE_SMALL 0.002 /* metres/sec */
#define RANGERATE_LARGE 0.032 /* metres/sec */
#define XYZ_SCALE 0.01 /* metres */
#define DXYZ_SCALE 0.1 /* metres */
#define LA_SCALE 90.0/32767.0 /* degrees */
#define LO_SCALE 180.0/32767.0 /* degrees */
#define FREQ_SCALE 0.1 /* kHz */
#define FREQ_OFFSET 190.0 /* kHz */
#define CNR_OFFSET 24 /* dB */
#define TU_SCALE 5 /* minutes */
char * RTCM::state_name[] = {
"NO_SYNC",
"WORD_SYNCING",
"WORD_SYNC",
"FRAME_SYNCING",
"FULL_SYNC"};
u_int RTCM::tx_speed[] = { 25, 50, 100, 110, 150, 200, 250, 300 };
/* parity stuff */
#define P_30_MASK 0x40000000
#define P_31_MASK 0x80000000
#define PARITY_25 0xbb1f3480
#define PARITY_26 0x5d8f9a40
#define PARITY_27 0xaec7cd00
#define PARITY_28 0x5763e680
#define PARITY_29 0x6bb1f340
#define PARITY_30 0x8b7a89c0
#define W_DATA_MASK 0x3fffffc0
u_char RTCM::parity_of[] = {
0,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,
1,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,
1,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,
0,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,
1,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,
0,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,
0,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,
1,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};
u_char RTCM::reverse_bits[] = {
0,32,16,48,8,40,24,56,4,36,20,52,12,44,28,60,
2,34,18,50,10,42,26,58,6,38,22,54,14,46,30,62,
1,33,17,49,9,41,25,57,5,37,21,53,13,45,29,61,
3,35,19,51,11,43,27,59,7,39,23,55,15,47,31,63};
#define DATA_SHIFT 6
#define B_DATA_MASK 0x3f
#define FILL_BASE 24
#define PREAMBLE 0x19800000
#define PREAMBLE_MASK 0x3fc00000
int RTCM::preamble() {
return ((data_word & PREAMBLE_MASK) == PREAMBLE);
}
/* state_change - change state & print a useful message */
void RTCM::state_change(u_int s) {
#if 0
printf("M\tstate change: %s -> %s\n",
state_name[rtcm_state], state_name[s]);
fflush(stdout);
#endif
rtcm_state = s;
}
/* check the parity on this_word. bits 31,30 are parity on previous word.
* bits 29-6 are data bits. Bits 5-0 are parity bits.
*/
u_int RTCM::parity_ok() {
u_int t, th, p;
th = this_word;
if (th & P_30_MASK)
th ^= W_DATA_MASK;
t = th & PARITY_25;
p = parity_of[t & 0xff] ^ parity_of[(t>>8)&0xff] ^
parity_of[(t>>16)&0xff] ^ parity_of[(t>>24)];
t = th & PARITY_26;
p = (p<<1) | (parity_of[t & 0xff] ^ parity_of[(t>>8)&0xff] ^
parity_of[(t>>16)&0xff] ^ parity_of[(t>>24)]);
t = th & PARITY_27;
p = (p<<1) | (parity_of[t & 0xff] ^ parity_of[(t>>8)&0xff] ^
parity_of[(t>>16)&0xff] ^ parity_of[(t>>24)]);
t = th & PARITY_28;
p = (p<<1) | (parity_of[t & 0xff] ^ parity_of[(t>>8)&0xff] ^
parity_of[(t>>16)&0xff] ^ parity_of[(t>>24)]);
t = th & PARITY_29;
p = (p<<1) | (parity_of[t & 0xff] ^ parity_of[(t>>8)&0xff] ^
parity_of[(t>>16)&0xff] ^ parity_of[(t>>24)]);
t = th & PARITY_30;
p = (p<<1) | (parity_of[t & 0xff] ^ parity_of[(t>>8)&0xff] ^
parity_of[(t>>16)&0xff] ^ parity_of[(t>>24)]);
if (!this_word || ((this_word &0x3f) != p)) {
if (rtcm_state > WORD_SYNCING)
pf_count++;
return 0;
}
return th;
}
/* find word sync by a successful parity check */
int RTCM::find_sync(u_char b) {
int i;
b <<= 2;
i = 1;
while (i <= DATA_SHIFT) {
this_word <<= 1;
this_word |= (b & 0x80) ? 1 : 0; /* next bit into 32 bits */
b <<= 1;
if ((data_word = parity_ok())) {
sync_bit = (i==DATA_SHIFT) ? 0 : i;
fill_shift = FILL_BASE - DATA_SHIFT + i;
next_bits = b >> 2;
return 1;
}
i++;
}
return 0;
}
void RTCM::next_word() {
this_word = (this_word << 30) | (next_bits << 24);
}
/* fill bits into this_word and indicate when filled. Put the residue bits in
next_bits.
*/
int RTCM::filled_word(u_char b) {
if (fill_shift <= 0) { /* can complete fill */
if (fill_shift) {
next_bits = b << (DATA_SHIFT + fill_shift);
this_word |= b >> (-fill_shift);
}
else {
next_bits = 0;
this_word |= b;
}
fill_shift += FILL_BASE;
word_count++;
return 1;
}
this_word |= b << fill_shift;
fill_shift -= DATA_SHIFT;
return 0;
}
//
//
//
void RTCM::msgRTKUncorrectedCarrierPhases() {
// const static char *freqIndicatorS[] = { "L1" , "?1" , "L2" , "?3" };
const u_int freqIndicator = (message[2]>>28)&0x3;
const u_int gnssTimeofMeasurement = (message[2]>>6)&0xfffff;
const double expandedTimeOfMeasurement = z_count*ZCOUNT_SCALE + gnssTimeofMeasurement * .000001;
int m;
for(m= 3 ; m < msg_len+2 ; m+=2 ) {
const int multipleMessageIndicator = (message[m] >> 29) & 0x1;
const int pCodeIndicator = (message[m] >> 28) & 0x1;
const int glonassIndicator = (message[m] >> 27) & 0x1;
#if 0
const u_int svprn = (message[m] >> 22) & 0x1f
+ (glonassIndicator ? glonass_svid : 0);
#else
const u_int svprn = (message[m] >> 22) & 0x1f;
#endif
if(!glonassIndicator) {
dumpOnNewTimeTag(expandedTimeOfMeasurement);
const u_int dataQuality = (message[m] >> 18) & 0x7;
const u_int cumuLossOfCont = (message[m] >> 14) & 0x1f;
obsMap[svprn].set_cumuLossOfCont(cumuLossOfCont);
obsMap[svprn].set_pCodeIndicator(pCodeIndicator);
const int carrierPhase = (int)( (((message[m] >> 6) & 0xff) << 24)
| ((message[m+1]>> 6) & 0xffffff));
switch(freqIndicator) {
case 0: /* L1 */
// TBD unroll L1
obsMap[svprn].set_l1(carrierPhase);
if(pCodeIndicator > 0) {
iPCode++;
}
break;
case 2: /* L2 */
// TBD unroll L2
obsMap[svprn].set_l2(carrierPhase);
break;
default:
/* unknown frequency */;
}
}
#if 0
printf( "RTKPhase: %s prn:%2d %s %s %f %f %f %11.3f\n"
, glonassIndicator ? "GLONASS" : "GPS "
, svprn
, freqIndicatorS[freqIndicator]
, pCodeIndicator ? "P-Code " : "CA-Code"
, expandedTimeOfMeasurement
, r.timeTag
, r.clockError
, carrierPhase/256.
);
#endif
}
}
//
//
//
void RTCM::msgRTKUncorrectedPseudoranges() {
// const static char *freqIndicatorS[] = { "L1" , "?1" , "L2" , "?3" };
const u_int freqIndicator = (message[2]>>28)&0x3;
const u_int smoothingIntervall = (message[2]>>26)&0x3;
const u_int gnssTimeofMeasurement = (message[2]>>6)&0xfffff;
const double expandedTimeOfMeasurement = z_count*ZCOUNT_SCALE + gnssTimeofMeasurement * .000001;
int m;
for(m= 3 ; m < msg_len+2 ; m+=2 ) {
const int multipleMessageIndicator = (message[m] >> 29) & 0x1;
const int pCodeIndicator = (message[m] >> 28) & 0x1;
const int glonassIndicator = (message[m] >> 27) & 0x1;
if(!glonassIndicator) {
dumpOnNewTimeTag(expandedTimeOfMeasurement);
const u_int svprn = (message[m] >> 22) & 0x1f
+ (glonassIndicator ? glonass_svid : 0) ;
const u_int dataQual = (message[m] >> 18) & 0xf;
const u_int multiPathError = (message[m] >> 14) &0xf;
const u_int pseudoRange = ( (((message[m] >> 6) & 0xff) << 24)
| ((message[m+1]>> 6) & 0xffffff));
switch(freqIndicator) {
case 0: /* L1 */
obsMap[svprn].set_C1(pseudoRange*0.02);
if(pCodeIndicator > 0) {
iPCode++;
}
break;
case 2: /* L2 */
obsMap[svprn].set_P2(pseudoRange*0.02);
break;
default:
/* unknown frequency */;
}
}
#if 0
printf( "RTKRange: %s prn:%2d %s %s %f %f %f %11.3f\n"
, glonassIndicator ? "GLONASS" : "GPS "
, svprn
, freqIndicatorS[freqIndicator]
, pCodeIndicator ? "P-Code " : "CA-Code"
, expandedTimeOfMeasurement
, r.timeTag
, r.clockError
, pseudoRange*0.02
);
#endif
}
}
/* printcor - print differential corrections - full or subset */
void RTCM::printcor() {
int i, w;
u_int m, n;
int scale, udre, sat, range, rangerate, iod;
i = 0;
w = 2;
m = 0;
if (pfptr) {
msg_len = (pfptr-message) - 2;
n = msg_len % 5;
if (n == 1 || n == 3) msg_len--;
if (msg_len < 2)
return;
}
while (w < msg_len+2) {
if ((i & 0x3) == 0){
m = message[w++] & W_DATA_MASK;
scale = m >> 29 & 0x1;
udre = (m >>27) & 0x3;
sat = (m >>22) & 0x1f;
range = (m >>6) & 0xffff;
if (range > 32767) range -= 65536;
m = message[w++] & W_DATA_MASK;
rangerate = (m >>22) & 0xff;
if (rangerate > 127) rangerate -= 256;
iod = (m >>14) & 0xff;
i++;
}
else if ((i & 0x3) == 1){
scale = m >> 13 & 0x1;
udre = (m >>11) & 0x3;
sat = (m >>6) & 0x1f;
m = message[w++] & W_DATA_MASK;
range = (m >>14) & 0xffff;
if (range > 32767) range -= 65536;
rangerate = (m >>6) & 0xff;
if (rangerate > 127) rangerate -= 256;
m = message[w++] & W_DATA_MASK;
iod = (m >>22) & 0xff;
i++;
}
else {
scale = m >> 21 & 0x1;
udre = (m >>19) & 0x3;
sat = (m >>14) & 0x1f;
range = (m <<2) & 0xff00;
m = message[w++] & W_DATA_MASK;
range |= (m >>22) & 0xff;
if (range > 32767) range -= 65536;
rangerate = (m >>14) & 0xff;
if (rangerate > 127) rangerate -= 256;
iod = (m >>6) & 0xff;
i+= 2;
}
#if 0
printf("S\t%d\t%d\t%d\t%.1f\t%.3f\t%.3f\n", sat, udre, iod,
z_count*ZCOUNT_SCALE, range*(scale?RANGE_LARGE:RANGE_SMALL),
rangerate*(scale?RANGERATE_LARGE:RANGERATE_SMALL));
#endif
}
}
/* printref - print reference position. The transmitted xyz quantities are
integers scaled in units of 0.01m
*/
void RTCM::printref(void) {
int x, y, z;
if (pfptr)
return;
x = ((message[2] & W_DATA_MASK) << 2) | ((message[3] & W_DATA_MASK) >> 22);
y = ((message[3] & W_DATA_MASK) << 10) | ((message[4] & W_DATA_MASK) >> 14);
z = ((message[4] & W_DATA_MASK) << 18) | ((message[5] & W_DATA_MASK) >> 6);
onPosition(x*XYZ_SCALE, y*XYZ_SCALE, z*XYZ_SCALE);
#if 0
printf("R\t%.2f\t%.2f\t%.2f\n", x*XYZ_SCALE, y*XYZ_SCALE, z*XYZ_SCALE);
#endif
}
/* printba - print beacon almanac
*/
void RTCM::printba() {
int la, lo, range, freq, hlth, id, bitrate;
if (pfptr)
return;
la = ((message[2] >> 14) & 0xffff);
if (la > 32767) la -= 65536;
lo = ((message[2] << 2) & 0xff00) | ((message[3] >> 22) & 0xff);
if (lo > 32767) lo -= 65536;
range = ((message[3] >> 12) & 0x3ff);
freq = ((message[3]) & 0xfc0) | ((message[4] >> 24) & 0x3f);
hlth = ((message[4] >> 22) & 0x3);
id = ((message[4] >> 12) & 0x3ff);
bitrate = ((message[4] >> 9) & 0x7);
#if 0
printf("A\t%.4f\t%.4f\t%d\t%.1f\t%d\t%d\t%d\n", (la*LA_SCALE), (lo*LO_SCALE),
range, (freq*FREQ_SCALE+FREQ_OFFSET), hlth, id, tx_speed[bitrate]);
#endif
}
/* printspec - print text of special message
*/
void RTCM::printspec() {
u_int i, d, c;
#if 0
if (pfptr)
msg_len = (pfptr-message)>>2;
printf("T\t");
for (i=0; i< msg_len; i++) {
d = message[i+2] & W_DATA_MASK;
if ((c=d>>22)) putchar(c); else break;
if ((c=((d>>14) & 0xff))) putchar(c); else break;
if ((c=((d>>6) & 0xff))) putchar(c); else break;
}
printf("\n");
#endif
}
/* printnull - print a marker for a null message
*/
void RTCM::printnull() {
#if 0
printf("N\n");
#endif
}
/* printdatum - print datum message
*/
void RTCM::printdatum() {
char dname[6];
char *dn;
u_int d, dgnss, dat;
int dx, dy, dz;
if (pfptr)
return;
d = message[2] & W_DATA_MASK;
dgnss = d>>27;
dat = (d>>26) & 1;
dname[0] = (d>>14) & 0xff;
if (dname[0]) { /* not null */
dname[1] = (d>>6) & 0xff;
d = message[3] & W_DATA_MASK;
dname[2] = (d>>22) & 0xff;
dname[3] = (d>>14) & 0xff;
dname[4] = (d>>6) & 0xff;
dname[5] = '\0';
dn = dname;
}
else
dn = "NUL";
#if 0
printf("D\t%s\t%1d\t%s", (dgnss==0)?"GPS":((dgnss==1)?"GLONASS":"???"),
dat, dn);
#endif
if (msg_len > 2) {
d = message[4] & W_DATA_MASK;
dx = (d>>14) & 0xffff;
if (dx > 32767) dx -= 65536;
dy = (d<<2) & 0xff00;
d = message[5] & W_DATA_MASK;
dy |= (d>>22) & 0xff;
if (dy > 32767) dy -= 65536;
dz = (d>>6) & 0xffff;
if (dz > 32767) dz -= 65536;
#if 0
printf("\t%.1f\t%.1f\t%.1f", dx*DXYZ_SCALE, dy*DXYZ_SCALE, dz*DXYZ_SCALE);
#endif
}
#if 0
printf("\n");
#endif
}
/* printconh - print constellation health message
*/
void RTCM::printconh() {
u_int i, d, sat, iodl, hlth, cnr, he, nd, lw, tu;
if (pfptr) {
msg_len = (pfptr-message) - 2;
if (!msg_len)
return;
}
for (i=0; i< msg_len; i++) {
d = message[i+2] & W_DATA_MASK;
sat = (d>>24) & 0x1f;
if (!sat) sat = 32;
iodl = (d>>23) & 1;
hlth = (d>>20) & 0x7;
cnr = (d>>15) & 0x1f;
he = (d>>14) & 1;
nd = (d>>13) & 1;
lw = (d>>12) & 1;
tu = (d>>8) & 0x0f;
#if 0
printf("C\t%2d\t%1d %1d\t%2d\t%1d %1d %1d\t%2d\n",
sat, iodl, hlth, (cnr?(cnr+CNR_OFFSET):-1), he, nd, lw, tu*TU_SCALE);
#endif
}
}
/* new_frame - called when a new frame is complete */
void RTCM::new_frame() {
char s[8];
frame_count++;
if (pfptr == message) /* dud frame */
return;
msg_type = (message[0]>>16)&0x3f;
station_id = (message[0]>>6)&0x3ff;
z_count = (message[1]>>17)&0x1fff;
health = (message[1]>>6)&0x7;
#if 0
if (pfptr)
sprintf(s, "\tT\t%d", (pfptr-message)-2);
printf("H\t%d\t%d\t%.1f\t%d\t%d\t%d%s\n", msg_type, station_id,
(z_count*ZCOUNT_SCALE), seqno, msg_len, health,
(pfptr)?s:"");
#endif
switch (msg_type) {
case MSG_FULLCOR:
case MSG_SUBSCOR:
printcor();
break;
case MSG_REFPARM:
printref();
break;
case MSG_DATUM:
printdatum();
break;
case MSG_CONHLTH:
printconh();
break;
case MSG_NULL:
printnull();
break;
case MSG_BEACALM:
printba();
break;
case MSG_SPECIAL:
printspec();
break;
case 18: /* RTK Uncorrected Carrier Phases */
msgRTKUncorrectedCarrierPhases();
break;
case 19: /* RTK Uncorrected Pseudoranges */
msgRTKUncorrectedPseudoranges();
break;
default:
#if 0
printf("?\t%d\n", msg_type);
#endif
break;
}
}
void RTCM::buffer(u_int w) {
*fillptr++ = w;
}
void RTCM::frame_start() {
frame_fill = -1;
fillptr = message;
pfptr = NULL;
buffer(data_word);
}
void RTCM::find_start() {
if ((data_word = parity_ok()))
p_fail = 0;
else if (++p_fail >= P_FAIL_TEST) { /* too many consecutive parity fails */
state_change(NO_SYNC);
return;
}
next_word();
if (preamble()) {
seqno = -1; /* resync at next word */
frame_start();
state_change(FRAME_SYNCING);
}
}
void RTCM::fill_frame() {
int seq;
if ((data_word = parity_ok()))
p_fail=0;
else if (++p_fail >= P_FAIL_TEST) {
state_change(NO_SYNC);
return;
}
next_word();
frame_fill++; /* another word */
if (frame_fill == 0) { /* this is second header word */
if (!data_word) { /* parity fail - bad news! */
state_change(WORD_SYNC); /* lost frame sync */
frame_sync = F_SYNC_TEST-1; /* resync rapidly */
return;
}
buffer(data_word);
seq = (data_word>>14)&0x7;
msg_len = (data_word>>9)&0x1f;
#if 0
#ifdef DEBUG
if (debug)
fprintf(stderr, "ff=%d: %08x %08x %d %d %d %d %s\n", frame_fill,
(u_int)fillptr,
data_word, msg_len, word_count, pf_count, seqno, state_name[rtcm_state]);
#endif
#endif
if ((seqno < 0) || (((seqno +1) & 0x7) == seq))
seqno = seq; /* resync */
else { /* sequence error */
state_change(WORD_SYNC); /* to be on the safe side */
#if 0
fprintf(stderr,"2\n");
#endif
return;
}
}
else if (frame_fill > msg_len) { /* should be next preamble */
#if 0
#ifdef DEBUG
if (debug)
fprintf(stderr, "ff=%d: %08x %08x %d %d %d %d %s\n", frame_fill,
(u_int)fillptr,
data_word, msg_len, word_count, pf_count, seqno, state_name[rtcm_state]);
#endif
#endif
if (rtcm_state == FRAME_SYNCING) { /* be very tough */
if (!(data_word && preamble())) {
state_change(WORD_SYNC); /* start again */
}
else if (++frame_sync >= F_SYNC_TEST) {
/* frame_count = 0; */
state_change(FULL_SYNC);
new_frame(); /* output the last frame acquired before we start a new one */
}
frame_start(); /* new frame here */
}
else {
frame_start(); /* new frame here */
if (!data_word) /* parity error on preamble - keep sync but lose message */
pfptr = message; /* indicates dud message */
else if (!preamble()) { /* good word but no preamble! */
state_change(WORD_SYNC); /* can't carry on */
}
}
}
else { /* other message words */
#if 0
#ifdef DEBUG
if (debug)
fprintf(stderr, "ff=%d: %08x %08x %d %d %d %d %s\n", frame_fill,
(u_int)fillptr,
data_word, msg_len, word_count, pf_count, seqno, state_name[rtcm_state]);
#endif
#endif
if (!data_word && !pfptr)
pfptr = fillptr; /* mark the (first) error */
buffer(data_word);
if ((frame_fill == msg_len) && (rtcm_state == FULL_SYNC)) /* frame completed */
new_frame();
}
}
void RTCM::status_byte(u_char b) {
#if 0
#if defined(PRINTSTATUS)
printf("-\tstatus\t-\t%d 0x%x\n", b, b);
#endif
#endif
}
/* take a data byte and process it. This will change state according to
* the data, place parity-checked data in a buffer and call a function to
* process completed frames.
*/
void RTCM::data_byte(u_char b) {
b = reverse_bits[b&0x3f];
if (rtcm_state == NO_SYNC) {
if(find_sync(b)) {
state_change(WORD_SYNCING);
#if 0
printf("M\tsync_bit: %d\n", sync_bit);
#endif
word_sync = 1;
next_word();
}
}
else if (filled_word(b)) {
switch (rtcm_state) {
case WORD_SYNCING:
data_word = parity_ok();
next_word();
if (data_word) {
if (++word_sync >= W_SYNC_TEST) {
state_change(WORD_SYNC);
p_fail = 0;
frame_sync = 1;
if (preamble()) { /* just in case we hit one immediately */
frame_start();
state_change(FRAME_SYNCING);
}
}
}
else {
if (--word_sync <= 0) {
state_change(NO_SYNC);
return;
}
}
break;
case WORD_SYNC: /* look for frame start */
find_start();
break;
case FRAME_SYNCING:
fill_frame();
break;
case FULL_SYNC:
fill_frame();
break;
}
}
}
void RTCM::new_byte(u_char b) {
switch (b >> DATA_SHIFT) {
case 0:
case 2:
#if 0
#ifdef DEBUG
fprintf(stderr, "RTCM 2.x: unknown byte type %d (%d 0x%0x)\n", b >> DATA_SHIFT,
b, b);
#endif
#endif
return;
case 3: /* status */
status_byte(b);
return;
case 1: /* data */
data_byte(b);
return;
}
}
#if 0
main()
{
int b;
initialise();
while ((b=getchar()) != EOF)
new_byte(b);
printf("M\tword count: %d\tparity failures: %d\tframe count: %d\n",
word_count, pf_count, frame_count);
}
#endif