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,


72  SLOT(slotNewGPSEph(t_ephGPS)));


73  connect(this, SIGNAL(newGlonassEph(t_ephGlo)), BNC_CORE,


74  SLOT(slotNewGlonassEph(t_ephGlo)));


75  connect(this, SIGNAL(newGalileoEph(t_ephGal)), BNC_CORE,


76  SLOT(slotNewGalileoEph(t_ephGal)));


77  connect(this, SIGNAL(newSBASEph(t_ephSBAS)), BNC_CORE,


78  SLOT(slotNewSBASEph(t_ephSBAS)));


79  connect(this, SIGNAL(newBDSEph(t_ephBDS)), BNC_CORE,


80  SLOT(slotNewBDSEph(t_ephBDS)));


81 


82  _MessageSize = _SkipBytes = _BlockSize = _NeedBytes = 0;


83  }


84 


85  // Destructor


86  ////////////////////////////////////////////////////////////////////////////


87  RTCM3Decoder::~RTCM3Decoder() {


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


89  while (it.hasNext())


90  {


91  it.next();


92  delete it.value();


93  }


94  }


95 


96  //


97  ////////////////////////////////////////////////////////////////////////////


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


99  bool decoded = false;


100  bncTime CurrentObsTime;


101  int i, numsats, syncf, type;


102  uint64_t numbits = 0, bitfield = 0;


103 


104  data += 3; /* header */


105  size = 6; /* header + crc */


106 


107  GETBITS(type, 12)


108  SKIPBITS(12)


109  /* id */


110  GETBITS(i, 30)


111 


112  CurrentObsTime.set(i);


113  if (_CurrentTime.valid() && CurrentObsTime != _CurrentTime) {


114  decoded = true;


115  _obsList.append(_CurrentObsList);


116  _CurrentObsList.clear();


117  }


118 


119  _CurrentTime = CurrentObsTime;


120 


121  GETBITS(syncf, 1)


122  /* sync */


123  GETBITS(numsats, 5)


124  SKIPBITS(4)


125  /* smind, smint */


126 


127  while (numsats) {


128  int sv, code, l1range, amb = 0;


129  t_satObs CurrentObs;


130  CurrentObs._time = CurrentObsTime;


131 


132  GETBITS(sv, 6)


133  if (sv < 40)


134  CurrentObs._prn.set('G', sv);


135  else


136  CurrentObs._prn.set('S', sv  20);


137 


138  t_frqObs *frqObs = new t_frqObs;


139  /* L1 */


140  GETBITS(code, 1);


141  (code) ?


142  frqObs>_rnxType2ch.assign("1W") : frqObs>_rnxType2ch.assign("1C");


143  GETBITS(l1range, 24);


144  GETBITSSIGN(i, 20);


145  if ((i & ((1 << 20)  1)) != 0x80000) {


146  frqObs>_code = l1range * 0.02;


147  frqObs>_phase = (l1range * 0.02 + i * 0.0005) / GPS_WAVELENGTH_L1;


148  frqObs>_codeValid = frqObs>_phaseValid = true;


149  }


150  GETBITS(i, 7);


151  frqObs>_lockTime = lti2sec(type,i);


152  frqObs>_lockTimeValid = (frqObs>_lockTime >= 0.0 && frqObs>_phaseValid);


153  frqObs>_slipCounter = i;


154  if (type == 1002  type == 1004) {


155  GETBITS(amb, 8);


156  if (amb) {


157  frqObs>_code += amb * 299792.458;


158  frqObs>_phase += (amb * 299792.458) / GPS_WAVELENGTH_L1;


159  }


160  GETBITS(i, 8);


161  if (i) {


162  frqObs>_snr = i * 0.25;


163  frqObs>_snrValid = true;


164  }


165  }


166  CurrentObs._obs.push_back(frqObs);


167  if (type == 1003  type == 1004) {


168  frqObs = new t_frqObs;


169  /* L2 */


170  GETBITS(code, 2);


171  switch (code) {


172  case 3:


173  frqObs>_rnxType2ch.assign("2W"); /* or "2Y"? */


174  break;


175  case 2:


176  frqObs>_rnxType2ch.assign("2W");


177  break;


178  case 1:


179  frqObs>_rnxType2ch.assign("2P");


180  break;


181  case 0:


182  frqObs>_rnxType2ch.assign("2X"); /* or "2S" or "2L"? */


183  break;


184  }


185  GETBITSSIGN(i, 14);


186  if ((i & ((1 << 14)  1)) != 0x2000) {


187  frqObs>_code = l1range * 0.02 + i * 0.02 + amb * 299792.458;


188  frqObs>_codeValid = true;


189  }


190  GETBITSSIGN(i, 20);


191  if ((i & ((1 << 20)  1)) != 0x80000) {


192  frqObs>_phase = (l1range * 0.02 + i * 0.0005 + amb * 299792.458)


193  / GPS_WAVELENGTH_L2;


194  frqObs>_phaseValid = true;


195  }


196  GETBITS(i, 7);


197  frqObs>_lockTime = lti2sec(type,i);


198  frqObs>_lockTimeValid = (frqObs>_lockTime >= 0.0 && frqObs>_phaseValid);


199  frqObs>_slipCounter = i;


200  if (type == 1004) {


201  GETBITS(i, 8);


202  if (i) {


203  frqObs>_snr = i * 0.25;


204  frqObs>_snrValid = true;


205  }


206  }


207  CurrentObs._obs.push_back(frqObs);


208  }


209  _CurrentObsList.push_back(CurrentObs);


210  }


211 


212  if (!syncf) {


213  decoded = true;


214  _obsList.append(_CurrentObsList);


215  _CurrentTime.reset();


216  _CurrentObsList.clear();


217  }


218  return decoded;


219  }


220 


221  #define RTCM3_MSM_NUMSIG 32


222  #define RTCM3_MSM_NUMSAT 64


223  #define RTCM3_MSM_NUMCELLS 96 /* arbitrary limit */


224 


225  /**


226  * Frequency numbers of GLONASS with an offset of 100 to detect unset values.


227  * Gets filled by ephemeris and data blocks and shared between different streams.


228  */


229  static int GLOFreq[RTCM3_MSM_NUMSAT];


230 


231  /*


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


233  * message */


234  struct CodeData {


235  double wl;


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


237  };


238 


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


240  static struct CodeData gps[RTCM3_MSM_NUMSIG] = {


241  {0.0, 0},


242  {GPS_WAVELENGTH_L1, "1C"},


243  {GPS_WAVELENGTH_L1, "1P"},


244  {GPS_WAVELENGTH_L1, "1W"},


245  {0.0, 0}/*{GPS_WAVELENGTH_L1,"1Y"}*/,


246  {0.0, 0},


247  {0.0, 0},


248  {GPS_WAVELENGTH_L2, "2C"},


249  {GPS_WAVELENGTH_L2, "2P"},


250  {GPS_WAVELENGTH_L2, "2W"},


251  {0.0, 0}/*{GPS_WAVELENGTH_L2,"2Y"}*/,


252  {0.0, 0},


253  {0.0, 0},


254  {0.0, 0},


255  {GPS_WAVELENGTH_L2, "2S"},


256  {GPS_WAVELENGTH_L2, "2L"},


257  {GPS_WAVELENGTH_L2, "2X"},


258  {0.0, 0},


259  {0.0, 0},


260  {0.0, 0},


261  {0.0, 0},


262  {GPS_WAVELENGTH_L5, "5I"},


263  {GPS_WAVELENGTH_L5, "5Q"},


264  {GPS_WAVELENGTH_L5, "5X"},


265  {0.0, 0},


266  {0.0, 0},


267  {0.0, 0},


268  {0.0, 0},


269  {0.0, 0},


270  {GPS_WAVELENGTH_L1, "1S"},


271  {GPS_WAVELENGTH_L1, "1L"},


272  {GPS_WAVELENGTH_L1, "1X"}


273  };


274 


275  /**


276  * MSM signal types for GLONASS


277  *


278  * NOTE: Uses 0.0, 1.0 for wavelength as sat index dependence is done later!


279  */


280  static struct CodeData glo[RTCM3_MSM_NUMSIG] = {


281  {0.0, 0},


282  {0.0, "1C"},


283  {0.0, "1P"},


284  {0.0, 0},


285  {0.0, 0},


286  {0.0, 0},


287  {0.0, 0},


288  {1.0, "2C"},


289  {1.0, "2P"},


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  {0.0, 0},


306  {0.0, 0},


307  {0.0, 0},


308  {0.0, 0},


309  {0.0, 0},


310  {0.0, 0},


311  {0.0, 0},


312  {0.0, 0}


313  };


314 


315  /** MSM signal types for Galileo */


316  static struct CodeData gal[RTCM3_MSM_NUMSIG] = {


317  {0.0, 0},


318  {GAL_WAVELENGTH_E1, "1C"},


319  {GAL_WAVELENGTH_E1, "1A"},


320  {GAL_WAVELENGTH_E1, "1B"},


321  {GAL_WAVELENGTH_E1, "1X"},


322  {GAL_WAVELENGTH_E1, "1Z"},


323  {0.0, 0},


324  {GAL_WAVELENGTH_E6, "6C"},


325  {GAL_WAVELENGTH_E6, "6A"},


326  {GAL_WAVELENGTH_E6, "6B"},


327  {GAL_WAVELENGTH_E6, "6X"},


328  {GAL_WAVELENGTH_E6, "6Z"},


329  {0.0, 0},


330  {GAL_WAVELENGTH_E5B, "7I"},


331  {GAL_WAVELENGTH_E5B, "7Q"},


332  {GAL_WAVELENGTH_E5B, "7X"},


333  {0.0, 0},


334  {GAL_WAVELENGTH_E5AB, "8I"},


335  {GAL_WAVELENGTH_E5AB, "8Q"},


336  {GAL_WAVELENGTH_E5AB, "8X"},


337  {0.0, 0},


338  {GAL_WAVELENGTH_E5A, "5I"},


339  {GAL_WAVELENGTH_E5A, "5Q"},


340  {GAL_WAVELENGTH_E5A, "5X"},


341  {0.0, 0},


342  {0.0, 0},


343  {0.0, 0},


344  {0.0, 0},


345  {0.0, 0},


346  {0.0, 0},


347  {0.0, 0},


348  {0.0, 0},


349  };


350 


351  /** MSM signal types for QZSS */


352  static struct CodeData qzss[RTCM3_MSM_NUMSIG] = {


353  {0.0, 0},


354  {GPS_WAVELENGTH_L1, "1C"},


355  {0.0, 0},


356  {0.0, 0},


357  {0.0, 0},


358  {GPS_WAVELENGTH_L1, "1Z"}, // not defined in RTCM3.3?


359  {0.0, 0},


360  {0.0, 0},


361  {QZSS_WAVELENGTH_L6, "6S"},


362  {QZSS_WAVELENGTH_L6, "6L"},


363  {QZSS_WAVELENGTH_L6, "6X"},


364  {0.0, 0},


365  {0.0, 0},


366  {0.0, 0},


367  {GPS_WAVELENGTH_L2, "2S"},


368  {GPS_WAVELENGTH_L2, "2L"},


369  {GPS_WAVELENGTH_L2, "2X"},


370  {0.0, 0},


371  {0.0, 0},


372  {0.0, 0},


373  {0.0, 0},


374  {GPS_WAVELENGTH_L5, "5I"},


375  {GPS_WAVELENGTH_L5, "5Q"},


376  {GPS_WAVELENGTH_L5, "5X"},


377  {0.0, 0},


378  {0.0, 0},


379  {0.0, 0},


380  {0.0, 0},


381  {0.0, 0},


382  {GPS_WAVELENGTH_L1, "1S"},


383  {GPS_WAVELENGTH_L1, "1L"},


384  {GPS_WAVELENGTH_L1, "1X"}


385  };


386 


387  /** MSM signal types for Beidou/BDS */


388  static struct CodeData bds[RTCM3_MSM_NUMSIG] = {


389  {0.0, 0},


390  {BDS_WAVELENGTH_B1, "2I"},


391  {BDS_WAVELENGTH_B1, "2Q"},


392  {BDS_WAVELENGTH_B1, "2X"},


393  {0.0, 0},


394  {0.0, 0},


395  {0.0, 0},


396  {BDS_WAVELENGTH_B3, "6I"},


397  {BDS_WAVELENGTH_B3, "6Q"},


398  {BDS_WAVELENGTH_B3, "6X"},


399  {0.0, 0},


400  {0.0, 0},


401  {0.0, 0},


402  {BDS_WAVELENGTH_B2, "7I"},


403  {BDS_WAVELENGTH_B2, "7Q"},


404  {BDS_WAVELENGTH_B2, "7X"},


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  {0.0, 0},


417  {0.0, 0},


418  {0.0, 0},


419  {0.0, 0},


420  {0.0, 0},


421  };


422 


423  #define UINT64(c) c ## ULL


424 


425  //


426  ////////////////////////////////////////////////////////////////////////////


427  bool RTCM3Decoder::DecodeRTCM3MSM(unsigned char* data, int size)


428  {


429  bool decoded = false;


430  int type, syncf, i;


431  uint64_t numbits = 0, bitfield = 0;


432 


433  data += 3; /* header */


434  size = 6; /* header + crc */


435 


436  GETBITS(type, 12)


437  SKIPBITS(12)


438  /* id */


439  char sys;


440  if (type >= 1121)


441  sys = 'C';


442  else if (type >= 1111)


443  sys = 'J';


444  else if (type >= 1101)


445  sys = 'S';


446  else if (type >= 1091)


447  sys = 'E';


448  else if (type >= 1081)


449  sys = 'R';


450  else


451  sys = 'G';


452 


453  bncTime CurrentObsTime;


454  if (sys == 'C') /* BDS */ {


455  GETBITS(i, 30)


456  CurrentObsTime.setBDS(i);


457  }


458  else if (sys == 'R') /* GLONASS */ {


459  SKIPBITS(3)


460  GETBITS(i, 27)


461  /* tk */


462  CurrentObsTime.setTk(i);


463  }


464  else /* GPS style date */ {


465  GETBITS(i, 30)


466  CurrentObsTime.set(i);


467  }


468  if (_CurrentTime.valid() && CurrentObsTime != _CurrentTime) {


469  decoded = true;


470  _obsList.append(_CurrentObsList);


471  _CurrentObsList.clear();


472  }


473  _CurrentTime = CurrentObsTime;


474 


475  GETBITS(syncf, 1)


476  /**


477  * Ignore unknown types except for sync flag


478  *


479  * We actually support types 13 in following code, but as they are missing


480  * the full cycles and can't be used later we skip interpretation here already.


481  */


482  if (type <= 1130 && (type % 10) >= 4 && (type % 10) <= 7) {


483  int sigmask, numsat = 0, numsig = 0;


484  uint64_t satmask, cellmask, ui;


485  double rrmod[RTCM3_MSM_NUMSAT];


486  int rrint[RTCM3_MSM_NUMSAT], rdop[RTCM3_MSM_NUMSAT],


487  extsat[RTCM3_MSM_NUMSAT];


488  int ll[RTCM3_MSM_NUMCELLS]/*, hc[RTCM3_MSM_NUMCELLS]*/;


489  double cnr[RTCM3_MSM_NUMCELLS];


490  double cp[RTCM3_MSM_NUMCELLS], psr[RTCM3_MSM_NUMCELLS],


491  dop[RTCM3_MSM_NUMCELLS];


492 


493  SKIPBITS(3 + 7 + 2 + 2 + 1 + 3)


494  GETBITS64(satmask, RTCM3_MSM_NUMSAT)


495 


496  /* http://gurmeetsingh.wordpress.com/2008/08/05/fastbitcountingroutines/ */


497  for (ui = satmask; ui; ui &= (ui  1) /* remove rightmost bit */)


498  ++numsat;


499  GETBITS(sigmask, RTCM3_MSM_NUMSIG)


500  for (i = sigmask; i; i &= (i  1) /* remove rightmost bit */)


501  ++numsig;


502  for (i = 0; i < RTCM3_MSM_NUMSAT; ++i)


503  extsat[i] = 15;


504 


505  i = numsat * numsig;


506  GETBITS64(cellmask, (unsigned )i)


507 


508  switch (type % 10) {


509  case 1:


510  case 2:


511  case 3:


512  /* partial data, already skipped above, but implemented for future expansion ! */


513  for (int j = numsat; j;)


514  GETFLOAT(rrmod[j], 10, 1.0 / 1024.0)


515  break;


516  case 4:


517  case 6:


518  for (int j = numsat; j;)


519  GETBITS(rrint[j], 8)


520  for (int j = numsat; j;)


521  GETFLOAT(rrmod[j], 10, 1.0 / 1024.0)


522  break;


523  case 5:


524  case 7:


525  for (int j = numsat; j;)


526  GETBITS(rrint[j], 8)


527  for (int j = numsat; j;)


528  GETBITS(extsat[j], 4)


529  for (int j = numsat; j;)


530  GETFLOAT(rrmod[j], 10, 1.0 / 1024.0)


531  for (int j = numsat; j;)


532  GETBITSSIGN(rdop[j], 14)


533  break;


534  }


535 


536  int numcells = numsat * numsig;


537  /** Drop anything which exceeds our cell limit. Increase limit definition


538  * when that happens. */


539  if (numcells <= RTCM3_MSM_NUMCELLS) {


540  switch (type % 10) {


541  case 1:


542  for (int count = numcells; count;)


543  if (cellmask & (UINT64(1) << count))


544  GETFLOATSIGN(psr[count], 15, 1.0 / (1 << 24))


545  break;


546  case 2:


547  for (int count = numcells; count;)


548  if (cellmask & (UINT64(1) << count))


549  GETFLOATSIGN(cp[count], 22, 1.0 / (1 << 29))


550  for (int count = numcells; count;)


551  if (cellmask & (UINT64(1) << count))


552  GETBITS(ll[count], 4)


553  for (int count = numcells; count;)


554  if (cellmask & (UINT64(1) << count))


555  SKIPBITS(1)/*GETBITS(hc[count], 1)*/


556  break;


557  case 3:


558  for (int count = numcells; count;)


559  if (cellmask & (UINT64(1) << count))


560  GETFLOATSIGN(psr[count], 15, 1.0 / (1 << 24))


561  for (int count = numcells; count;)


562  if (cellmask & (UINT64(1) << count))


563  GETFLOATSIGN(cp[count], 22, 1.0 / (1 << 29))


564  for (int count = numcells; count;)


565  if (cellmask & (UINT64(1) << count))


566  GETBITS(ll[count], 4)


567  for (int count = numcells; count;)


568  if (cellmask & (UINT64(1) << count))


569  SKIPBITS(1)/*GETBITS(hc[count], 1)*/


570  break;


571  case 4:


572  for (int count = numcells; count;)


573  if (cellmask & (UINT64(1) << count))


574  GETFLOATSIGN(psr[count], 15, 1.0 / (1 << 24))


575  for (int count = numcells; count;)


576  if (cellmask & (UINT64(1) << count))


577  GETFLOATSIGN(cp[count], 22, 1.0 / (1 << 29))


578  for (int count = numcells; count;)


579  if (cellmask & (UINT64(1) << count))


580  GETBITS(ll[count], 4)


581  for (int count = numcells; count;)


582  if (cellmask & (UINT64(1) << count))


583  SKIPBITS(1)/*GETBITS(hc[count], 1)*/


584  for (int count = numcells; count;)


585  if (cellmask & (UINT64(1) << count))


586  GETBITS(cnr[count], 6)


587  break;


588  case 5:


589  for (int count = numcells; count;)


590  if (cellmask & (UINT64(1) << count))


591  GETFLOATSIGN(psr[count], 15, 1.0 / (1 << 24))


592  for (int count = numcells; count;)


593  if (cellmask & (UINT64(1) << count))


594  GETFLOATSIGN(cp[count], 22, 1.0 / (1 << 29))


595  for (int count = numcells; count;)


596  if (cellmask & (UINT64(1) << count))


597  GETBITS(ll[count], 4)


598  for (int count = numcells; count;)


599  if (cellmask & (UINT64(1) << count))


600  SKIPBITS(1)/*GETBITS(hc[count], 1)*/


601  for (int count = numcells; count;)


602  if (cellmask & (UINT64(1) << count))


603  GETFLOAT(cnr[count], 6, 1.0)


604  for (int count = numcells; count;)


605  if (cellmask & (UINT64(1) << count))


606  GETFLOATSIGN(dop[count], 15, 0.0001)


607  break;


608  case 6:


609  for (int count = numcells; count;)


610  if (cellmask & (UINT64(1) << count))


611  GETFLOATSIGN(psr[count], 20, 1.0 / (1 << 29))


612  for (int count = numcells; count;)


613  if (cellmask & (UINT64(1) << count))


614  GETFLOATSIGN(cp[count], 24, 1.0 / (1U << 31))


615  for (int count = numcells; count;)


616  if (cellmask & (UINT64(1) << count))


617  GETBITS(ll[count], 10)


618  for (int count = numcells; count;)


619  if (cellmask & (UINT64(1) << count))


620  SKIPBITS(1)/*GETBITS(hc[count], 1)*/


621  for (int count = numcells; count;)


622  if (cellmask & (UINT64(1) << count))


623  GETFLOAT(cnr[count], 10, 1.0 / (1 << 4))


624  break;


625  case 7:


626  for (int count = numcells; count;)


627  if (cellmask & (UINT64(1) << count))


628  GETFLOATSIGN(psr[count], 20, 1.0 / (1 << 29))


629  for (int count = numcells; count;)


630  if (cellmask & (UINT64(1) << count))


631  GETFLOATSIGN(cp[count], 24, 1.0 / (1U << 31))


632  for (int count = numcells; count;)


633  if (cellmask & (UINT64(1) << count))


634  GETBITS(ll[count], 10)


635  for (int count = numcells; count;)


636  if (cellmask & (UINT64(1) << count))


637  SKIPBITS(1)/*GETBITS(hc[count], 1)*/


638  for (int count = numcells; count;)


639  if (cellmask & (UINT64(1) << count))


640  GETFLOAT(cnr[count], 10, 1.0 / (1 << 4))


641  for (int count = numcells; count;)


642  if (cellmask & (UINT64(1) << count))


643  GETFLOATSIGN(dop[count], 15, 0.0001)


644  break;


645  }


646  i = RTCM3_MSM_NUMSAT;


647  int j = 1;


648  t_satObs CurrentObs;


649  for (int count = numcells; count;) {


650  while (j >= 0 && !(sigmask & (1 << j)))


651  ;


652  if (j < 0) {


653  while (!(satmask & (UINT64(1) << (i))))


654  /* next satellite */


655  ;


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


657  _CurrentObsList.push_back(CurrentObs);


658  CurrentObs.clear();


659  CurrentObs._time = CurrentObsTime;


660  if (sys == 'S')


661  CurrentObs._prn.set(sys, 20  1 + RTCM3_MSM_NUMSAT  i);


662  else


663  CurrentObs._prn.set(sys, RTCM3_MSM_NUMSAT  i);


664  j = RTCM3_MSM_NUMSIG;


665  while (!(sigmask & (1 << j)))


666  ;


667  numsat;


668  }


669  if (cellmask & (UINT64(1) << count)) {


670  struct CodeData cd = {0.0, 0};


671  switch (sys) {


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':


679  case 'S':


680  cd = gps[RTCM3_MSM_NUMSIG  j  1];


681  break;


682  case 'R':


683  cd = glo[RTCM3_MSM_NUMSIG  j  1];


684  {


685  int k = GLOFreq[RTCM3_MSM_NUMSAT  i  1];


686  if (extsat[numsat] < 14) {


687  k = GLOFreq[RTCM3_MSM_NUMSAT  i  1] = 100 + extsat[numsat]


688   7;


689  }


690  if (k)


691  cd.wl = (


692  cd.wl == 0.0 ?


693  GLO_WAVELENGTH_L1(k  100) :


694  GLO_WAVELENGTH_L2(k  100));


695  else


696  cd.code = 0;


697  }


698  break;


699  case 'E':


700  cd = gal[RTCM3_MSM_NUMSIG  j  1];


701  break;


702  }


703  if (cd.code) {


704  t_frqObs *frqObs = new t_frqObs;


705  frqObs>_rnxType2ch.assign(cd.code);


706 


707  switch (type % 10) {


708  case 1:


709  if (psr[count] > 1.0 / (1 << 10)) {


710  frqObs>_code = psr[count] * LIGHTSPEED / 1000.0


711  + (rrmod[numsat]) * LIGHTSPEED / 1000.0;


712  frqObs>_codeValid = true;


713  }


714  break;


715  case 2:


716  if (cp[count] > 1.0 / (1 << 8)) {


717  frqObs>_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl


718  + (rrmod[numsat]) * LIGHTSPEED / 1000.0 / cd.wl;


719  frqObs>_phaseValid = true;


720  frqObs>_lockTime = lti2sec(type,ll[count]);


721  frqObs>_lockTimeValid = (frqObs>_lockTime >= 0.0);


722  frqObs>_slipCounter = ll[count];


723  }


724  break;


725  case 3:


726  if (psr[count] > 1.0 / (1 << 10)) {


727  frqObs>_code = psr[count] * LIGHTSPEED / 1000.0


728  + (rrmod[numsat]) * LIGHTSPEED / 1000.0;


729  frqObs>_codeValid = true;


730  }


731 


732  if (cp[count] > 1.0 / (1 << 8)) {


733  frqObs>_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl


734  + rrmod[numsat] * LIGHTSPEED / 1000.0 / cd.wl;


735  frqObs>_phaseValid = true;


736  frqObs>_lockTime = lti2sec(type,ll[count]);


737  frqObs>_lockTimeValid = (frqObs>_lockTime >= 0.0);


738  frqObs>_slipCounter = ll[count];


739  }


740  break;


741  case 4:


742  if (psr[count] > 1.0 / (1 << 10)) {


743  frqObs>_code = psr[count] * LIGHTSPEED / 1000.0


744  + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0;


745  frqObs>_codeValid = true;


746  }


747 


748  if (cp[count] > 1.0 / (1 << 8)) {


749  frqObs>_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl


750  + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0


751  / cd.wl;


752  frqObs>_phaseValid = true;


753  frqObs>_lockTime = lti2sec(type,ll[count]);


754  frqObs>_lockTimeValid = (frqObs>_lockTime >= 0.0);


755  frqObs>_slipCounter = ll[count];


756  }


757 


758  frqObs>_snr = cnr[count];


759  frqObs>_snrValid = true;


760  break;


761  case 5:


762  if (psr[count] > 1.0 / (1 << 10)) {


763  frqObs>_code = psr[count] * LIGHTSPEED / 1000.0


764  + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0;


765  frqObs>_codeValid = true;


766  }


767 


768  if (cp[count] > 1.0 / (1 << 8)) {


769  frqObs>_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl


770  + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0


771  / cd.wl;


772  frqObs>_phaseValid = true;


773  frqObs>_lockTime = lti2sec(type,ll[count]);


774  frqObs>_lockTimeValid = (frqObs>_lockTime >= 0.0);


775  frqObs>_slipCounter = ll[count];


776  }


777 


778  frqObs>_snr = cnr[count];


779  frqObs>_snrValid = true;


780 


781  if (dop[count] > 1.6384) {


782  frqObs>_doppler = (dop[count] + rdop[numsat]) / cd.wl;


783  frqObs>_dopplerValid = true;


784  }


785  break;


786  case 6:


787  if (psr[count] > 1.0 / (1 << 10)) {


788  frqObs>_code = psr[count] * LIGHTSPEED / 1000.0


789  + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0;


790  frqObs>_codeValid = true;


791  }


792 


793  if (cp[count] > 1.0 / (1 << 8)) {


794  frqObs>_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl


795  + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0


796  / cd.wl;


797  frqObs>_phaseValid = true;


798  frqObs>_lockTime = lti2sec(type,ll[count]);


799  frqObs>_lockTimeValid = (frqObs>_lockTime >= 0.0);


800  frqObs>_slipCounter = ll[count];


801  }


802 


803  frqObs>_snr = cnr[count];


804  frqObs>_snrValid = true;


805  break;


806  case 7:


807  if (psr[count] > 1.0 / (1 << 10)) {


808  frqObs>_code = psr[count] * LIGHTSPEED / 1000.0


809  + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0;


810  frqObs>_codeValid = true;


811  }


812 


813  if (cp[count] > 1.0 / (1 << 8)) {


814  frqObs>_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl


815  + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0


816  / cd.wl;


817  frqObs>_phaseValid = true;


818  frqObs>_lockTime = lti2sec(type,ll[count]);


819  frqObs>_lockTimeValid = (frqObs>_lockTime >= 0.0);


820  frqObs>_slipCounter = ll[count];


821  }


822 


823  frqObs>_snr = cnr[count];


824  frqObs>_snrValid = true;


825 


826  if (dop[count] > 1.6384) {


827  frqObs>_doppler = (dop[count] + rdop[numsat]) / cd.wl;


828  frqObs>_dopplerValid = true;


829  }


830  break;


831  }


832  CurrentObs._obs.push_back(frqObs);


833  }


834  }


835  }


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


837  _CurrentObsList.push_back(CurrentObs);


838  }


839  }


840  else if ((type % 10) < 3) {


841  emit(newMessage(QString("%1: Block %2 contain partial data! Ignored!")


842  .arg(_staID).arg(type).toAscii(), true));


843  }


844  if (!syncf) {


845  decoded = true;


846  _obsList.append(_CurrentObsList);


847  _CurrentTime.reset();


848  _CurrentObsList.clear();


849  }


850  return decoded;


851  }


852 


853  //


854  ////////////////////////////////////////////////////////////////////////////


855  bool RTCM3Decoder::DecodeRTCM3GLONASS(unsigned char* data, int size) {


856  bool decoded = false;


857  bncTime CurrentObsTime;


858  int i, numsats, syncf, type;


859  uint64_t numbits = 0, bitfield = 0;


860 


861  data += 3; /* header */


862  size = 6; /* header + crc */


863 


864  GETBITS(type, 12)


865  SKIPBITS(12)


866  /* id */


867  GETBITS(i, 27)


868  /* tk */


869 


870  CurrentObsTime.setTk(i);


871  if (_CurrentTime.valid() && CurrentObsTime != _CurrentTime) {


872  decoded = true;


873  _obsList.append(_CurrentObsList);


874  _CurrentObsList.clear();


875  }


876  _CurrentTime = CurrentObsTime;


877 


878  GETBITS(syncf, 1)


879  /* sync */


880  GETBITS(numsats, 5)


881  SKIPBITS(4)


882  /* smind, smint */


883 


884  while (numsats) {


885  int sv, code, l1range, amb = 0, freq;


886  t_satObs CurrentObs;


887  CurrentObs._time = CurrentObsTime;


888 


889  GETBITS(sv, 6)


890  CurrentObs._prn.set('R', sv);


891  GETBITS(code, 1)


892  GETBITS(freq, 5)


893  GLOFreq[sv  1] = 100 + freq  7; /* store frequency for other users (MSM) */


894 


895  t_frqObs *frqObs = new t_frqObs;


896  /* L1 */


897  (code) ?


898  frqObs>_rnxType2ch.assign("1P") : frqObs>_rnxType2ch.assign("1C");


899  GETBITS(l1range, 25);


900  GETBITSSIGN(i, 20);


901  if ((i & ((1 << 20)  1)) != 0x80000) {


902  frqObs>_code = l1range * 0.02;


903  frqObs>_phase = (l1range * 0.02 + i * 0.0005)


904  / GLO_WAVELENGTH_L1(freq  7);


905  frqObs>_codeValid = frqObs>_phaseValid = true;


906  }


907  GETBITS(i, 7);


908  frqObs>_lockTime = lti2sec(type,i);


909  frqObs>_lockTimeValid = (frqObs>_lockTime >= 0.0 && frqObs>_phaseValid);


910  frqObs>_slipCounter = i;


911  if (type == 1010  type == 1012) {


912  GETBITS(amb, 7);


913  if (amb) {


914  frqObs>_code += amb * 599584.916;


915  frqObs>_phase += (amb * 599584.916) / GLO_WAVELENGTH_L1(freq  7);


916  }


917  GETBITS(i, 8);


918  if (i) {


919  frqObs>_snr = i * 0.25;


920  frqObs>_snrValid = true;


921  }


922  }


923  CurrentObs._obs.push_back(frqObs);


924  if (type == 1011  type == 1012) {


925  frqObs = new t_frqObs;


926  /* L2 */


927  GETBITS(code, 2);


928  switch (code) {


929  case 3:


930  frqObs>_rnxType2ch.assign("2P");


931  break;


932  case 2:


933  frqObs>_rnxType2ch.assign("2P");


934  break;


935  case 1:


936  frqObs>_rnxType2ch.assign("2P");


937  break;


938  case 0:


939  frqObs>_rnxType2ch.assign("2C");


940  break;


941  }


942  GETBITSSIGN(i, 14);


943  if ((i & ((1 << 14)  1)) != 0x2000) {


944  frqObs>_code = l1range * 0.02 + i * 0.02 + amb * 599584.916;


945  frqObs>_codeValid = true;


946  }


947  GETBITSSIGN(i, 20);


948  if ((i & ((1 << 20)  1)) != 0x80000) {


949  frqObs>_phase = (l1range * 0.02 + i * 0.0005 + amb * 599584.916)


950  / GLO_WAVELENGTH_L2(freq  7);


951  frqObs>_phaseValid = true;


952  }


953  GETBITS(i, 7);


954  frqObs>_lockTime = lti2sec(type,i);


955  frqObs>_lockTimeValid = (frqObs>_lockTime >= 0.0 && frqObs>_phaseValid);


956  frqObs>_slipCounter = i;


957  if (type == 1012) {


958  GETBITS(i, 8);


959  if (i) {


960  frqObs>_snr = i * 0.25;


961  frqObs>_snrValid = true;


962  }


963  }


964  CurrentObs._obs.push_back(frqObs);


965  }


966  _CurrentObsList.push_back(CurrentObs);


967  }


968  if (!syncf) {


969  decoded = true;


970  _obsList.append(_CurrentObsList);


971  _CurrentTime.reset();


972  _CurrentObsList.clear();


973  }


974  return decoded;


975  }


976 


977  //


978  ////////////////////////////////////////////////////////////////////////////


979  bool RTCM3Decoder::DecodeGPSEphemeris(unsigned char* data, int size) {


980  bool decoded = false;


981 


982  if (size == 67) {


983  t_ephGPS eph;


984  int i, week;


985  uint64_t numbits = 0, bitfield = 0;


986 


987  data += 3; /* header */


988  size = 6; /* header + crc */


989  SKIPBITS(12)


990 


991  eph._receptDateTime = currentDateAndTimeGPS();


992 


993  GETBITS(i, 6)


994  eph._prn.set('G', i);


995  GETBITS(week, 10)


996  GETBITS(i, 4)


997  eph._ura = accuracyFromIndex(i, eph.type());


998  GETBITS(eph._L2Codes, 2)


999  GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))


1000  GETBITS(eph._IODE, 8)


1001  GETBITS(i, 16)


1002  i <<= 4;


1003  eph._TOC.set(i * 1000);


1004  GETFLOATSIGN(eph._clock_driftrate, 8,


1005  1.0 / (double )(1 << 30) / (double )(1 << 25))


1006  GETFLOATSIGN(eph._clock_drift, 16,


1007  1.0 / (double )(1 << 30) / (double )(1 << 13))


1008  GETFLOATSIGN(eph._clock_bias, 22,


1009  1.0 / (double )(1 << 30) / (double )(1 << 1))


1010  GETBITS(eph._IODC, 10)


1011  GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5))


1012  GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))


1013  GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))


1014  GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29))


1015  GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))


1016  GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29))


1017  GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))


1018  GETBITS(i, 16)


1019  i <<= 4;


1020  eph._TOEsec = i;


1021  bncTime t;


1022  t.set(i * 1000);


1023  eph._TOEweek = t.gpsw();


1024  int numOfRollOvers = int(floor(t.gpsw()/1024.0));


1025  week += (numOfRollOvers * 1024);


1026  /* week from HOW, differs from TOC, TOE week, we use adapted value instead */


1027  if (eph._TOEweek > week + 1  eph._TOEweek < week  1) /* invalid week */


1028  return false;


1029  GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29))


1030  GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))


1031  GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29))


1032  GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))


1033  GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5))


1034  GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))


1035  GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))


1036  GETFLOATSIGN(eph._TGD, 8, 1.0 / (double )(1 << 30) / (double )(1 << 1))


1037  GETBITS(eph._health, 6)


1038  GETBITS(eph._L2PFlag, 1)


1039  GETBITS(eph._fitInterval, 1)


1040  eph._TOT = 0.9999e9;


1041 


1042  emit newGPSEph(eph);


1043  decoded = true;


1044  }


1045  return decoded;


1046  }


1047 


1048  //


1049  ////////////////////////////////////////////////////////////////////////////


1050  bool RTCM3Decoder::DecodeGLONASSEphemeris(unsigned char* data, int size) {


1051  bool decoded = false;


1052 


1053  if (size == 51) {


1054  t_ephGlo eph;


1055  int sv, i, tk;


1056  uint64_t numbits = 0, bitfield = 0;


1057 


1058  data += 3; /* header */


1059  size = 6; /* header + crc */


1060  SKIPBITS(12)


1061 


1062  eph._receptDateTime = currentDateAndTimeGPS();


1063 


1064  GETBITS(sv, 6)


1065  eph._prn.set('R', sv);


1066 


1067  GETBITS(i, 5)


1068  eph._frequency_number = i  7;


1069  GETBITS(eph._almanac_health, 1) /* almanac healthy */


1070  GETBITS(eph._almanac_health_availablility_indicator, 1) /* almanac health ok */


1071  GETBITS(eph._P1, 2) /* P1 */


1072  GETBITS(i, 5)


1073  tk = i * 60 * 60;


1074  GETBITS(i, 6)


1075  tk += i * 60;


1076  GETBITS(i, 1)


1077  tk += i * 30;


1078  eph._tki = tk < 3 * 60 * 60 ? tk  3 * 60 * 60 + 86400 : tk  3 * 60 * 60;


1079  GETBITS(eph._health, 1) /* MSB of Bn*/


1080  GETBITS(eph._P2, 1) /* P2 */


1081  GETBITS(i, 7)


1082  eph._TOC.setTk(i * 15 * 60 * 1000); /* tb */


1083 


1084  GETFLOATSIGNM(eph._x_velocity, 24, 1.0 / (double )(1 << 20))


1085  GETFLOATSIGNM(eph._x_pos, 27, 1.0 / (double )(1 << 11))


1086  GETFLOATSIGNM(eph._x_acceleration, 5, 1.0 / (double )(1 << 30))


1087  GETFLOATSIGNM(eph._y_velocity, 24, 1.0 / (double )(1 << 20))


1088  GETFLOATSIGNM(eph._y_pos, 27, 1.0 / (double )(1 << 11))


1089  GETFLOATSIGNM(eph._y_acceleration, 5, 1.0 / (double )(1 << 30))


1090  GETFLOATSIGNM(eph._z_velocity, 24, 1.0 / (double )(1 << 20))


1091  GETFLOATSIGNM(eph._z_pos, 27, 1.0 / (double )(1 << 11))


1092  GETFLOATSIGNM(eph._z_acceleration, 5, 1.0 / (double )(1 << 30))


1093 


1094  eph._xv(1) = eph._x_pos * 1.e3;


1095  eph._xv(2) = eph._y_pos * 1.e3;


1096  eph._xv(3) = eph._z_pos * 1.e3;


1097  eph._xv(4) = eph._x_velocity * 1.e3;


1098  eph._xv(5) = eph._y_velocity * 1.e3;


1099  eph._xv(6) = eph._z_velocity * 1.e3;


1100 


1101  GETBITS(eph._P3, 1) /* P3 */


1102  GETFLOATSIGNM(eph._gamma, 11, 1.0 / (double )(1 << 30) / (double )(1 << 10))


1103  GETBITS(eph._M_P, 2) /* GLONASSM P, */


1104  GETBITS(eph._M_l3, 1) /*GLONASSM ln (third string) */


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


1106  GETFLOATSIGNM(eph._M_delta_tau, 5, 1.0 / (double )(1 << 30)) /* GLONASSM delta tau n(tb) */


1107  GETBITS(eph._E, 5)


1108  GETBITS(eph._M_P4, 1) /* GLONASSM P4 */


1109  GETBITS(eph._M_FT, 4) /* GLONASSM Ft */


1110  GETBITS(eph._M_NT, 11) /* GLONASSM Nt */


1111  GETBITS(eph._M_M, 2) /* GLONASSM M */


1112  GETBITS(eph._additional_data_availability, 1) /* GLONASSM The Availability of Additional Data */


1113  GETBITS(eph._NA, 11) /* GLONASSM Na */


1114  GETFLOATSIGNM(eph._tauC, 32, 1.0/(double)(1<<30)/(double)(1<<1)) /* GLONASS tau c */


1115  GETBITS(eph._M_N4, 5) /* GLONASSM N4 */


1116  GETFLOATSIGNM(eph._M_tau_GPS, 22, 1.0/(double)(1<<30)) /* GLONASSM tau GPS */


1117  GETBITS(eph._M_l5, 1) /* GLONASSM ln (fifth string) */


1118 


1119  unsigned year, month, day;


1120  eph._TOC.civil_date(year, month, day);


1121  eph._gps_utc = gnumleap(year, month, day);


1122  eph._tt = eph._TOC;


1123 


1124  GLOFreq[sv  1] = 100 + eph._frequency_number ; /* store frequency for other users (MSM) */


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


1126 


1127  if (eph._xv.size() == 6) {


1128  emit newGlonassEph(eph);


1129  decoded = true;


1130  }


1131  }


1132  return decoded;


1133  }


1134 


1135  //


1136  ////////////////////////////////////////////////////////////////////////////


1137  bool RTCM3Decoder::DecodeQZSSEphemeris(unsigned char* data, int size) {


1138  bool decoded = false;


1139 


1140  if (size == 67) {


1141  t_ephGPS eph;


1142  int i, week;


1143  uint64_t numbits = 0, bitfield = 0;


1144 


1145  data += 3; /* header */


1146  size = 6; /* header + crc */


1147  SKIPBITS(12)


1148 


1149  eph._receptDateTime = currentDateAndTimeGPS();


1150 


1151  GETBITS(i, 4)


1152  eph._prn.set('J', i);


1153 


1154  GETBITS(i, 16)


1155  i <<= 4;


1156  eph._TOC.set(i * 1000);


1157 


1158  GETFLOATSIGN(eph._clock_driftrate, 8,


1159  1.0 / (double )(1 << 30) / (double )(1 << 25))


1160  GETFLOATSIGN(eph._clock_drift, 16,


1161  1.0 / (double )(1 << 30) / (double )(1 << 13))


1162  GETFLOATSIGN(eph._clock_bias, 22,


1163  1.0 / (double )(1 << 30) / (double )(1 << 1))


1164  GETBITS(eph._IODE, 8)


1165  GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5))


1166  GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))


1167  GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))


1168  GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29))


1169  GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))


1170  GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29))


1171  GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))


1172  GETBITS(i, 16)


1173  i <<= 4;


1174  eph._TOEsec = i;


1175  bncTime t;


1176  t.set(i*1000);


1177  eph._TOEweek = t.gpsw();


1178  GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29))


1179  GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))


1180  GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29))


1181  GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))


1182  GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5))


1183  GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))


1184  GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))


1185  GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))


1186  GETBITS(eph._L2Codes, 2)


1187  GETBITS(week, 10)


1188  int numOfRollOvers = int(floor(t.gpsw()/1024.0));


1189  week += (numOfRollOvers * 1024);


1190  /* week from HOW, differs from TOC, TOE week, we use adapted value instead */


1191  if (eph._TOEweek > week + 1  eph._TOEweek < week  1) /* invalid week */


1192  return false;


1193 


1194  GETBITS(i, 4)


1195  if (i <= 6)


1196  eph._ura = ceil(10.0 * pow(2.0, 1.0 + i / 2.0)) / 10.0;


1197  else


1198  eph._ura = ceil(10.0 * pow(2.0, i / 2.0)) / 10.0;


1199  GETBITS(eph._health, 6)


1200  GETFLOATSIGN(eph._TGD, 8, 1.0 / (double )(1 << 30) / (double )(1 << 1))


1201  GETBITS(eph._IODC, 10)


1202  GETBITS(eph._fitInterval, 1)


1203  eph._TOT = 0.9999e9;


1204  eph._L2PFlag = 0; /* does not exist for QZSS */


1205 


1206  emit newGPSEph(eph);


1207  decoded = true;


1208  }


1209  return decoded;


1210  }


1211 


1212  //


1213  ////////////////////////////////////////////////////////////////////////////


1214  bool RTCM3Decoder::DecodeSBASEphemeris(unsigned char* data, int size) {


1215  bool decoded = false;


1216 


1217  if (size == 35) {


1218  t_ephSBAS eph;


1219  int i;


1220  uint64_t numbits = 0, bitfield = 0;


1221 


1222  data += 3; /* header */


1223  size = 6; /* header + crc */


1224  SKIPBITS(12)


1225 


1226  eph._receptDateTime = currentDateAndTimeGPS();


1227 


1228  GETBITS(i, 6)


1229  eph._prn.set('S', 20 + i);


1230  GETBITS(eph._IODN, 8)


1231  GETBITS(i, 13)


1232  i <<= 4;


1233  eph._TOC.setTOD(i * 1000);


1234  GETBITS(i, 4)


1235  eph._ura = accuracyFromIndex(i, eph.type());


1236  GETFLOATSIGN(eph._x_pos, 30, 0.08)


1237  GETFLOATSIGN(eph._y_pos, 30, 0.08)


1238  GETFLOATSIGN(eph._z_pos, 25, 0.4)


1239  GETFLOATSIGN(eph._x_velocity, 17, 0.000625)


1240  GETFLOATSIGN(eph._y_velocity, 17, 0.000625)


1241  GETFLOATSIGN(eph._z_velocity, 18, 0.004)


1242  GETFLOATSIGN(eph._x_acceleration, 10, 0.0000125)


1243  GETFLOATSIGN(eph._y_acceleration, 10, 0.0000125)


1244  GETFLOATSIGN(eph._z_acceleration, 10, 0.0000625)


1245  GETFLOATSIGN(eph._agf0, 12, 1.0 / (1 << 30) / (1 << 1))


1246  GETFLOATSIGN(eph._agf1, 8, 1.0 / (1 << 30) / (1 << 10))


1247 


1248  eph._TOT = 0.9999E9;


1249  eph._health = 0;


1250 


1251  emit newSBASEph(eph);


1252  decoded = true;


1253  }


1254  return decoded;


1255  }


1256 


1257  //


1258  ////////////////////////////////////////////////////////////////////////////


1259  bool RTCM3Decoder::DecodeGalileoEphemeris(unsigned char* data, int size) {


1260  bool decoded = false;


1261  uint64_t numbits = 0, bitfield = 0;


1262  int i;


1263 


1264  data += 3; /* header */


1265  size = 6; /* header + crc */


1266  GETBITS(i, 12)


1267 


1268  if ((i == 1046 && size == 61)  (i == 1045 && size == 60)) {


1269  t_ephGal eph;


1270 


1271  eph._receptDateTime = currentDateAndTimeGPS();


1272 


1273  eph._inav = (i == 1046);


1274  eph._fnav = (i == 1045);


1275  GETBITS(i, 6)


1276  eph._prn.set('E', i, eph._inav ? 1 : 0);


1277 


1278  GETBITS(eph._TOEweek, 12)


1279  GETBITS(eph._IODnav, 10)


1280  GETBITS(i, 8)


1281  eph._SISA = accuracyFromIndex(i, eph.type());


1282  GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))


1283  GETBITSFACTOR(i, 14, 60)


1284  eph._TOC.set(1024 + eph._TOEweek, i);


1285  GETFLOATSIGN(eph._clock_driftrate, 6,


1286  1.0 / (double )(1 << 30) / (double )(1 << 29))


1287  GETFLOATSIGN(eph._clock_drift, 21,


1288  1.0 / (double )(1 << 30) / (double )(1 << 16))


1289  GETFLOATSIGN(eph._clock_bias, 31,


1290  1.0 / (double )(1 << 30) / (double )(1 << 4))


1291  GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5))


1292  GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))


1293  GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))


1294  GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29))


1295  GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))


1296  GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29))


1297  GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))


1298  GETBITSFACTOR(eph._TOEsec, 14, 60)


1299  /* FIXME: overwrite value, copied from old code */


1300  eph._TOEsec = eph._TOC.gpssec();


1301  GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29))


1302  GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))


1303  GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29))


1304  GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))


1305  GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5))


1306  GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))


1307  GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))


1308  GETFLOATSIGN(eph._BGD_1_5A, 10,


1309  1.0 / (double )(1 << 30) / (double )(1 << 2))


1310  if (eph._inav) {


1311  /* set unused F/NAV values */


1312  eph._E5aHS = 0.0;


1313  eph._e5aDataInValid = false;


1314 


1315  GETFLOATSIGN(eph._BGD_1_5B, 10,


1316  1.0 / (double )(1 << 30) / (double )(1 << 2))


1317  GETBITS(eph._E5bHS, 2)


1318  GETBITS(eph._e5bDataInValid, 1)


1319  GETBITS(eph._E1_bHS, 2)


1320  GETBITS(eph._e1DataInValid, 1)


1321  }


1322  else {


1323  /* set unused I/NAV values */


1324  eph._BGD_1_5B = 0.0;


1325  eph._E5bHS = 0.0;


1326  eph._E1_bHS = 0.0;


1327  eph._e1DataInValid = false;


1328  eph._e5bDataInValid = false;


1329 


1330  GETBITS(eph._E5aHS, 2)


1331  GETBITS(eph._e5aDataInValid, 1)


1332  }


1333  eph._TOT = 0.9999e9;


1334 


1335  emit newGalileoEph(eph);


1336  decoded = true;


1337  }


1338  return decoded;


1339  }


1340 


1341  //


1342  ////////////////////////////////////////////////////////////////////////////


1343  bool RTCM3Decoder::DecodeBDSEphemeris(unsigned char* data, int size) {


1344  bool decoded = false;


1345 


1346  if (size == 70) {


1347  t_ephBDS eph;


1348  int i;


1349  uint64_t numbits = 0, bitfield = 0;


1350 


1351  data += 3; /* header */


1352  size = 6; /* header + crc */


1353  SKIPBITS(12)


1354 


1355  eph._receptDateTime = currentDateAndTimeGPS();


1356 


1357  GETBITS(i, 6)


1358  eph._prn.set('C', i);


1359 


1360  SKIPBITS(13)


1361  /* week */


1362  GETBITS(i, 4)


1363  eph._URA = accuracyFromIndex(i, eph.type());


1364  GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))


1365  GETBITS(eph._AODE, 5)


1366  GETBITS(i, 17)


1367  i <<= 3;


1368  eph._TOC.setBDS(i * 1000);


1369  GETFLOATSIGN(eph._clock_driftrate, 11,


1370  1.0 / (double )(1 << 30) / (double )(1 << 30) / (double )(1 << 6))


1371  GETFLOATSIGN(eph._clock_drift, 22,


1372  1.0 / (double )(1 << 30) / (double )(1 << 20))


1373  GETFLOATSIGN(eph._clock_bias, 24,


1374  1.0 / (double )(1 << 30) / (double )(1 << 3))


1375  GETBITS(eph._AODC, 5)


1376  GETFLOATSIGN(eph._Crs, 18, 1.0 / (double )(1 << 6))


1377  GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))


1378  GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))


1379  GETFLOATSIGN(eph._Cuc, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1))


1380  GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))


1381  GETFLOATSIGN(eph._Cus, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1))


1382  GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))


1383  GETBITS(i, 17)


1384  i <<= 3;


1385  eph._TOEsec = i;


1386  eph._TOE.setBDS(i * 1000);


1387  GETFLOATSIGN(eph._Cic, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1))


1388  GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))


1389  GETFLOATSIGN(eph._Cis, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1))


1390  GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))


1391  GETFLOATSIGN(eph._Crc, 18, 1.0 / (double )(1 << 6))


1392  GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))


1393  GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))


1394  GETFLOATSIGN(eph._TGD1, 10, 0.0000000001)


1395  GETFLOATSIGN(eph._TGD2, 10, 0.0000000001)


1396  GETBITS(eph._SatH1, 1)


1397 


1398  eph._TOT = 0.9999E9;


1399  emit newBDSEph(eph);


1400  decoded = true;


1401  }


1402  return decoded;


1403  }


1404 


1405  //


1406  ////////////////////////////////////////////////////////////////////////////


1407  bool RTCM3Decoder::DecodeAntennaReceiver(unsigned char* data, int size) {


1408  char *antenna, anttype[256];


1409  char *dummy;


1410  char *receiver, rectype[256];


1411 


1412  int type;


1413 


1414  int dnum = 1;


1415  int antnum = 1;


1416  int recnum = 1;


1417  uint64_t numbits = 0, bitfield = 0;


1418 


1419  data += 3; /* header*/


1420  size = 6; /* header + crc */


1421 


1422  GETBITS(type, 12)


1423  SKIPBITS(12)


1424  GETSTRING(antnum, antenna)


1425  if (antnum > 1 && antnum < 265) {


1426  memcpy(anttype, antenna, antnum);


1427  anttype[antnum] = 0;


1428  if (!_antType.contains(anttype)) {


1429  _antType.push_back(anttype);


1430  }


1431  }


1432  if (type == 1033) {


1433  SKIPBITS(8)


1434  GETSTRING(dnum, dummy)


1435  GETSTRING(recnum, receiver)


1436  if (recnum > 1 && recnum < 265) {


1437  memcpy(rectype, receiver, recnum);


1438  rectype[recnum] = 0;


1439  if (!_recType.contains(rectype)) {


1440  _recType.push_back(rectype);


1441  }


1442  }


1443  }


1444  return true;


1445  }


1446 


1447  //


1448  ////////////////////////////////////////////////////////////////////////////


1449  bool RTCM3Decoder::DecodeAntennaPosition(unsigned char* data, int size) {


1450  int type;


1451  uint64_t numbits = 0, bitfield = 0;


1452  double x, y, z;


1453 


1454  data += 3; /* header */


1455  size = 6; /* header + crc */


1456 


1457  GETBITS(type, 12)


1458  _antList.push_back(t_antInfo());


1459  _antList.back().type = t_antInfo::ARP;


1460  SKIPBITS(22)


1461  GETBITSSIGN(x, 38)


1462  _antList.back().xx = x * 1e4;


1463  SKIPBITS(2)


1464  GETBITSSIGN(y, 38)


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


1466  SKIPBITS(2)


1467  GETBITSSIGN(z, 38)


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


1469  if (type == 1006)


1470  {


1471  double h;


1472  GETBITS(h, 16)


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


1474  _antList.back().height_f = true;


1475  }


1476  _antList.back().message = type;


1477 


1478  return true;


1479  }


1480 


1481  //


1482  ////////////////////////////////////////////////////////////////////////////


1483  t_irc RTCM3Decoder::Decode(char* buffer, int bufLen, vector<string>& errmsg) {


1484  bool decoded = false;


1485 


1486  errmsg.clear();


1487 


1488  while (bufLen && _MessageSize < sizeof(_Message)) {


1489  int l = sizeof(_Message)  _MessageSize;


1490  if (l > bufLen)


1491  l = bufLen;


1492  memcpy(_Message + _MessageSize, buffer, l);


1493  _MessageSize += l;


1494  bufLen = l;


1495  buffer += l;


1496  int id;


1497  while ((id = GetMessage())) {


1498  /* reset station ID for file loading as it can change */


1499  if (_rawFile)


1500  _staID = _rawFile>staID();


1501  /* store the id into the list of loaded blocks */


1502  _typeList.push_back(id);


1503 


1504  /* SSR I+II data handled in another function, already pass the


1505  * extracted data block. That does no harm, as it anyway skip everything


1506  * else. */


1507  if ((id >= 1057 && id <= 1068)  (id >= 1240 && id <= 1270)) {


1508  if (!_coDecoders.contains(_staID.toAscii()))


1509  _coDecoders[_staID.toAscii()] = new RTCM3coDecoder(_staID);


1510  RTCM3coDecoder* coDecoder = _coDecoders[_staID.toAscii()];


1511  if (coDecoder>Decode(reinterpret_cast<char *>(_Message), _BlockSize,


1512  errmsg) == success) {


1513  decoded = true;


1514  }


1515  }


1516  else if (id >= 1070 && id <= 1229) /* MSM */ {


1517  if (DecodeRTCM3MSM(_Message, _BlockSize))


1518  decoded = true;


1519  }


1520  else {


1521  switch (id) {


1522  case 1001:


1523  case 1003:


1524  emit(newMessage(


1525  QString("%1: Block %2 contain partial data! Ignored!")


1526  .arg(_staID).arg(id).toAscii(), true));


1527  break; /* no use decoding partial data ATM, remove break when data can be used */


1528  case 1002:


1529  case 1004:


1530  if (DecodeRTCM3GPS(_Message, _BlockSize))


1531  decoded = true;


1532  break;


1533  case 1009:


1534  case 1011:


1535  emit(newMessage(


1536  QString("%1: Block %2 contain partial data! Ignored!")


1537  .arg(_staID).arg(id).toAscii(), true));


1538  break; /* no use decoding partial data ATM, remove break when data can be used */


1539  case 1010:


1540  case 1012:


1541  if (DecodeRTCM3GLONASS(_Message, _BlockSize))


1542  decoded = true;


1543  break;


1544  case 1019:


1545  if (DecodeGPSEphemeris(_Message, _BlockSize))


1546  decoded = true;


1547  break;


1548  case 1020:


1549  if (DecodeGLONASSEphemeris(_Message, _BlockSize))


1550  decoded = true;


1551  break;


1552  case 1043:


1553  if (DecodeSBASEphemeris(_Message, _BlockSize))


1554  decoded = true;


1555  break;


1556  case 1044:


1557  if (DecodeQZSSEphemeris(_Message, _BlockSize))


1558  decoded = true;


1559  break;


1560  case 1045:


1561  case 1046:


1562  if (DecodeGalileoEphemeris(_Message, _BlockSize))


1563  decoded = true;


1564  break;


1565  case RTCM3ID_BDS:


1566  if (DecodeBDSEphemeris(_Message, _BlockSize))


1567  decoded = true;


1568  break;


1569  case 1007:


1570  case 1008:


1571  case 1033:


1572  DecodeAntennaReceiver(_Message, _BlockSize);


1573  break;


1574  case 1005:


1575  case 1006:


1576  DecodeAntennaPosition(_Message, _BlockSize);


1577  break;


1578  }


1579  }


1580  }


1581  }


1582  return decoded ? success : failure;


1583  }


1584  ;


1585 


1586  //


1587  ////////////////////////////////////////////////////////////////////////////


1588  uint32_t RTCM3Decoder::CRC24(long size, const unsigned char *buf) {


1589  uint32_t crc = 0;


1590  int i;


1591 


1592  while (size) {


1593  crc ^= (*buf++) << (16);


1594  for (i = 0; i < 8; i++)


1595  {


1596  crc <<= 1;


1597  if (crc & 0x1000000)


1598  crc ^= 0x01864cfb;


1599  }


1600  }


1601  return crc;


1602  }


1603 


1604  //


1605  ////////////////////////////////////////////////////////////////////////////


1606  int RTCM3Decoder::GetMessage(void) {


1607  unsigned char *m, *e;


1608  int i;


1609 


1610  m = _Message + _SkipBytes;


1611  e = _Message + _MessageSize;


1612  _NeedBytes = _SkipBytes = 0;


1613  while (e  m >= 3) {


1614  if (m[0] == 0xD3) {


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


1616  if (e  m >= static_cast<int>(_BlockSize + 6)) {


1617  if (static_cast<uint32_t>((m[3 + _BlockSize] << 16)


1618   (m[3 + _BlockSize + 1] << 8)


1619   (m[3 + _BlockSize + 2])) == CRC24(_BlockSize + 3, m)) {


1620  _BlockSize += 6;


1621  _SkipBytes = _BlockSize;


1622  break;


1623  }


1624  else


1625  ++m;


1626  }


1627  else {


1628  _NeedBytes = _BlockSize;


1629  break;


1630  }


1631  }


1632  else


1633  ++m;


1634  }


1635  if (e  m < 3)


1636  _NeedBytes = 3;


1637 


1638  /* copy buffer to front */


1639  i = m  _Message;


1640  if (i && m < e)


1641  memmove(_Message, m, static_cast<size_t>(_MessageSize  i));


1642  _MessageSize = i;


1643 


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


1645  }


1646 


1647  // Time of Corrections


1648  //////////////////////////////////////////////////////////////////////////////


1649  int RTCM3Decoder::corrGPSEpochTime() const {


1650  return


1651  _coDecoders.size() > 0 ?


1652  _coDecoders.begin().value()>corrGPSEpochTime() : 1;


1653  }

