1  // Part of BNC, a utility for retrieving decoding and


2  // converting GNSS data streams from NTRIP broadcasters.


3  //


4  // Copyright (C) 2007


5  // German Federal Agency for Cartography and Geodesy (BKG)


6  // http://www.bkg.bund.de


7  // Czech Technical University Prague, Department of Geodesy


8  // http://www.fsv.cvut.cz


9  //


10  // Email: eurefip@bkg.bund.de


11  //


12  // This program is free software; you can redistribute it and/or


13  // modify it under the terms of the GNU General Public License


14  // as published by the Free Software Foundation, version 2.


15  //


16  // This program is distributed in the hope that it will be useful,


17  // but WITHOUT ANY WARRANTY; without even the implied warranty of


18  // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the


19  // GNU General Public License for more details.


20  //


21  // You should have received a copy of the GNU General Public License


22  // along with this program; if not, write to the Free Software


23  // Foundation, Inc., 59 Temple Place  Suite 330, Boston, MA 021111307, USA.


24 


25  /* 


26  * BKG NTRIP Client


27  * 


28  *


29  * Class: RTCM3Decoder


30  *


31  * Purpose: RTCM3 Decoder


32  *


33  * Author: L. Mervart


34  *


35  * Created: 24Aug2006


36  *


37  * Changes:


38  *


39  * */


40 


41  #include <iostream>


42  #include <iomanip>


43  #include <sstream>


44  #include <math.h>


45  #include <string.h>


46 


47  #include "bits.h"


48  #include "gnss.h"


49  #include "RTCM3Decoder.h"


50  #include "rtcm_utils.h"


51  #include "bncconst.h"


52  #include "bnccore.h"


53  #include "bncutils.h"


54  #include "bncsettings.h"


55 


56  using namespace std;


57 


58  // Error Handling


59  ////////////////////////////////////////////////////////////////////////////


60  void RTCM3Error(const char*, ...) {


61  }


62 


63  // Constructor


64  ////////////////////////////////////////////////////////////////////////////


65  RTCM3Decoder::RTCM3Decoder(const QString& staID, bncRawFile* rawFile) :


66  GPSDecoder() {


67 


68  _staID = staID;


69  _rawFile = rawFile;


70 


71  connect(this, SIGNAL(newGPSEph(t_ephGPS)), BNC_CORE, SLOT(slotNewGPSEph(t_ephGPS)));


72  connect(this, SIGNAL(newGlonassEph(t_ephGlo)), BNC_CORE, SLOT(slotNewGlonassEph(t_ephGlo)));


73  connect(this, SIGNAL(newGalileoEph(t_ephGal)), BNC_CORE, SLOT(slotNewGalileoEph(t_ephGal)));


74  connect(this, SIGNAL(newSBASEph(t_ephSBAS)), BNC_CORE, SLOT(slotNewSBASEph(t_ephSBAS)));


75  connect(this, SIGNAL(newBDSEph(t_ephBDS)), BNC_CORE, SLOT(slotNewBDSEph(t_ephBDS)));


76 


77  _MessageSize = _SkipBytes = _BlockSize = _NeedBytes = 0;


78  }


79 


80  // Destructor


81  ////////////////////////////////////////////////////////////////////////////


82  RTCM3Decoder::~RTCM3Decoder() {


83  QMapIterator<QByteArray, RTCM3coDecoder*> it(_coDecoders);


84  while(it.hasNext())


85  {


86  it.next();


87  delete it.value();


88  }


89  }


90 


91  //


92  ////////////////////////////////////////////////////////////////////////////


93  bool RTCM3Decoder::DecodeRTCM3GPS(unsigned char* data, int size)


94  {


95  bool decoded = false;


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', sv20);


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;


143  }


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;


159  }


160  }


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  */


219  static int GLOFreq[RTCM3_MSM_NUMSAT];


220 


221  /*


222  * Storage structure to store frequency and RINEX ID assignment for MSM


223  * message */


224  struct CodeData {


225  double wl;


226  const char *code; /* currently unused */


227  };


228 


229  /** MSM signal types for GPS and SBAS */


230  static 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  */


271  static 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 */


308  static 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 */


345  static 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 */


382  static struct CodeData bds[RTCM3_MSM_NUMSIG] =


383  {


384  {0.0,0},


385  {BDS_WAVELENGTH_B1,"2I"},


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  ////////////////////////////////////////////////////////////////////////////


422  bool 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 13 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/fastbitcountingroutines/ */


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, 201+RTCM3_MSM_NUMSATi);


660  else


661  CurrentObs._prn.set(sys, RTCM3_MSM_NUMSATi);


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_NUMSIGj1];


674  break;


675  case 'C':


676  cd = bds[RTCM3_MSM_NUMSIGj1];


677  break;


678  case 'G': case 'S':


679  cd = gps[RTCM3_MSM_NUMSIGj1];


680  break;


681  case 'R':


682  cd = glo[RTCM3_MSM_NUMSIGj1];


683  {


684  int k = GLOFreq[RTCM3_MSM_NUMSATi1];


685  if(extsat[numsat] < 14)


686  {


687  k = GLOFreq[RTCM3_MSM_NUMSATi1] = 100+extsat[numsat]7;


688  }


689  if(k)


690  cd.wl = (cd.wl == 0.0 ? GLO_WAVELENGTH_L1(k100) : GLO_WAVELENGTH_L2(k100));


691  else


692  cd.code = 0;


693  }


694  break;


695  case 'E':


696  cd = gal[RTCM3_MSM_NUMSIGj1];


697  break;


698  }


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;


827  }


828  CurrentObs._obs.push_back(frqObs);


829  }


830  }


831  }


832  if(CurrentObs._obs.size() > 0)


833  _CurrentObsList.push_back(CurrentObs);


834  }


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  ////////////////////////////////////////////////////////////////////////////


853  bool 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[sv1] = 100+freq7; /* 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(freq7);


901  frqObs>_codeValid = frqObs>_phaseValid = true;


902  }


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(freq7);


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(freq7);


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  ////////////////////////////////////////////////////////////////////////////


972  bool 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 < week1) /* 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  ////////////////////////////////////////////////////////////////////////////


1041  bool 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 = i7;


1062  GLOFreq[sv1] = 100+i7; /* store frequency for other users (MSM) */


1063  _gloFrq = QString("%1 %2").arg(eph._prn.toString().c_str()).arg(eph._frequency_number,2,'f',0);


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 ? tk3*60*60+86400 : tk3*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) /* GLONASSM P, GLONASSM ln (third string) */


1089  GETFLOATSIGNM(eph._tau, 22, 1.0/(double)(1<<30)) /* GLONASS tau n(tb) */


1090  SKIPBITS(5) /* GLONASSM delta tau n(tb) */


1091  GETBITS(eph._E, 5)


1092  /* GETBITS(i, 1) / * GLONASSM P4 */


1093  /* GETBITS(i, 4) / * GLONASSM Ft */


1094  /* GETBITS(i, 11) / * GLONASSM Nt */


1095  /* GETBITS(i, 2) / * GLONASSM M */


1096  /* GETBITS(i, 1) / * GLONASSM The Availability of Additional Data */


1097  /* GETBITS(i, 11) / * GLONASSM Na */


1098  /* GETFLOATSIGNM(i, 32, 1.0/(double)(1<<30)/(double)(1<<1)) / * GLONASS tau c */


1099  /* GETBITS(i, 5) / * GLONASSM N4 */


1100  /* GETFLOATSIGNM(i, 22, 1.0/(double)(1<<30)) / * GLONASSM tau GPS */


1101  /* GETBITS(i, 1) / * GLONASSM 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  ////////////////////////////////////////////////////////////////////////////


1123  bool 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 < week1) /* 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  ////////////////////////////////////////////////////////////////////////////


1199  bool 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  ////////////////////////////////////////////////////////////////////////////


1246  bool 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(1024+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  ////////////////////////////////////////////////////////////////////////////


1329  bool 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  ////////////////////////////////////////////////////////////////////////////


1391  bool 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  ////////////////////////////////////////////////////////////////////////////


1409  bool 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 * 1e4;


1424  SKIPBITS(2)


1425  GETBITSSIGN(y, 38)


1426  _antList.back().yy = y * 1e4;


1427  SKIPBITS(2)


1428  GETBITSSIGN(z, 38)


1429  _antList.back().zz = z * 1e4;


1430  if(type == 1006)


1431  {


1432  double h;


1433  GETBITS(h, 16)


1434  _antList.back().height = h * 1e4;


1435  _antList.back().height_f = true;


1436  }


1437  _antList.back().message = type;


1438 


1439  return true;


1440  }


1441 


1442  //


1443  ////////////////////////////////////////////////////////////////////////////


1444  t_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  /* SSR I+II 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 <= 1270))


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  ////////////////////////////////////////////////////////////////////////////


1546  uint32_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  ////////////////////////////////////////////////////////////////////////////


1566  int 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(em >= 3)


1575  {


1576  if(m[0] == 0xD3)


1577  {


1578  _BlockSize = ((m[1]&3)<<8)m[2];


1579  if(em >= 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(em < 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>(_MessageSizei));


1607  _MessageSize = i;


1608 


1609  return !_NeedBytes ? ((_Message[3]<<4)(_Message[4]>>4)) : 0;


1610  }


1611 


1612  // Time of Corrections


1613  //////////////////////////////////////////////////////////////////////////////


1614  int RTCM3Decoder::corrGPSEpochTime() const {


1615  return _coDecoders.size() > 0 ? _coDecoders.begin().value()>corrGPSEpochTime() : 1;


1616  }

