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

// -*- C++ -*-
// RTCM.C
// $Id: RTCM.cpp,v 1.1.1.1 2006/01/11 09:34:31 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, "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