Ignore:
Timestamp:
May 5, 2015, 3:44:39 PM (9 years ago)
Author:
stoecker
Message:

integrate RTCM3 parsing into BNC and directly fill target structures, add doxygen documentation

Location:
trunk/BNC/src
Files:
3 edited

Legend:

Unmodified
Added
Removed

  • trunk/BNC/src

    • Property svn:ignore

      •  

        old new  
        55debug
        66release
         7html

  • trunk/BNC/src/RTCM3

    • Property svn:externals

      •  

        old new  
        11clock_and_orbit http://software.rtcm-ntrip.org/svn/trunk/clock_and_orbit/lib/
        2 rtcm3torinex    http://software.rtcm-ntrip.org/svn/trunk/rtcm3torinex/lib/

  • trunk/BNC/src/RTCM3/RTCM3Decoder.cpp

    r6598 r6812  
    4545#include <string.h>
    4646
     47#include "bits.h"
     48#include "gnss.h"
    4749#include "RTCM3Decoder.h"
    48 #include "../RTCM/rtcm_utils.h"
     50#include "rtcm_utils.h"
    4951#include "bncconst.h"
    5052#include "bnccore.h"
    … …  
    5456using namespace std;
    5557
    56 #ifndef isinf
    57 #  define isinf(x) 0
    58 #endif
    59 
    6058// Error Handling
    6159////////////////////////////////////////////////////////////////////////////
    … …  
    7068  _staID   = staID;
    7169  _rawFile = rawFile;
    72 
    73   bncSettings settings;
    74   _checkMountPoint = settings.value("miscMount").toString();
    7570
    7671  connect(this, SIGNAL(newGPSEph(t_ephGPS)),     BNC_CORE, SLOT(slotNewGPSEph(t_ephGPS)));
    … …  
    8075  connect(this, SIGNAL(newBDSEph(t_ephBDS)),     BNC_CORE, SLOT(slotNewBDSEph(t_ephBDS)));
    8176
    82   // Mode can be either observations or corrections
    83   // ----------------------------------------------
    84   _mode = unknown;
    85 
    86   // Antenna position (used for decoding of message 1003)
    87   // ----------------------------------------------------
    88   _antXYZ[0] = _antXYZ[1] = _antXYZ[2] = 0;
    89 
     77  _MessageSize = _SkipBytes = _BlockSize = _NeedBytes = 0;
    9078}
    9179
    … …  
    9482RTCM3Decoder::~RTCM3Decoder() {
    9583  QMapIterator<QByteArray, RTCM3coDecoder*> it(_coDecoders);
    96   while (it.hasNext()) {
     84  while(it.hasNext())
     85  {
    9786    it.next();
    9887    delete it.value();
    … …  
    10291//
    10392////////////////////////////////////////////////////////////////////////////
    104 t_irc RTCM3Decoder::Decode(char* buffer, int bufLen, vector<string>& errmsg) {
    105 
    106   errmsg.clear();
    107 
     93bool RTCM3Decoder::DecodeRTCM3GPS(unsigned char* data, int size)
     94{
    10895  bool decoded = false;
    109 
    110   // If read from file, mode is always uknown
    111   // ----------------------------------------
    112   if (_rawFile) {
    113     _mode  = unknown;
    114     _staID = _rawFile->staID();
    115   }
    116 
    117   // Try to decode Clock and Orbit Corrections
    118   // -----------------------------------------
    119   if (_mode == unknown || _mode == corrections) {
    120 
    121     // Find the corresponding coDecoder
    122     // --------------------------------
    123     if (!_coDecoders.contains(_staID.toAscii())) {
    124       _coDecoders[_staID.toAscii()] = new RTCM3coDecoder(_staID);
     96  bncTime CurrentObsTime;
     97  int i, numsats, syncf, type;
     98  uint64_t numbits = 0, bitfield = 0;
     99
     100  data += 3; /* header */
     101  size -= 6; /* header + crc */
     102
     103  GETBITS(type, 12)
     104  SKIPBITS(12) /* id */
     105  GETBITS(i,30)
     106
     107  CurrentObsTime.set(i);
     108  if(_CurrentTime.valid() && CurrentObsTime != _CurrentTime)
     109  {
     110    decoded = true;
     111    _obsList = _CurrentObsList;
     112    _CurrentObsList.clear();
     113  }
     114  _CurrentTime = CurrentObsTime;
     115
     116  GETBITS(syncf,1) /* sync */
     117  GETBITS(numsats,5)
     118  SKIPBITS(4) /* smind, smint */
     119
     120  while(numsats--)
     121  {
     122    int sv, code, l1range, amb=0;
     123    t_satObs CurrentObs;
     124    CurrentObs._time = CurrentObsTime;
     125
     126    GETBITS(sv, 6)
     127    if(sv < 40)
     128      CurrentObs._prn.set('G', sv);
     129    else
     130      CurrentObs._prn.set('S', sv-20);
     131
     132    t_frqObs *frqObs = new t_frqObs;
     133    /* L1 */
     134    GETBITS(code, 1);
     135    frqObs->_rnxType2ch = code ? "1W" : "1C";
     136    GETBITS(l1range, 24);
     137    GETBITSSIGN(i, 20);
     138    if((i&((1<<20)-1)) != 0x80000)
     139    {
     140      frqObs->_code = l1range*0.02;
     141      frqObs->_phase = (l1range*0.02+i*0.0005)/GPS_WAVELENGTH_L1;
     142      frqObs->_codeValid = frqObs->_phaseValid = true;
    125143    }
    126     RTCM3coDecoder* coDecoder = _coDecoders[_staID.toAscii()];
    127 
    128     if ( coDecoder->Decode(buffer, bufLen, errmsg) == success ) {
    129       decoded = true;
    130       if  (!_rawFile && _mode == unknown) {
    131         _mode = corrections;
     144    GETBITS(i, 7);
     145    frqObs->_slipCounter = i;
     146    if(type == 1002 || type == 1004)
     147    {
     148      GETBITS(amb,8);
     149      if(amb)
     150      {
     151        frqObs->_code += amb*299792.458;
     152        frqObs->_phase += (amb*299792.458)/GPS_WAVELENGTH_L1;
     153      }
     154      GETBITS(i, 8);
     155      if(i)
     156      {
     157        frqObs->_snr = i*0.25;
     158        frqObs->_snrValid = true;
    132159      }
    133160    }
    134   }
    135 
    136   // Find the corresponding parser, initialize a new parser if necessary
    137   // -------------------------------------------------------------------
    138   bool newParser = !_parsers.contains(_staID.toAscii());
    139   RTCM3ParserData& parser = _parsers[_staID.toAscii()];
    140   if (newParser) { 
    141     memset(&parser, 0, sizeof(parser));
    142     parser.rinex3 = 0;
    143     double secGPS;
    144     currentGPSWeeks(parser.GPSWeek, secGPS);
    145     parser.GPSTOW = int(secGPS);
    146   }
    147 
    148   // Remaining part decodes the Observations
    149   // ---------------------------------------
    150   if (_mode == unknown || _mode == observations ||
    151       _checkMountPoint == _staID || _checkMountPoint == "ALL") {
    152 
    153     for (int iByte = 0; iByte < bufLen; iByte++) {
    154 
    155       parser.Message[parser.MessageSize++] = buffer[iByte];
    156 
    157       if (parser.MessageSize >= parser.NeedBytes) {
    158 
    159         while (int rr = RTCM3Parser(&parser)) {
    160 
    161           // RTCMv3 message types
    162           // --------------------
    163           _typeList.push_back(parser.blocktype);
    164 
    165           // RTCMv3 antenna descriptor
    166           // -------------------------
    167           if (rr == 1007 || rr == 1008 || rr == 1033) {
    168             _antType.push_back(parser.antenna);
     161    CurrentObs._obs.push_back(frqObs);
     162    if(type == 1003 || type == 1004)
     163    {
     164      frqObs = new t_frqObs;
     165      /* L2 */
     166      GETBITS(code,2);
     167      switch(code)
     168      {
     169      case 3: frqObs->_rnxType2ch = "2W"; /* or "2Y"? */ break;
     170      case 2: frqObs->_rnxType2ch = "2W"; break;
     171      case 1: frqObs->_rnxType2ch = "2P"; break;
     172      case 0: frqObs->_rnxType2ch = "2X"; /* or "2S" or "2L"? */ break;
     173      }
     174      GETBITSSIGN(i,14);
     175      if((i&((1<<14)-1)) != 0x2000)
     176      {
     177        frqObs->_code = l1range*0.02+i*0.02+amb*299792.458;
     178        frqObs->_codeValid = true;
     179      }
     180      GETBITSSIGN(i,20);
     181      if((i&((1<<20)-1)) != 0x80000)
     182      {
     183        frqObs->_phase = (l1range*0.02+i*0.0005+amb*299792.458)/GPS_WAVELENGTH_L2;
     184        frqObs->_phaseValid = true;
     185      }
     186      GETBITS(i,7);
     187      frqObs->_slipCounter = i;
     188      if(type == 1004)
     189      {
     190        GETBITS(i, 8);
     191        if(i)
     192        {
     193          frqObs->_snr = i*0.25;
     194          frqObs->_snrValid = true;
     195        }
     196      }
     197      CurrentObs._obs.push_back(frqObs);
     198    }
     199    _CurrentObsList.push_back(CurrentObs);
     200  }
     201  if(!syncf)
     202  {
     203    decoded = true;
     204    _obsList = _CurrentObsList;
     205    _CurrentTime.reset();
     206    _CurrentObsList.clear();
     207  }
     208  return decoded;
     209}
     210
     211#define RTCM3_MSM_NUMSIG      32
     212#define RTCM3_MSM_NUMSAT      64
     213#define RTCM3_MSM_NUMCELLS    96 /* arbitrary limit */
     214
     215/**
     216 * Frequency numbers of GLONASS with an offset of 100 to detect unset values.
     217 * Gets filled by ephemeris and data blocks and shared between different streams.
     218 */
     219static int GLOFreq[RTCM3_MSM_NUMSAT];
     220
     221/*
     222 * Storage structure to store frequency and RINEX ID assignment for MSM
     223 * message */
     224struct CodeData {
     225  double wl;
     226  const char *code; /* currently unused */
     227};
     228
     229/** MSM signal types for GPS and SBAS */
     230static struct CodeData gps[RTCM3_MSM_NUMSIG] =
     231{
     232  {0.0,0},
     233  {GPS_WAVELENGTH_L1,"1C"},
     234  {GPS_WAVELENGTH_L1,"1P"},
     235  {GPS_WAVELENGTH_L1,"1W"},
     236  {0.0,0}/*{GPS_WAVELENGTH_L1,"1Y"}*/,
     237  {0.0,0},
     238  {0.0,0},
     239  {GPS_WAVELENGTH_L2,"2C"},
     240  {GPS_WAVELENGTH_L2,"2P"},
     241  {GPS_WAVELENGTH_L2,"2W"},
     242  {0.0,0}/*{GPS_WAVELENGTH_L2,"2Y"}*/,
     243  {0.0,0},
     244  {0.0,0},
     245  {0.0,0},
     246  {GPS_WAVELENGTH_L2,"2S"},
     247  {GPS_WAVELENGTH_L2,"2L"},
     248  {GPS_WAVELENGTH_L2,"2X"},
     249  {0.0,0},
     250  {0.0,0},
     251  {0.0,0},
     252  {0.0,0},
     253  {GPS_WAVELENGTH_L5,"5I"},
     254  {GPS_WAVELENGTH_L5,"5Q"},
     255  {GPS_WAVELENGTH_L5,"5X"},
     256  {0.0,0},
     257  {0.0,0},
     258  {0.0,0},
     259  {0.0,0},
     260  {0.0,0},
     261  {GPS_WAVELENGTH_L1,"1S"},
     262  {GPS_WAVELENGTH_L1,"1L"},
     263  {GPS_WAVELENGTH_L1,"1X"}
     264};
     265
     266/**
     267 * MSM signal types for GLONASS
     268 *
     269 * NOTE: Uses 0.0, 1.0 for wavelength as sat index dependence is done later!
     270 */
     271static struct CodeData glo[RTCM3_MSM_NUMSIG] =
     272{
     273  {0.0,0},
     274  {0.0,"1C"},
     275  {0.0,"1P"},
     276  {0.0,0},
     277  {0.0,0},
     278  {0.0,0},
     279  {0.0,0},
     280  {1.0,"2C"},
     281  {1.0,"2P"},
     282  {0.0,0},
     283  {0.0,0},
     284  {0.0,0},
     285  {0.0,0},
     286  {0.0,0},
     287  {0.0,0},
     288  {0.0,0},
     289  {0.0,0},
     290  {0.0,0},
     291  {0.0,0},
     292  {0.0,0},
     293  {0.0,0},
     294  {0.0,0},
     295  {0.0,0},
     296  {0.0,0},
     297  {0.0,0},
     298  {0.0,0},
     299  {0.0,0},
     300  {0.0,0},
     301  {0.0,0},
     302  {0.0,0},
     303  {0.0,0},
     304  {0.0,0}
     305};
     306
     307/** MSM signal types for Galileo */
     308static struct CodeData gal[RTCM3_MSM_NUMSIG] =
     309{
     310  {0.0,0},
     311  {GAL_WAVELENGTH_E1,"1C"},
     312  {GAL_WAVELENGTH_E1,"1A"},
     313  {GAL_WAVELENGTH_E1,"1B"},
     314  {GAL_WAVELENGTH_E1,"1X"},
     315  {GAL_WAVELENGTH_E1,"1Z"},
     316  {0.0,0},
     317  {GAL_WAVELENGTH_E6,"6C"},
     318  {GAL_WAVELENGTH_E6,"6A"},
     319  {GAL_WAVELENGTH_E6,"6B"},
     320  {GAL_WAVELENGTH_E6,"6X"},
     321  {GAL_WAVELENGTH_E6,"6Z"},
     322  {0.0,0},
     323  {GAL_WAVELENGTH_E5B,"7I"},
     324  {GAL_WAVELENGTH_E5B,"7Q"},
     325  {GAL_WAVELENGTH_E5B,"7X"},
     326  {0.0,0},
     327  {GAL_WAVELENGTH_E5AB,"8I"},
     328  {GAL_WAVELENGTH_E5AB,"8Q"},
     329  {GAL_WAVELENGTH_E5AB,"8X"},
     330  {0.0,0},
     331  {GAL_WAVELENGTH_E5A,"5I"},
     332  {GAL_WAVELENGTH_E5A,"5Q"},
     333  {GAL_WAVELENGTH_E5A,"5X"},
     334  {0.0,0},
     335  {0.0,0},
     336  {0.0,0},
     337  {0.0,0},
     338  {0.0,0},
     339  {0.0,0},
     340  {0.0,0},
     341  {0.0,0},
     342};
     343
     344/** MSM signal types for QZSS */
     345static struct CodeData qzss[RTCM3_MSM_NUMSIG] =
     346{
     347  {0.0,0},
     348  {GPS_WAVELENGTH_L1,"1C"},
     349  {0.0,0},
     350  {0.0,0},
     351  {0.0,0},
     352  {GPS_WAVELENGTH_L1,"1Z"},
     353  {0.0,0},
     354  {0.0,0},
     355  {QZSS_WAVELENGTH_LEX,"6S"},
     356  {QZSS_WAVELENGTH_LEX,"6L"},
     357  {QZSS_WAVELENGTH_LEX,"6X"},
     358  {0.0,0},
     359  {0.0,0},
     360  {0.0,0},
     361  {GPS_WAVELENGTH_L2,"2S"},
     362  {GPS_WAVELENGTH_L2,"2L"},
     363  {GPS_WAVELENGTH_L2,"2X"},
     364  {0.0,0},
     365  {0.0,0},
     366  {0.0,0},
     367  {0.0,0},
     368  {GPS_WAVELENGTH_L5,"5I"},
     369  {GPS_WAVELENGTH_L5,"5Q"},
     370  {GPS_WAVELENGTH_L5,"5X"},
     371  {0.0,0},
     372  {0.0,0},
     373  {0.0,0},
     374  {0.0,0},
     375  {0.0,0},
     376  {GPS_WAVELENGTH_L1,"1D"},
     377  {GPS_WAVELENGTH_L1,"1P"},
     378  {GPS_WAVELENGTH_L1,"1X"}
     379};
     380
     381/** MSM signal types for Beidou/BDS */
     382static struct CodeData bds[RTCM3_MSM_NUMSIG] =
     383{
     384  {0.0,0},
     385  {BDS_WAVELENGTH_B1,"1I"},
     386  {0.0,0},
     387  {0.0,0},
     388  {0.0,0},
     389  {0.0,0},
     390  {0.0,0},
     391  {BDS_WAVELENGTH_B3,"6I"},
     392  {0.0,0},
     393  {0.0,0},
     394  {0.0,0},
     395  {0.0,0},
     396  {0.0,0},
     397  {BDS_WAVELENGTH_B2,"7I"},
     398  {0.0,0},
     399  {0.0,0},
     400  {0.0,0},
     401  {0.0,0},
     402  {0.0,0},
     403  {0.0,0},
     404  {0.0,0},
     405  {0.0,0},
     406  {0.0,0},
     407  {0.0,0},
     408  {0.0,0},
     409  {0.0,0},
     410  {0.0,0},
     411  {0.0,0},
     412  {0.0,0},
     413  {0.0,0},
     414  {0.0,0},
     415  {0.0,0},
     416};
     417
     418#define UINT64(c) c ## ULL
     419
     420//
     421////////////////////////////////////////////////////////////////////////////
     422bool RTCM3Decoder::DecodeRTCM3MSM(unsigned char* data, int size)
     423{
     424  bool decoded = false;
     425  int type, syncf, i;
     426  uint64_t numbits = 0, bitfield = 0;
     427
     428  data += 3; /* header */
     429  size -= 6; /* header + crc */
     430
     431  GETBITS(type, 12)
     432  SKIPBITS(12) /* id */
     433  char sys;
     434  if(type >= 1121)
     435    sys = 'C';
     436  else if(type >= 1111)
     437    sys = 'J';
     438  else if(type >= 1101)
     439    sys = 'S';
     440  else if(type >= 1091)
     441    sys = 'E';
     442  else if(type >= 1081)
     443    sys = 'R';
     444  else
     445    sys = 'G';
     446
     447  bncTime CurrentObsTime;
     448  if(sys == 'C') /* BDS */
     449  {
     450    GETBITS(i,30)
     451    CurrentObsTime.setBDS(i);
     452  }
     453  else if(sys == 'R') /* GLONASS */
     454  {
     455    SKIPBITS(3)
     456    GETBITS(i,27) /* tk */
     457    CurrentObsTime.setTk(i);
     458  }
     459  else /* GPS style date */
     460  {
     461    GETBITS(i,30)
     462    CurrentObsTime.set(i);
     463  }
     464  if(_CurrentTime.valid() && CurrentObsTime != _CurrentTime)
     465  {
     466    decoded = true;
     467    _obsList = _CurrentObsList;
     468    _CurrentObsList.clear();
     469  }
     470  _CurrentTime = CurrentObsTime;
     471
     472  GETBITS(syncf, 1)
     473  /**
     474   * Ignore unknown types except for sync flag
     475   *
     476   * We actually support types 1-3 in following code, but as they are missing
     477   * the full cycles and can't be used later we skip interpretation here already.
     478   */
     479  if(type <= 1130 && (type % 10) >= 4 && (type % 10) <= 7)
     480  {
     481    int sigmask, numsat = 0, numsig = 0;
     482    uint64_t satmask, cellmask, ui;
     483    double rrmod[RTCM3_MSM_NUMSAT];
     484    int rrint[RTCM3_MSM_NUMSAT], rdop[RTCM3_MSM_NUMSAT],
     485    extsat[RTCM3_MSM_NUMSAT];
     486    int ll[RTCM3_MSM_NUMCELLS]/*, hc[RTCM3_MSM_NUMCELLS]*/;
     487    double cnr[RTCM3_MSM_NUMCELLS];
     488    double cp[RTCM3_MSM_NUMCELLS], psr[RTCM3_MSM_NUMCELLS],
     489    dop[RTCM3_MSM_NUMCELLS];
     490
     491    SKIPBITS(3+7+2+2+1+3)
     492    GETBITS64(satmask, RTCM3_MSM_NUMSAT)
     493
     494    /* http://gurmeetsingh.wordpress.com/2008/08/05/fast-bit-counting-routines/ */
     495    for(ui = satmask; ui; ui &= (ui - 1) /* remove rightmost bit */)
     496      ++numsat;
     497    GETBITS(sigmask, RTCM3_MSM_NUMSIG)
     498    for(i = sigmask; i; i &= (i - 1) /* remove rightmost bit */)
     499      ++numsig;
     500    for(i = 0; i < RTCM3_MSM_NUMSAT; ++i)
     501      extsat[i] = 15;
     502
     503    i = numsat*numsig;
     504    GETBITS64(cellmask, (unsigned)i)
     505
     506    switch(type % 10)
     507    {
     508    case 1: case 2: case 3:
     509      /* partial data, already skipped above, but implemented for future expansion ! */
     510      for(int j = numsat; j--;)
     511        GETFLOAT(rrmod[j], 10, 1.0/1024.0)
     512      break;
     513    case 4: case 6:
     514      for(int j = numsat; j--;)
     515        GETBITS(rrint[j], 8)
     516      for(int j = numsat; j--;)
     517        GETFLOAT(rrmod[j], 10, 1.0/1024.0)
     518      break;
     519    case 5: case 7:
     520      for(int j = numsat; j--;)
     521        GETBITS(rrint[j], 8)
     522      for(int j = numsat; j--;)
     523        GETBITS(extsat[j], 4)
     524      for(int j = numsat; j--;)
     525        GETFLOAT(rrmod[j], 10, 1.0/1024.0)
     526      for(int j = numsat; j--;)
     527        GETBITSSIGN(rdop[j], 14)
     528      break;
     529    }
     530
     531    int numcells = numsat*numsig;
     532    /** Drop anything which exceeds our cell limit. Increase limit definition
     533     * when that happens. */
     534    if(numcells <= RTCM3_MSM_NUMCELLS)
     535    {
     536      switch(type % 10)
     537      {
     538      case 1:
     539        for(int count = numcells; count--;)
     540          if(cellmask & (UINT64(1)<<count))
     541            GETFLOATSIGN(psr[count], 15, 1.0/(1<<24))
     542        break;
     543      case 2:
     544        for(int count = numcells; count--;)
     545          if(cellmask & (UINT64(1)<<count))
     546            GETFLOATSIGN(cp[count], 22, 1.0/(1<<29))
     547        for(int count = numcells; count--;)
     548          if(cellmask & (UINT64(1)<<count))
     549            GETBITS(ll[count], 4)
     550        for(int count = numcells; count--;)
     551          if(cellmask & (UINT64(1)<<count))
     552            SKIPBITS(1)/*GETBITS(hc[count], 1)*/
     553        break;
     554      case 3:
     555        for(int count = numcells; count--;)
     556          if(cellmask & (UINT64(1)<<count))
     557            GETFLOATSIGN(psr[count], 15, 1.0/(1<<24))
     558        for(int count = numcells; count--;)
     559          if(cellmask & (UINT64(1)<<count))
     560            GETFLOATSIGN(cp[count], 22, 1.0/(1<<29))
     561        for(int count = numcells; count--;)
     562          if(cellmask & (UINT64(1)<<count))
     563            GETBITS(ll[count], 4)
     564        for(int count = numcells; count--;)
     565          if(cellmask & (UINT64(1)<<count))
     566            SKIPBITS(1)/*GETBITS(hc[count], 1)*/
     567        break;
     568      case 4:
     569        for(int count = numcells; count--;)
     570          if(cellmask & (UINT64(1)<<count))
     571            GETFLOATSIGN(psr[count], 15, 1.0/(1<<24))
     572        for(int count = numcells; count--;)
     573          if(cellmask & (UINT64(1)<<count))
     574            GETFLOATSIGN(cp[count], 22, 1.0/(1<<29))
     575        for(int count = numcells; count--;)
     576          if(cellmask & (UINT64(1)<<count))
     577            GETBITS(ll[count], 4)
     578        for(int count = numcells; count--;)
     579          if(cellmask & (UINT64(1)<<count))
     580            SKIPBITS(1)/*GETBITS(hc[count], 1)*/
     581        for(int count = numcells; count--;)
     582          if(cellmask & (UINT64(1)<<count))
     583            GETBITS(cnr[count], 6)
     584        break;
     585      case 5:
     586        for(int count = numcells; count--;)
     587          if(cellmask & (UINT64(1)<<count))
     588            GETFLOATSIGN(psr[count], 15, 1.0/(1<<24))
     589        for(int count = numcells; count--;)
     590          if(cellmask & (UINT64(1)<<count))
     591            GETFLOATSIGN(cp[count], 22, 1.0/(1<<29))
     592        for(int count = numcells; count--;)
     593          if(cellmask & (UINT64(1)<<count))
     594            GETBITS(ll[count], 4)
     595        for(int count = numcells; count--;)
     596          if(cellmask & (UINT64(1)<<count))
     597            SKIPBITS(1)/*GETBITS(hc[count], 1)*/
     598        for(int count = numcells; count--;)
     599          if(cellmask & (UINT64(1)<<count))
     600            GETFLOAT(cnr[count], 6, 1.0)
     601        for(int count = numcells; count--;)
     602          if(cellmask & (UINT64(1)<<count))
     603            GETFLOATSIGN(dop[count], 15, 0.0001)
     604        break;
     605      case 6:
     606        for(int count = numcells; count--;)
     607          if(cellmask & (UINT64(1)<<count))
     608            GETFLOATSIGN(psr[count], 20, 1.0/(1<<29))
     609        for(int count = numcells; count--;)
     610          if(cellmask & (UINT64(1)<<count))
     611            GETFLOATSIGN(cp[count], 24, 1.0/(1U<<31))
     612        for(int count = numcells; count--;)
     613          if(cellmask & (UINT64(1)<<count))
     614            GETBITS(ll[count], 10)
     615        for(int count = numcells; count--;)
     616          if(cellmask & (UINT64(1)<<count))
     617            SKIPBITS(1)/*GETBITS(hc[count], 1)*/
     618        for(int count = numcells; count--;)
     619          if(cellmask & (UINT64(1)<<count))
     620            GETFLOAT(cnr[count], 10, 1.0/(1<<4))
     621        break;
     622      case 7:
     623        for(int count = numcells; count--;)
     624          if(cellmask & (UINT64(1)<<count))
     625            GETFLOATSIGN(psr[count], 20, 1.0/(1<<29))
     626        for(int count = numcells; count--;)
     627          if(cellmask & (UINT64(1)<<count))
     628            GETFLOATSIGN(cp[count], 24, 1.0/(1U<<31))
     629        for(int count = numcells; count--;)
     630          if(cellmask & (UINT64(1)<<count))
     631            GETBITS(ll[count], 10)
     632        for(int count = numcells; count--;)
     633          if(cellmask & (UINT64(1)<<count))
     634            SKIPBITS(1)/*GETBITS(hc[count], 1)*/
     635        for(int count = numcells; count--;)
     636          if(cellmask & (UINT64(1)<<count))
     637            GETFLOAT(cnr[count], 10, 1.0/(1<<4))
     638        for(int count = numcells; count--;)
     639          if(cellmask & (UINT64(1)<<count))
     640            GETFLOATSIGN(dop[count], 15, 0.0001)
     641        break;
     642      }
     643      i = RTCM3_MSM_NUMSAT;
     644      int j = -1;
     645      t_satObs CurrentObs;
     646      for(int count = numcells; count--;)
     647      {
     648        while(j >= 0 && !(sigmask&(1<<--j)))
     649          ;
     650        if(j < 0)
     651        {
     652          while(!(satmask&(UINT64(1)<<(--i)))) /* next satellite */
     653            ;
     654          if(CurrentObs._obs.size() > 0)
     655            _CurrentObsList.push_back(CurrentObs);
     656          CurrentObs.clear();
     657          CurrentObs._time = CurrentObsTime;
     658          if(sys == 'S')
     659            CurrentObs._prn.set(sys, 20-1+RTCM3_MSM_NUMSAT-i);
     660          else
     661            CurrentObs._prn.set(sys, RTCM3_MSM_NUMSAT-i);
     662          j = RTCM3_MSM_NUMSIG;
     663          while(!(sigmask&(1<<--j)))
     664            ;
     665          --numsat;
     666        }
     667        if(cellmask & (UINT64(1)<<count))
     668        {
     669          struct CodeData cd = {0.0,0};
     670          switch(sys)
     671          {
     672          case 'J':
     673            cd = qzss[RTCM3_MSM_NUMSIG-j-1];
     674            break;
     675          case 'C':
     676            cd = bds[RTCM3_MSM_NUMSIG-j-1];
     677            break;
     678          case 'G': case 'S':
     679            cd = gps[RTCM3_MSM_NUMSIG-j-1];
     680            break;
     681          case 'R':
     682            cd = glo[RTCM3_MSM_NUMSIG-j-1];
     683            {
     684              int k = GLOFreq[RTCM3_MSM_NUMSAT-i-1];
     685              if(extsat[numsat] < 14)
     686              {
     687                k = GLOFreq[RTCM3_MSM_NUMSAT-i-1] = 100+extsat[numsat]-7;
     688              }
     689              if(k)
     690                cd.wl = (cd.wl == 0.0 ? GLO_WAVELENGTH_L1(k-100) : GLO_WAVELENGTH_L2(k-100));
     691              else
     692                cd.code = 0;
     693            }
     694            break;
     695          case 'E':
     696            cd = gal[RTCM3_MSM_NUMSIG-j-1];
     697            break;
    169698          }
    170 
    171           // RTCMv3 antenna XYZ
    172           // ------------------
    173           else if (rr == 1005) {
    174             _antList.push_back(t_antInfo());
    175             _antList.back().type     = t_antInfo::ARP;
    176             _antList.back().xx       = parser.antX * 1e-4;
    177             _antList.back().yy       = parser.antY * 1e-4;
    178             _antList.back().zz       = parser.antZ * 1e-4;
    179             _antList.back().message  = rr;
    180 
    181             // Remember station position for 1003 message decoding
    182             _antXYZ[0] = parser.antX * 1e-4;
    183             _antXYZ[1] = parser.antY * 1e-4;
    184             _antXYZ[2] = parser.antZ * 1e-4;
    185           }
    186 
    187           // RTCMv3 antenna XYZ-H
    188           // --------------------
    189           else if(rr == 1006) {
    190             _antList.push_back(t_antInfo());
    191             _antList.back().type     = t_antInfo::ARP;
    192             _antList.back().xx       = parser.antX * 1e-4;
    193             _antList.back().yy       = parser.antY * 1e-4;
    194             _antList.back().zz       = parser.antZ * 1e-4;
    195             _antList.back().height   = parser.antH * 1e-4;
    196             _antList.back().height_f = true;
    197             _antList.back().message  = rr;
    198 
    199             // Remember station position for 1003 message decoding
    200             _antXYZ[0] = parser.antX * 1e-4;
    201             _antXYZ[1] = parser.antY * 1e-4;
    202             _antXYZ[2] = parser.antZ * 1e-4;
    203           }
    204 
    205           // GNSS Observations
    206           // -----------------
    207           else if (rr == 1 || rr == 2) {
    208             decoded = true;
    209    
    210             if (!parser.init) {
    211               HandleHeader(&parser);
    212               parser.init = 1;
     699          if(cd.code)
     700          {
     701            t_frqObs *frqObs = new t_frqObs;
     702            frqObs->_rnxType2ch = cd.code;
     703
     704            switch(type % 10)
     705            {
     706            case 1:
     707              if(psr[count] > -1.0/(1<<10))
     708              {
     709                frqObs->_code = psr[count]*LIGHTSPEED/1000.0
     710                +(rrmod[numsat])*LIGHTSPEED/1000.0;
     711                frqObs->_codeValid = true;
     712              }
     713              break;
     714            case 2:
     715              if(cp[count] > -1.0/(1<<8))
     716              {
     717                frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl
     718                +(rrmod[numsat])*LIGHTSPEED/1000.0/cd.wl;
     719                frqObs->_phaseValid = true;
     720                frqObs->_slipCounter = ll[count];
     721              }
     722              break;
     723            case 3:
     724              if(psr[count] > -1.0/(1<<10))
     725              {
     726                frqObs->_code = psr[count]*LIGHTSPEED/1000.0
     727                +(rrmod[numsat])*LIGHTSPEED/1000.0;
     728                frqObs->_codeValid = true;
     729              }
     730
     731              if(cp[count] > -1.0/(1<<8))
     732              {
     733                frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl
     734                +rrmod[numsat]*LIGHTSPEED/1000.0/cd.wl;
     735                frqObs->_phaseValid = true;
     736                frqObs->_slipCounter = ll[count];
     737              }
     738              break;
     739            case 4:
     740              if(psr[count] > -1.0/(1<<10))
     741              {
     742                frqObs->_code = psr[count]*LIGHTSPEED/1000.0
     743                +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0;
     744                frqObs->_codeValid = true;
     745              }
     746
     747              if(cp[count] > -1.0/(1<<8))
     748              {
     749                frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl
     750                +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0/cd.wl;
     751                frqObs->_phaseValid = true;
     752                frqObs->_slipCounter = ll[count];
     753              }
     754
     755              frqObs->_snr = cnr[count];
     756              frqObs->_snrValid = true;
     757              break;
     758            case 5:
     759              if(psr[count] > -1.0/(1<<10))
     760              {
     761                frqObs->_code = psr[count]*LIGHTSPEED/1000.0
     762                +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0;
     763                frqObs->_codeValid = true;
     764              }
     765
     766              if(cp[count] > -1.0/(1<<8))
     767              {
     768                frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl
     769                +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0/cd.wl;
     770                frqObs->_phaseValid = true;
     771                frqObs->_slipCounter = ll[count];
     772              }
     773
     774              frqObs->_snr = cnr[count];
     775              frqObs->_snrValid = true;
     776
     777              if(dop[count] > -1.6384)
     778              {
     779                frqObs->_doppler = -(dop[count]+rdop[numsat])/cd.wl;
     780                frqObs->_dopplerValid = true;
     781              }
     782              break;
     783            case 6:
     784              if(psr[count] > -1.0/(1<<10))
     785              {
     786                frqObs->_code = psr[count]*LIGHTSPEED/1000.0
     787                +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0;
     788                frqObs->_codeValid = true;
     789              }
     790
     791              if(cp[count] > -1.0/(1<<8))
     792              {
     793                frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl
     794                +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0/cd.wl;
     795                frqObs->_phaseValid = true;
     796                frqObs->_slipCounter = ll[count];
     797              }
     798
     799              frqObs->_snr = cnr[count];
     800              frqObs->_snrValid = true;
     801              break;
     802            case 7:
     803              if(psr[count] > -1.0/(1<<10))
     804              {
     805                frqObs->_code = psr[count]*LIGHTSPEED/1000.0
     806                +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0;
     807                frqObs->_codeValid = true;
     808              }
     809
     810              if(cp[count] > -1.0/(1<<8))
     811              {
     812                frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl
     813                +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0/cd.wl;
     814                frqObs->_phaseValid = true;
     815                frqObs->_slipCounter = ll[count];
     816              }
     817
     818              frqObs->_snr = cnr[count];
     819              frqObs->_snrValid = true;
     820
     821              if(dop[count] > -1.6384)
     822              {
     823                frqObs->_doppler = -(dop[count]+rdop[numsat])/cd.wl;
     824                frqObs->_dopplerValid = true;
     825              }
     826              break;
    213827            }
    214            
    215             if (rr == 2) {
    216               emit(newMessage( (_staID +
    217                ": No valid RINEX! All values are modulo 299792.458!").toAscii(),
    218                true));
    219             }
    220 
    221             gnssdata& gnssData = parser.Data;
    222            
    223             for (int iSat = 0; iSat < gnssData.numsats; iSat++) {
    224 
    225               t_satObs obs;
    226               int   satID = gnssData.satellites[iSat];
    227 
    228               // GPS
    229               // ---
    230               if      (satID >= PRN_GPS_START     && satID <= PRN_GPS_END) {
    231                 obs._prn.set('G', satID);
    232               }
    233 
    234               // Glonass
    235               // -------
    236               else if (satID >= PRN_GLONASS_START && satID <= PRN_GLONASS_END) {
    237                 obs._prn.set('R', satID - PRN_GLONASS_START + 1);
    238               }
    239 
    240               // Galileo
    241               // -------
    242               else if (satID >= PRN_GALILEO_START && satID <= PRN_GALILEO_END) {
    243                 obs._prn.set('E', satID - PRN_GALILEO_START + 1);
    244               }
    245 
    246               // SBAS
    247               // ----
    248               else if (satID >= PRN_SBAS_START && satID <= PRN_SBAS_END) {
    249                 obs._prn.set('S', satID - PRN_SBAS_START + 20);
    250               }
    251 
    252               // Giove A and B
    253               // -------------
    254               else if (satID >= PRN_GIOVE_START && satID <= PRN_GIOVE_END) {
    255                 obs._prn.set('E', satID - PRN_GIOVE_START + PRN_GIOVE_OFFSET);
    256               }
    257 
    258               // QZSS
    259               // -------------
    260               else if (satID >= PRN_QZSS_START && satID <= PRN_QZSS_END) {
    261                 obs._prn.set('J', satID - PRN_QZSS_START + 1);
    262               }
    263 
    264               // BDS
    265               // -------------
    266               else if (satID >= PRN_BDS_START && satID <= PRN_BDS_END) {
    267                 obs._prn.set('C', satID - PRN_BDS_START + 1);
    268               }
    269 
    270               // Unknown System
    271               // --------------
    272               else {
    273                 continue;
    274               }
    275 
    276               obs._time.set(gnssData.week, gnssData.timeofweek / 1000.0);
    277 
    278               QString prn(obs._prn.toString().c_str());
    279 
    280               int obs_slip_cnt_L1 = 0;
    281               int obs_slip_cnt_L2 = 0;
    282               int obs_slip_cnt_L5 = 0;
    283 
    284               // Handle loss-of-lock flags
    285               // -------------------------
    286               const int maxSlipCnt = 100;
    287               if (!_slip_cnt_L1.contains(prn)) {
    288                 _slip_cnt_L1[prn] = 0;
    289                 _slip_cnt_L2[prn] = 0;
    290                 _slip_cnt_L5[prn] = 0;
    291               }
    292               if (GNSSDF2_LOCKLOSSL1 & gnssData.dataflags2[iSat]) {
    293                 if (_slip_cnt_L1[prn] < maxSlipCnt) {
    294                   ++_slip_cnt_L1[prn];
    295                 }
    296                 else {
    297                   _slip_cnt_L1[prn] = 1;
    298                 }
    299                 obs_slip_cnt_L1 = _slip_cnt_L1[prn];
    300               }
    301               if (GNSSDF2_LOCKLOSSL2 & gnssData.dataflags2[iSat]) {
    302                 if (_slip_cnt_L2[prn] < maxSlipCnt) {
    303                   ++_slip_cnt_L2[prn];
    304                 }
    305                 else {
    306                   _slip_cnt_L2[prn] = 1;
    307                 }
    308                 obs_slip_cnt_L2 = _slip_cnt_L2[prn];
    309               }
    310               if (GNSSDF2_LOCKLOSSL5 & gnssData.dataflags2[iSat]) {
    311                 if (_slip_cnt_L5[prn] < maxSlipCnt) {
    312                   ++_slip_cnt_L5[prn];
    313                 }
    314                 else {
    315                   _slip_cnt_L5[prn] = 1;
    316                 }
    317                 obs_slip_cnt_L5 = _slip_cnt_L5[prn];
    318               }
    319 
    320               // Loop over all data types
    321               // ------------------------
    322               for (int iEntry = 0; iEntry < GNSSENTRY_NUMBER; ++iEntry) {
    323                 if (gnssData.codetype[iSat][iEntry] == 0) {
    324                   continue;
    325                 }
    326                 string rnxType(gnssData.codetype[iSat][iEntry]);
    327 
    328                 t_frqObs* frqObs = 0;
    329                 for (unsigned iFrq = 0; iFrq < obs._obs.size(); iFrq++) {
    330                   if (obs._obs[iFrq]->_rnxType2ch == rnxType) {
    331                     frqObs = obs._obs[iFrq];
    332                     break;
    333                   }
    334                 }
    335                 if (frqObs == 0) {
    336                   frqObs = new t_frqObs;
    337                   frqObs->_rnxType2ch = rnxType;
    338                   obs._obs.push_back(frqObs);
    339                 }
    340 
    341                 switch(iEntry & 3) {
    342                 case GNSSENTRY_CODE:
    343                   frqObs->_codeValid = true;
    344                   frqObs->_code      = gnssData.measdata[iSat][iEntry];
    345                   break;
    346                 case GNSSENTRY_PHASE:
    347                   frqObs->_phaseValid = true;
    348                   frqObs->_phase      = gnssData.measdata[iSat][iEntry];
    349                   if      (rnxType[0] == '1') {
    350                     frqObs->_slipCounter = obs_slip_cnt_L1;
    351                   }
    352                   else if (rnxType[0] == '2') {
    353                     frqObs->_slipCounter = obs_slip_cnt_L2;
    354                   }
    355                   else if (rnxType[0] == '5') {
    356                     frqObs->_slipCounter = obs_slip_cnt_L5;
    357                   }
    358                   break;
    359                 case GNSSENTRY_DOPPLER:
    360                   frqObs->_dopplerValid = true;
    361                   frqObs->_doppler      = gnssData.measdata[iSat][iEntry];
    362                   break;
    363                 case GNSSENTRY_SNR:
    364                   frqObs->_snrValid = true;
    365                   frqObs->_snr      = gnssData.measdata[iSat][iEntry];
    366                   break;
    367                 }
    368               }
    369               _obsList.push_back(obs);
    370             }
    371           }
    372    
    373           // GPS Ephemeris
    374           // -------------
    375           else if (rr == 1019) {
    376             decoded = true;
    377             t_ephGPS eph; eph.set(&parser.ephemerisGPS);
    378             emit newGPSEph(eph);
    379           }
    380    
    381           // GLONASS Ephemeris
    382           // -----------------
    383           else if (rr == 1020 && parser.ephemerisGLONASS.almanac_number >= 1 &&
    384                                  parser.ephemerisGLONASS.almanac_number <= PRN_GLONASS_NUM) {
    385             decoded = true;
    386             t_ephGlo eph; eph.set(&parser.ephemerisGLONASS);
    387             emit newGlonassEph(eph);
    388           }
    389 
    390           // Galileo Ephemeris
    391           // -----------------
    392           else if (rr == 1045 || rr == 1046) {
    393             decoded = true;
    394             t_ephGal eph; eph.set(&parser.ephemerisGALILEO);
    395             emit newGalileoEph(eph);
    396           }
    397 
    398           // QZSS Ephemeris
    399           // --------------
    400           else if (rr == 1044) {
    401             decoded = true;
    402             t_ephGPS eph; eph.set(&parser.ephemerisGPS);
    403             emit newGPSEph(eph);
    404           }
    405 
    406           // SBAS Ephemeris
    407           // --------------
    408           else if (rr == 1043) {
    409             decoded = true;
    410             t_ephSBAS eph; eph.set(&parser.ephemerisSBAS);
    411             emit newSBASEph(eph);
    412           }
    413 
    414           // BDS Ephemeris
    415           // -----------------
    416           else if (rr == RTCM3ID_BDS) {
    417             decoded = true;
    418             t_ephBDS eph; eph.set(&parser.ephemerisBDS);
    419             emit newBDSEph(eph);
     828            CurrentObs._obs.push_back(frqObs);
    420829          }
    421830        }
    422831      }
     832      if(CurrentObs._obs.size() > 0)
     833        _CurrentObsList.push_back(CurrentObs);
    423834    }
    424     if (!_rawFile && _mode == unknown && decoded) {
    425       _mode = observations;
     835  }
     836  else if((type % 10) < 3)
     837  {
     838    emit(newMessage(QString("%1: Block %2 contain partial data! Ignored!")
     839    .arg(_staID).arg(type).toAscii(), true));
     840  }
     841  if(!syncf)
     842  {
     843    decoded = true;
     844    _obsList = _CurrentObsList;
     845    _CurrentTime.reset();
     846    _CurrentObsList.clear();
     847  }
     848  return decoded;
     849}
     850
     851//
     852////////////////////////////////////////////////////////////////////////////
     853bool RTCM3Decoder::DecodeRTCM3GLONASS(unsigned char* data, int size)
     854{
     855  bool decoded = false;
     856  bncTime CurrentObsTime;
     857  int i, numsats, syncf, type;
     858  uint64_t numbits = 0, bitfield = 0;
     859
     860  data += 3; /* header */
     861  size -= 6; /* header + crc */
     862
     863  GETBITS(type, 12)
     864  SKIPBITS(12) /* id */
     865  GETBITS(i,27) /* tk */
     866
     867  CurrentObsTime.setTk(i);
     868  if(_CurrentTime.valid() && CurrentObsTime != _CurrentTime)
     869  {
     870    decoded = true;
     871    _obsList = _CurrentObsList;
     872    _CurrentObsList.clear();
     873  }
     874  _CurrentTime = CurrentObsTime;
     875
     876  GETBITS(syncf,1) /* sync */
     877  GETBITS(numsats,5)
     878  SKIPBITS(4) /* smind, smint */
     879
     880  while(numsats--)
     881  {
     882    int sv, code, l1range, amb=0, freq;
     883    t_satObs CurrentObs;
     884    CurrentObs._time = CurrentObsTime;
     885
     886    GETBITS(sv, 6)
     887    CurrentObs._prn.set('R', sv);
     888    GETBITS(code, 1)
     889    GETBITS(freq, 5)
     890    GLOFreq[sv-1] = 100+freq-7; /* store frequency for other users (MSM) */
     891
     892    t_frqObs *frqObs = new t_frqObs;
     893    /* L1 */
     894    frqObs->_rnxType2ch = code ? "1P" : "1C";
     895    GETBITS(l1range, 25);
     896    GETBITSSIGN(i, 20);
     897    if((i&((1<<20)-1)) != 0x80000)
     898    {
     899      frqObs->_code = l1range*0.02;
     900      frqObs->_phase = (l1range*0.02+i*0.0005)/GLO_WAVELENGTH_L1(freq-7);
     901      frqObs->_codeValid = frqObs->_phaseValid = true;
    426902    }
    427   }
    428 
    429   if (decoded) {
    430     return success;
    431   }
    432   else {
    433     return failure;
    434   }
     903    GETBITS(i, 7);
     904    frqObs->_slipCounter = i;
     905    if(type == 1010 || type == 1012)
     906    {
     907      GETBITS(amb,7);
     908      if(amb)
     909      {
     910        frqObs->_code += amb*599584.916;
     911        frqObs->_phase += (amb*599584.916)/GLO_WAVELENGTH_L1(freq-7);
     912      }
     913      GETBITS(i, 8);
     914      if(i)
     915      {
     916        frqObs->_snr = i*0.25;
     917        frqObs->_snrValid = true;
     918      }
     919    }
     920    CurrentObs._obs.push_back(frqObs);
     921    if(type == 1011 || type == 1012)
     922    {
     923      frqObs = new t_frqObs;
     924      /* L2 */
     925      GETBITS(code,2);
     926      switch(code)
     927      {
     928      case 3: frqObs->_rnxType2ch = "2P"; break;
     929      case 2: frqObs->_rnxType2ch = "2P"; break;
     930      case 1: frqObs->_rnxType2ch = "2P"; break;
     931      case 0: frqObs->_rnxType2ch = "2C"; break;
     932      }
     933      GETBITSSIGN(i,14);
     934      if((i&((1<<14)-1)) != 0x2000)
     935      {
     936        frqObs->_code = l1range*0.02+i*0.02+amb*599584.916;
     937        frqObs->_codeValid = true;
     938      }
     939      GETBITSSIGN(i,20);
     940      if((i&((1<<20)-1)) != 0x80000)
     941      {
     942        frqObs->_phase = (l1range*0.02+i*0.0005+amb*599584.916)/GLO_WAVELENGTH_L2(freq-7);
     943        frqObs->_phaseValid = true;
     944      }
     945      GETBITS(i,7);
     946      frqObs->_slipCounter = i;
     947      if(type == 1012)
     948      {
     949        GETBITS(i, 8);
     950        if(i)
     951        {
     952          frqObs->_snr = i*0.25;
     953          frqObs->_snrValid = true;
     954        }
     955      }
     956      CurrentObs._obs.push_back(frqObs);
     957    }
     958    _CurrentObsList.push_back(CurrentObs);
     959  }
     960  if(!syncf)
     961  {
     962    decoded = true;
     963    _obsList = _CurrentObsList;
     964    _CurrentTime.reset();
     965    _CurrentObsList.clear();
     966  }
     967  return decoded;
     968}
     969
     970//
     971////////////////////////////////////////////////////////////////////////////
     972bool RTCM3Decoder::DecodeGPSEphemeris(unsigned char* data, int size)
     973{
     974  bool decoded = false;
     975
     976  if(size == 67)
     977  {
     978    t_ephGPS eph;
     979    int i, week;
     980    uint64_t numbits = 0, bitfield = 0;
     981
     982    data += 3; /* header */
     983    size -= 6; /* header + crc */
     984    SKIPBITS(12)
     985
     986    eph._receptDateTime = currentDateAndTimeGPS();
     987
     988    GETBITS(i, 6)
     989    eph._prn.set('G', i);
     990    GETBITS(week, 10)
     991    week += 1024;
     992    GETBITS(i, 4)
     993    eph._ura = accuracyFromIndex(i, eph.type());
     994    GETBITS(eph._L2Codes, 2)
     995    GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
     996    GETBITS(eph._IODE, 8)
     997    GETBITS(i, 16)
     998    i <<= 4;
     999    eph._TOC.set(i*1000);
     1000    GETFLOATSIGN(eph._clock_driftrate, 8, 1.0/(double)(1<<30)/(double)(1<<25))
     1001    GETFLOATSIGN(eph._clock_drift, 16, 1.0/(double)(1<<30)/(double)(1<<13))
     1002    GETFLOATSIGN(eph._clock_bias, 22, 1.0/(double)(1<<30)/(double)(1<<1))
     1003    GETBITS(eph._IODC, 10)
     1004    GETFLOATSIGN(eph._Crs, 16, 1.0/(double)(1<<5))
     1005    GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
     1006    GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
     1007    GETFLOATSIGN(eph._Cuc, 16, 1.0/(double)(1<<29))
     1008    GETFLOAT(eph._e, 32, 1.0/(double)(1<<30)/(double)(1<<3))
     1009    GETFLOATSIGN(eph._Cus, 16, 1.0/(double)(1<<29))
     1010    GETFLOAT(eph._sqrt_A, 32, 1.0/(double)(1<<19))
     1011    GETBITS(i, 16)
     1012    i <<= 4;
     1013    eph._TOEsec = i;
     1014    bncTime t;
     1015    t.set(i*1000);
     1016    eph._TOEweek = t.gpsw();
     1017    /* week from HOW, differs from TOC, TOE week, we use adapted value instead */
     1018    if(eph._TOEweek > week + 1 || eph._TOEweek < week-1) /* invalid week */
     1019      return false;
     1020    GETFLOATSIGN(eph._Cic, 16, 1.0/(double)(1<<29))
     1021    GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
     1022    GETFLOATSIGN(eph._Cis, 16, 1.0/(double)(1<<29))
     1023    GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
     1024    GETFLOATSIGN(eph._Crc, 16, 1.0/(double)(1<<5))
     1025    GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
     1026    GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
     1027    GETFLOATSIGN(eph._TGD, 8, 1.0/(double)(1<<30)/(double)(1<<1))
     1028    GETBITS(eph._health, 6)
     1029    GETBITS(eph._L2PFlag, 1)
     1030    GETBITS(eph._fitInterval, 1)
     1031    eph._TOT = 0.9999e9;
     1032
     1033    emit newGPSEph(eph);
     1034    decoded = true;
     1035  }
     1036  return decoded;
     1037}
     1038
     1039//
     1040////////////////////////////////////////////////////////////////////////////
     1041bool RTCM3Decoder::DecodeGLONASSEphemeris(unsigned char* data, int size)
     1042{
     1043  bool decoded = false;
     1044
     1045  if(size == 51)
     1046  {
     1047    t_ephGlo eph;
     1048    int sv, i, tk;
     1049    uint64_t numbits = 0, bitfield = 0;
     1050
     1051    data += 3; /* header */
     1052    size -= 6; /* header + crc */
     1053    SKIPBITS(12)
     1054
     1055    eph._receptDateTime = currentDateAndTimeGPS();
     1056
     1057    GETBITS(sv, 6)
     1058    eph._prn.set('R', sv);
     1059
     1060    GETBITS(i, 5)
     1061    eph._frequency_number = i-7;
     1062    GLOFreq[sv-1] = 100+i-7; /* store frequency for other users (MSM) */
     1063
     1064    SKIPBITS(4) /* almanac healthy, almanac health ok, P1 */
     1065    GETBITS(i, 5)
     1066    tk = i*60*60;
     1067    GETBITS(i, 6)
     1068    tk += i*60;
     1069    GETBITS(i, 1)
     1070    tk += i*30;
     1071    eph._tki = tk < 3*60*60 ? tk-3*60*60+86400 : tk-3*60*60;
     1072    GETBITS(eph._health, 1)
     1073    SKIPBITS(1) /* P2 */
     1074    GETBITS(i, 7)
     1075    eph._TOC.setTk(i*15*60*1000); /* tb */
     1076
     1077    GETFLOATSIGNM(eph._x_velocity, 24, 1.0/(double)(1<<20))
     1078    GETFLOATSIGNM(eph._x_pos, 27, 1.0/(double)(1<<11))
     1079    GETFLOATSIGNM(eph._x_acceleration, 5, 1.0/(double)(1<<30))
     1080    GETFLOATSIGNM(eph._y_velocity, 24, 1.0/(double)(1<<20))
     1081    GETFLOATSIGNM(eph._y_pos, 27, 1.0/(double)(1<<11))
     1082    GETFLOATSIGNM(eph._y_acceleration, 5, 1.0/(double)(1<<30))
     1083    GETFLOATSIGNM(eph._z_velocity, 24, 1.0/(double)(1<<20))
     1084    GETFLOATSIGNM(eph._z_pos, 27, 1.0/(double)(1<<11))
     1085    GETFLOATSIGNM(eph._z_acceleration, 5, 1.0/(double)(1<<30))
     1086    SKIPBITS(1) /* P3 */
     1087    GETFLOATSIGNM(eph._gamma, 11, 1.0/(double)(1<<30)/(double)(1<<10))
     1088    SKIPBITS(3) /* GLONASS-M P, GLONASS-M ln (third string) */
     1089    GETFLOATSIGNM(eph._tau, 22, 1.0/(double)(1<<30)) /* GLONASS tau n(tb) */
     1090    SKIPBITS(5) /* GLONASS-M delta tau n(tb) */
     1091    GETBITS(eph._E, 5)
     1092    /* GETBITS(i, 1) / * GLONASS-M P4 */
     1093    /* GETBITS(i, 4) / * GLONASS-M Ft */
     1094    /* GETBITS(i, 11) / * GLONASS-M Nt */
     1095    /* GETBITS(i, 2) / * GLONASS-M M */
     1096    /* GETBITS(i, 1) / * GLONASS-M The Availability of Additional Data */
     1097    /* GETBITS(i, 11) / * GLONASS-M Na */
     1098    /* GETFLOATSIGNM(i, 32, 1.0/(double)(1<<30)/(double)(1<<1)) / * GLONASS tau c */
     1099    /* GETBITS(i, 5) / * GLONASS-M N4 */
     1100    /* GETFLOATSIGNM(i, 22, 1.0/(double)(1<<30)) / * GLONASS-M tau GPS */
     1101    /* GETBITS(i, 1) / * GLONASS-M ln (fifth string) */
     1102
     1103    unsigned year, month, day;
     1104    eph._TOC.civil_date(year, month, day);
     1105    eph._gps_utc = gnumleap(year, month, day);
     1106    eph._tt = eph._TOC;
     1107
     1108    eph._xv(1) = eph._x_pos * 1.e3;
     1109    eph._xv(2) = eph._y_pos * 1.e3;
     1110    eph._xv(3) = eph._z_pos * 1.e3;
     1111    eph._xv(4) = eph._x_velocity * 1.e3;
     1112    eph._xv(5) = eph._y_velocity * 1.e3;
     1113    eph._xv(6) = eph._z_velocity * 1.e3;
     1114
     1115    emit newGlonassEph(eph);
     1116    decoded = true;
     1117  }
     1118  return decoded;
     1119}
     1120
     1121//
     1122////////////////////////////////////////////////////////////////////////////
     1123bool RTCM3Decoder::DecodeQZSSEphemeris(unsigned char* data, int size)
     1124{
     1125  bool decoded = false;
     1126
     1127  if(size == 67)
     1128  {
     1129    t_ephGPS eph;
     1130    int i, week;
     1131    uint64_t numbits = 0, bitfield = 0;
     1132
     1133    data += 3; /* header */
     1134    size -= 6; /* header + crc */
     1135    SKIPBITS(12)
     1136
     1137    eph._receptDateTime = currentDateAndTimeGPS();
     1138
     1139    GETBITS(i, 4)
     1140    eph._prn.set('J', i);
     1141
     1142    GETBITS(i, 16)
     1143    i <<= 4;
     1144    eph._TOC.set(i*1000);
     1145
     1146    GETFLOATSIGN(eph._clock_driftrate, 8, 1.0/(double)(1<<30)/(double)(1<<25))
     1147    GETFLOATSIGN(eph._clock_drift, 16, 1.0/(double)(1<<30)/(double)(1<<13))
     1148    GETFLOATSIGN(eph._clock_bias, 22, 1.0/(double)(1<<30)/(double)(1<<1))
     1149    GETBITS(eph._IODE, 8)
     1150    GETFLOATSIGN(eph._Crs, 16, 1.0/(double)(1<<5))
     1151    GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
     1152    GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
     1153    GETFLOATSIGN(eph._Cuc, 16, 1.0/(double)(1<<29))
     1154    GETFLOAT(eph._e, 32, 1.0/(double)(1<<30)/(double)(1<<3))
     1155    GETFLOATSIGN(eph._Cus, 16, 1.0/(double)(1<<29))
     1156    GETFLOAT(eph._sqrt_A, 32, 1.0/(double)(1<<19))
     1157    GETBITS(i, 16)
     1158    i <<= 4;
     1159    eph._TOEsec = i;
     1160    bncTime t;
     1161    t.set(i);
     1162
     1163    GETFLOATSIGN(eph._Cic, 16, 1.0/(double)(1<<29))
     1164    GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
     1165    GETFLOATSIGN(eph._Cis, 16, 1.0/(double)(1<<29))
     1166    GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
     1167    GETFLOATSIGN(eph._Crc, 16, 1.0/(double)(1<<5))
     1168    GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
     1169    GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
     1170    GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
     1171    GETBITS(eph._L2Codes, 2)
     1172    GETBITS(week, 10)
     1173    week += 1024;
     1174    eph._TOEweek = t.gpsw();
     1175    /* week from HOW, differs from TOC, TOE week, we use adapted value instead */
     1176    if(eph._TOEweek > week + 1 || eph._TOEweek < week-1) /* invalid week */
     1177      return false;
     1178
     1179    GETBITS(i, 4)
     1180    if(i <= 6)
     1181      eph._ura = ceil(10.0*pow(2.0, 1.0+i/2.0))/10.0;
     1182    else
     1183      eph._ura = ceil(10.0*pow(2.0, i/2.0))/10.0;
     1184    GETBITS(eph._health, 6)
     1185    GETFLOATSIGN(eph._TGD, 8, 1.0/(double)(1<<30)/(double)(1<<1))
     1186    GETBITS(eph._IODC, 10)
     1187    GETBITS(eph._fitInterval, 1)
     1188    eph._TOT = 0.9999e9;
     1189    eph._L2PFlag = 0; /* does not exist for QZSS */
     1190
     1191    emit newGPSEph(eph);
     1192    decoded = true;
     1193  }
     1194  return decoded;
     1195}
     1196
     1197//
     1198////////////////////////////////////////////////////////////////////////////
     1199bool RTCM3Decoder::DecodeSBASEphemeris(unsigned char* data, int size)
     1200{
     1201  bool decoded = false;
     1202
     1203  if(size == 35)
     1204  {
     1205    t_ephSBAS eph;
     1206    int i;
     1207    uint64_t numbits = 0, bitfield = 0;
     1208
     1209    data += 3; /* header */
     1210    size -= 6; /* header + crc */
     1211    SKIPBITS(12)
     1212
     1213    eph._receptDateTime = currentDateAndTimeGPS();
     1214
     1215    GETBITS(i, 6)
     1216    eph._prn.set('S', 20+i);
     1217    GETBITS(eph._IODN, 8)
     1218    GETBITS(i, 13)
     1219    i <<= 4;
     1220    eph._TOC.setTOD(i*1000);
     1221    GETBITS(i, 4)
     1222    eph._ura = accuracyFromIndex(i, eph.type());
     1223    GETFLOATSIGN(eph._x_pos, 30, 0.08)
     1224    GETFLOATSIGN(eph._y_pos, 30, 0.08)
     1225    GETFLOATSIGN(eph._z_pos, 25, 0.4)
     1226    GETFLOATSIGN(eph._x_velocity, 17, 0.000625)
     1227    GETFLOATSIGN(eph._y_velocity, 17, 0.000625)
     1228    GETFLOATSIGN(eph._z_velocity, 18, 0.004)
     1229    GETFLOATSIGN(eph._x_acceleration, 10, 0.0000125)
     1230    GETFLOATSIGN(eph._y_acceleration, 10, 0.0000125)
     1231    GETFLOATSIGN(eph._z_acceleration, 10, 0.0000625)
     1232    GETFLOATSIGN(eph._agf0, 12, 1.0/(1<<30)/(1<<1))
     1233    GETFLOATSIGN(eph._agf1, 8, 1.0/(1<<30)/(1<<10))
     1234
     1235    eph._TOW = 0.9999E9;
     1236    eph._health = 0;
     1237
     1238    emit newSBASEph(eph);
     1239    decoded = true;
     1240  }
     1241  return decoded;
     1242}
     1243
     1244//
     1245////////////////////////////////////////////////////////////////////////////
     1246bool RTCM3Decoder::DecodeGalileoEphemeris(unsigned char* data, int size)
     1247{
     1248  bool decoded = false;
     1249  uint64_t numbits = 0, bitfield = 0;
     1250  int i;
     1251
     1252  data += 3; /* header */
     1253  size -= 6; /* header + crc */
     1254  GETBITS(i, 12)
     1255
     1256  if((i == 1046 && size == 61) || (i == 1045 && size == 60))
     1257  {
     1258    t_ephGal eph;
     1259
     1260    eph._receptDateTime = currentDateAndTimeGPS();
     1261
     1262    eph._inav = (i == 1046);
     1263    eph._fnav = (i == 1045);
     1264    GETBITS(i, 6)
     1265    eph._prn.set('E', i, eph._inav ? 1 : 0);
     1266
     1267    GETBITS(eph._TOEweek, 12)
     1268    GETBITS(eph._IODnav, 10)
     1269    GETBITS(i, 8)
     1270    eph._SISA = accuracyFromIndex(i, eph.type());
     1271    GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
     1272    GETBITSFACTOR(i, 14, 60)
     1273    eph._TOC.set(eph._TOEweek, i);
     1274    GETFLOATSIGN(eph._clock_driftrate, 6, 1.0/(double)(1<<30)/(double)(1<<29))
     1275    GETFLOATSIGN(eph._clock_drift, 21, 1.0/(double)(1<<30)/(double)(1<<16))
     1276    GETFLOATSIGN(eph._clock_bias, 31, 1.0/(double)(1<<30)/(double)(1<<4))
     1277    GETFLOATSIGN(eph._Crs, 16, 1.0/(double)(1<<5))
     1278    GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
     1279    GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
     1280    GETFLOATSIGN(eph._Cuc, 16, 1.0/(double)(1<<29))
     1281    GETFLOAT(eph._e, 32, 1.0/(double)(1<<30)/(double)(1<<3))
     1282    GETFLOATSIGN(eph._Cus, 16, 1.0/(double)(1<<29))
     1283    GETFLOAT(eph._sqrt_A, 32, 1.0/(double)(1<<19))
     1284    GETBITSFACTOR(eph._TOEsec, 14, 60)
     1285    /* FIXME: overwrite value, copied from old code */
     1286    eph._TOEsec = eph._TOC.gpssec();
     1287    GETFLOATSIGN(eph._Cic, 16, 1.0/(double)(1<<29))
     1288    GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
     1289    GETFLOATSIGN(eph._Cis, 16, 1.0/(double)(1<<29))
     1290    GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
     1291    GETFLOATSIGN(eph._Crc, 16, 1.0/(double)(1<<5))
     1292    GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
     1293    GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
     1294    GETFLOATSIGN(eph._BGD_1_5A, 10, 1.0/(double)(1<<30)/(double)(1<<2))
     1295    if(eph._inav)
     1296    {
     1297      /* set usused F/NAV values */
     1298      eph._E5aHS = 0.0;
     1299      eph._e5aDataInValid = false;
     1300
     1301      GETFLOATSIGN(eph._BGD_1_5B, 10, 1.0/(double)(1<<30)/(double)(1<<2))
     1302      GETBITS(eph._E5bHS, 2)
     1303      GETBITS(eph._e5bDataInValid, 1)
     1304      GETBITS(eph._E1_bHS, 2)
     1305      GETBITS(eph._e1DataInValid, 1)
     1306    }
     1307    else
     1308    {
     1309      /* set usused I/NAV values */
     1310      eph._BGD_1_5B = 0.0;
     1311      eph._E5bHS = 0.0;
     1312      eph._E1_bHS = 0.0;
     1313      eph._e1DataInValid = false;
     1314      eph._e5bDataInValid = false;
     1315
     1316      GETBITS(eph._E5aHS, 2)
     1317      GETBITS(eph._e5aDataInValid, 1)
     1318    }
     1319    eph._TOT = 0.9999e9;
     1320
     1321    emit newGalileoEph(eph);
     1322    decoded = true;
     1323  }
     1324  return decoded;
     1325}
     1326
     1327//
     1328////////////////////////////////////////////////////////////////////////////
     1329bool RTCM3Decoder::DecodeBDSEphemeris(unsigned char* data, int size)
     1330{
     1331  bool decoded = false;
     1332
     1333  if(size == 70)
     1334  {
     1335    t_ephBDS eph;
     1336    int i;
     1337    uint64_t numbits = 0, bitfield = 0;
     1338
     1339    data += 3; /* header */
     1340    size -= 6; /* header + crc */
     1341    SKIPBITS(12)
     1342
     1343    eph._receptDateTime = currentDateAndTimeGPS();
     1344
     1345    GETBITS(i, 6)
     1346    eph._prn.set('C', i);
     1347
     1348    SKIPBITS(13) /* week */
     1349    GETBITS(i, 4)
     1350    eph._URA = accuracyFromIndex(i, eph.type());
     1351    GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
     1352    GETBITS(eph._AODE, 5)
     1353    GETBITS(i, 17)
     1354    i <<= 3;
     1355    eph._TOC.setBDS(i*1000);
     1356    GETFLOATSIGN(eph._clock_driftrate, 11, 1.0/(double)(1<<30)/(double)(1<<30)/(double)(1<<6))
     1357    GETFLOATSIGN(eph._clock_drift, 22, 1.0/(double)(1<<30)/(double)(1<<20))
     1358    GETFLOATSIGN(eph._clock_bias, 24, 1.0/(double)(1<<30)/(double)(1<<3))
     1359    GETBITS(eph._AODC, 5)
     1360    GETFLOATSIGN(eph._Crs, 18, 1.0/(double)(1<<6))
     1361    GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
     1362    GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
     1363    GETFLOATSIGN(eph._Cuc, 18, 1.0/(double)(1<<30)/(double)(1<<1))
     1364    GETFLOAT(eph._e, 32, 1.0/(double)(1<<30)/(double)(1<<3))
     1365    GETFLOATSIGN(eph._Cus, 18, 1.0/(double)(1<<30)/(double)(1<<1))
     1366    GETFLOAT(eph._sqrt_A, 32, 1.0/(double)(1<<19))
     1367    GETBITS(i, 17)
     1368    i <<= 3;
     1369    eph._TOEsec = i;
     1370    eph._TOE.setBDS(i*1000);
     1371    GETFLOATSIGN(eph._Cic, 18, 1.0/(double)(1<<30)/(double)(1<<1))
     1372    GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
     1373    GETFLOATSIGN(eph._Cis, 18, 1.0/(double)(1<<30)/(double)(1<<1))
     1374    GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
     1375    GETFLOATSIGN(eph._Crc, 18, 1.0/(double)(1<<6))
     1376    GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
     1377    GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
     1378    GETFLOATSIGN(eph._TGD1, 10, 0.0000000001)
     1379    GETFLOATSIGN(eph._TGD2, 10, 0.0000000001)
     1380    GETBITS(eph._SatH1, 1)
     1381
     1382    eph._TOW = 0.9999E9;
     1383    emit newBDSEph(eph);
     1384    decoded = true;
     1385  }
     1386  return decoded;
     1387}
     1388
     1389//
     1390////////////////////////////////////////////////////////////////////////////
     1391bool RTCM3Decoder::DecodeAntenna(unsigned char* data, int size)
     1392{
     1393  char *antenna;
     1394  int antnum;
     1395  uint64_t numbits = 0, bitfield = 0;
     1396
     1397  data += 3; /* header */
     1398  size -= 6; /* header + crc */
     1399
     1400  SKIPBITS(12)
     1401  GETSTRING(antnum,antenna)
     1402  _antType.push_back(antenna);
     1403
     1404  return true;
     1405}
     1406
     1407//
     1408////////////////////////////////////////////////////////////////////////////
     1409bool RTCM3Decoder::DecodeAntennaPosition(unsigned char* data, int size)
     1410{
     1411  int type;
     1412  uint64_t numbits = 0, bitfield = 0;
     1413  double x, y, z;
     1414
     1415  data += 3; /* header */
     1416  size -= 6; /* header + crc */
     1417
     1418  GETBITS(type, 12)
     1419  _antList.push_back(t_antInfo());
     1420  _antList.back().type = t_antInfo::ARP;
     1421  SKIPBITS(22)
     1422  GETBITSSIGN(x, 38)
     1423  _antList.back().xx = x * 1e-4;
     1424  SKIPBITS(2)
     1425  GETBITSSIGN(y, 38)
     1426  _antList.back().yy = y * 1e-4;
     1427  SKIPBITS(2)
     1428  GETBITSSIGN(z, 38)
     1429  _antList.back().zz = z * 1e-4;
     1430  if(type == 1006)
     1431  {
     1432    double h;
     1433    GETBITS(h, 16)
     1434    _antList.back().height = h * 1e-4;
     1435    _antList.back().height_f = true;
     1436  }
     1437  _antList.back().message  = type;
     1438
     1439  return true;
     1440}
     1441
     1442//
     1443////////////////////////////////////////////////////////////////////////////
     1444t_irc RTCM3Decoder::Decode(char* buffer, int bufLen, vector<string>& errmsg)
     1445{
     1446  bool decoded = false;
     1447
     1448  errmsg.clear();
     1449
     1450  while(bufLen && _MessageSize < sizeof(_Message))
     1451  {
     1452    int l = sizeof(_Message) - _MessageSize;
     1453    if(l > bufLen)
     1454      l = bufLen;
     1455    memcpy(_Message+_MessageSize, buffer, l);
     1456    _MessageSize += l;
     1457    bufLen -= l;
     1458    buffer += l;
     1459    int id;
     1460    while((id = GetMessage()))
     1461    {
     1462      /* reset station ID for file loading as it can change */
     1463      if(_rawFile)
     1464        _staID = _rawFile->staID();
     1465      /* store the id into the list of loaded blocks */
     1466      _typeList.push_back(id);
     1467
     1468      /* Clock and orbit data handled in another function, already pass the
     1469       * extracted data block. That does no harm, as it anyway skip everything
     1470       * else. */
     1471      if((id >= 1057 && id <= 1068) || (id >= 1240 && id <= 1263))
     1472      {
     1473        if (!_coDecoders.contains(_staID.toAscii()))
     1474          _coDecoders[_staID.toAscii()] = new RTCM3coDecoder(_staID);
     1475        RTCM3coDecoder* coDecoder = _coDecoders[_staID.toAscii()];
     1476        if(coDecoder->Decode(reinterpret_cast<char *>(_Message), _BlockSize,
     1477        errmsg) == success)
     1478        {
     1479          decoded = true;
     1480        }
     1481      }
     1482      else if(id >= 1070 && id <= 1229) /* MSM */
     1483      {
     1484        if(DecodeRTCM3MSM(_Message, _BlockSize))
     1485          decoded = true;
     1486      }
     1487      else
     1488      {
     1489        switch(id)
     1490        {
     1491        case 1001: case 1003:
     1492          emit(newMessage(QString("%1: Block %2 contain partial data! Ignored!")
     1493          .arg(_staID).arg(id).toAscii(), true));
     1494          break; /* no use decoding partial data ATM, remove break when data can be used */
     1495        case 1002: case 1004:
     1496          if(DecodeRTCM3GPS(_Message, _BlockSize))
     1497            decoded = true;
     1498          break;
     1499        case 1009: case 1011:
     1500          emit(newMessage(QString("%1: Block %2 contain partial data! Ignored!")
     1501          .arg(_staID).arg(id).toAscii(), true));
     1502          break; /* no use decoding partial data ATM, remove break when data can be used */
     1503        case 1010: case 1012:
     1504          if(DecodeRTCM3GLONASS(_Message, _BlockSize))
     1505            decoded = true;
     1506          break;
     1507        case 1019:
     1508          if(DecodeGPSEphemeris(_Message, _BlockSize))
     1509            decoded = true;
     1510          break;
     1511        case 1020:
     1512          if(DecodeGLONASSEphemeris(_Message, _BlockSize))
     1513            decoded = true;
     1514          break;
     1515        case 1043:
     1516          if(DecodeSBASEphemeris(_Message, _BlockSize))
     1517            decoded = true;
     1518          break;
     1519        case 1044:
     1520          if(DecodeQZSSEphemeris(_Message, _BlockSize))
     1521            decoded = true;
     1522          break;
     1523        case 1045: case 1046:
     1524          if(DecodeGalileoEphemeris(_Message, _BlockSize))
     1525            decoded = true;
     1526          break;
     1527        case RTCM3ID_BDS:
     1528          if(DecodeBDSEphemeris(_Message, _BlockSize))
     1529            decoded = true;
     1530          break;
     1531        case 1007: case 1008: case 1033:
     1532          DecodeAntenna(_Message, _BlockSize);
     1533          break;
     1534        case 1005: case 1006:
     1535          DecodeAntennaPosition(_Message, _BlockSize);
     1536          break;
     1537        }
     1538      }
     1539    }
     1540  }
     1541  return decoded ? success : failure;
     1542};
     1543
     1544//
     1545////////////////////////////////////////////////////////////////////////////
     1546uint32_t RTCM3Decoder::CRC24(long size, const unsigned char *buf)
     1547{
     1548  uint32_t crc = 0;
     1549  int i;
     1550
     1551  while(size--)
     1552  {
     1553    crc ^= (*buf++) << (16);
     1554    for(i = 0; i < 8; i++)
     1555    {
     1556      crc <<= 1;
     1557      if(crc & 0x1000000)
     1558        crc ^= 0x01864cfb;
     1559    }
     1560  }
     1561  return crc;
     1562}
     1563
     1564//
     1565////////////////////////////////////////////////////////////////////////////
     1566int RTCM3Decoder::GetMessage(void)
     1567{
     1568  unsigned char *m, *e;
     1569  int i;
     1570
     1571  m = _Message+_SkipBytes;
     1572  e = _Message+_MessageSize;
     1573  _NeedBytes = _SkipBytes = 0;
     1574  while(e-m >= 3)
     1575  {
     1576    if(m[0] == 0xD3)
     1577    {
     1578      _BlockSize = ((m[1]&3)<<8)|m[2];
     1579      if(e-m >= static_cast<int>(_BlockSize+6))
     1580      {
     1581        if(static_cast<uint32_t>((m[3+_BlockSize]<<16)|(m[3+_BlockSize+1]<<8)
     1582        |(m[3+_BlockSize+2])) == CRC24(_BlockSize+3, m))
     1583        {
     1584          _BlockSize +=6;
     1585          _SkipBytes = _BlockSize;
     1586          break;
     1587        }
     1588        else
     1589          ++m;
     1590      }
     1591      else
     1592      {
     1593        _NeedBytes = _BlockSize;
     1594        break;
     1595      }
     1596    }
     1597    else
     1598      ++m;
     1599  }
     1600  if(e-m < 3)
     1601    _NeedBytes = 3;
     1602
     1603  /* copy buffer to front */
     1604  i = m - _Message;
     1605  if(i && m < e)
     1606    memmove(_Message, m, static_cast<size_t>(_MessageSize-i));
     1607  _MessageSize -= i;
     1608
     1609  return !_NeedBytes ? ((_Message[3]<<4)|(_Message[4]>>4)) : 0;
    4351610}
    4361611
    … …  
    4381613//////////////////////////////////////////////////////////////////////////////
    4391614int RTCM3Decoder::corrGPSEpochTime() const {
    440   if (_mode == corrections && _coDecoders.size() > 0) {
    441     return _coDecoders.begin().value()->corrGPSEpochTime();
    442   }
    443   else {
    444     return -1;
    445   }
     1615  return _coDecoders.size() > 0 ? _coDecoders.begin().value()->corrGPSEpochTime() : -1;
    4461616}
Note: See TracChangeset for help on using the changeset viewer.