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>_slipCounter = i;


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


153  GETBITS(amb, 8);


154  if (amb) {


155  frqObs>_code += amb * 299792.458;


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


157  }


158  GETBITS(i, 8);


159  if (i) {


160  frqObs>_snr = i * 0.25;


161  frqObs>_snrValid = true;


162  }


163  }


164  CurrentObs._obs.push_back(frqObs);


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


166  frqObs = new t_frqObs;


167  /* L2 */


168  GETBITS(code, 2);


169  switch (code) {


170  case 3:


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


172  break;


173  case 2:


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


175  break;


176  case 1:


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


178  break;


179  case 0:


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


181  break;


182  }


183  GETBITSSIGN(i, 14);


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


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


186  frqObs>_codeValid = true;


187  }


188  GETBITSSIGN(i, 20);


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


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


191  / GPS_WAVELENGTH_L2;


192  frqObs>_phaseValid = true;


193  }


194  GETBITS(i, 7);


195  frqObs>_slipCounter = i;


196  if (type == 1004) {


197  GETBITS(i, 8);


198  if (i) {


199  frqObs>_snr = i * 0.25;


200  frqObs>_snrValid = true;


201  }


202  }


203  CurrentObs._obs.push_back(frqObs);


204  }


205  _CurrentObsList.push_back(CurrentObs);


206  }


207 


208  if (!syncf) {


209  decoded = true;


210  _obsList.append(_CurrentObsList);


211  _CurrentTime.reset();


212  _CurrentObsList.clear();


213  }


214  return decoded;


215  }


216 


217  #define RTCM3_MSM_NUMSIG 32


218  #define RTCM3_MSM_NUMSAT 64


219  #define RTCM3_MSM_NUMCELLS 96 /* arbitrary limit */


220 


221  /**


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


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


224  */


225  static int GLOFreq[RTCM3_MSM_NUMSAT];


226 


227  /*


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


229  * message */


230  struct CodeData {


231  double wl;


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


233  };


234 


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


236  static struct CodeData gps[RTCM3_MSM_NUMSIG] = {


237  {0.0, 0},


238  {GPS_WAVELENGTH_L1, "1C"},


239  {GPS_WAVELENGTH_L1, "1P"},


240  {GPS_WAVELENGTH_L1, "1W"},


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


242  {0.0, 0},


243  {0.0, 0},


244  {GPS_WAVELENGTH_L2, "2C"},


245  {GPS_WAVELENGTH_L2, "2P"},


246  {GPS_WAVELENGTH_L2, "2W"},


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


248  {0.0, 0},


249  {0.0, 0},


250  {0.0, 0},


251  {GPS_WAVELENGTH_L2, "2S"},


252  {GPS_WAVELENGTH_L2, "2L"},


253  {GPS_WAVELENGTH_L2, "2X"},


254  {0.0, 0},


255  {0.0, 0},


256  {0.0, 0},


257  {0.0, 0},


258  {GPS_WAVELENGTH_L5, "5I"},


259  {GPS_WAVELENGTH_L5, "5Q"},


260  {GPS_WAVELENGTH_L5, "5X"},


261  {0.0, 0},


262  {0.0, 0},


263  {0.0, 0},


264  {0.0, 0},


265  {0.0, 0},


266  {GPS_WAVELENGTH_L1, "1S"},


267  {GPS_WAVELENGTH_L1, "1L"},


268  {GPS_WAVELENGTH_L1, "1X"}


269  };


270 


271  /**


272  * MSM signal types for GLONASS


273  *


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


275  */


276  static struct CodeData glo[RTCM3_MSM_NUMSIG] = {


277  {0.0, 0},


278  {0.0, "1C"},


279  {0.0, "1P"},


280  {0.0, 0},


281  {0.0, 0},


282  {0.0, 0},


283  {0.0, 0},


284  {1.0, "2C"},


285  {1.0, "2P"},


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


306  {0.0, 0},


307  {0.0, 0},


308  {0.0, 0}


309  };


310 


311  /** MSM signal types for Galileo */


312  static struct CodeData gal[RTCM3_MSM_NUMSIG] = {


313  {0.0, 0},


314  {GAL_WAVELENGTH_E1, "1C"},


315  {GAL_WAVELENGTH_E1, "1A"},


316  {GAL_WAVELENGTH_E1, "1B"},


317  {GAL_WAVELENGTH_E1, "1X"},


318  {GAL_WAVELENGTH_E1, "1Z"},


319  {0.0, 0},


320  {GAL_WAVELENGTH_E6, "6C"},


321  {GAL_WAVELENGTH_E6, "6A"},


322  {GAL_WAVELENGTH_E6, "6B"},


323  {GAL_WAVELENGTH_E6, "6X"},


324  {GAL_WAVELENGTH_E6, "6Z"},


325  {0.0, 0},


326  {GAL_WAVELENGTH_E5B, "7I"},


327  {GAL_WAVELENGTH_E5B, "7Q"},


328  {GAL_WAVELENGTH_E5B, "7X"},


329  {0.0, 0},


330  {GAL_WAVELENGTH_E5AB, "8I"},


331  {GAL_WAVELENGTH_E5AB, "8Q"},


332  {GAL_WAVELENGTH_E5AB, "8X"},


333  {0.0, 0},


334  {GAL_WAVELENGTH_E5A, "5I"},


335  {GAL_WAVELENGTH_E5A, "5Q"},


336  {GAL_WAVELENGTH_E5A, "5X"},


337  {0.0, 0},


338  {0.0, 0},


339  {0.0, 0},


340  {0.0, 0},


341  {0.0, 0},


342  {0.0, 0},


343  {0.0, 0},


344  {0.0, 0},


345  };


346 


347  /** MSM signal types for QZSS */


348  static struct CodeData qzss[RTCM3_MSM_NUMSIG] = {


349  {0.0, 0},


350  {GPS_WAVELENGTH_L1, "1C"},


351  {0.0, 0},


352  {0.0, 0},


353  {0.0, 0},


354  {GPS_WAVELENGTH_L1, "1Z"},


355  {0.0, 0},


356  {0.0, 0},


357  {QZSS_WAVELENGTH_LEX, "6S"},


358  {QZSS_WAVELENGTH_LEX, "6L"},


359  {QZSS_WAVELENGTH_LEX, "6X"},


360  {0.0, 0},


361  {0.0, 0},


362  {0.0, 0},


363  {GPS_WAVELENGTH_L2, "2S"},


364  {GPS_WAVELENGTH_L2, "2L"},


365  {GPS_WAVELENGTH_L2, "2X"},


366  {0.0, 0},


367  {0.0, 0},


368  {0.0, 0},


369  {0.0, 0},


370  {GPS_WAVELENGTH_L5, "5I"},


371  {GPS_WAVELENGTH_L5, "5Q"},


372  {GPS_WAVELENGTH_L5, "5X"},


373  {0.0, 0},


374  {0.0, 0},


375  {0.0, 0},


376  {0.0, 0},


377  {0.0, 0},


378  {GPS_WAVELENGTH_L1, "1D"},


379  {GPS_WAVELENGTH_L1, "1P"},


380  {GPS_WAVELENGTH_L1, "1X"}


381  };


382 


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


384  static struct CodeData bds[RTCM3_MSM_NUMSIG] = {


385  {0.0, 0},


386  {BDS_WAVELENGTH_B1, "2I"},


387  {0.0, 0},


388  {0.0, 0},


389  {0.0, 0},


390  {0.0, 0},


391  {0.0, 0},


392  {BDS_WAVELENGTH_B3, "6I"},


393  {0.0, 0},


394  {0.0, 0},


395  {0.0, 0},


396  {0.0, 0},


397  {0.0, 0},


398  {BDS_WAVELENGTH_B2, "7I"},


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


417  };


418 


419  #define UINT64(c) c ## ULL


420 


421  //


422  ////////////////////////////////////////////////////////////////////////////


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


424  {


425  bool decoded = false;


426  int type, syncf, i;


427  uint64_t numbits = 0, bitfield = 0;


428 


429  data += 3; /* header */


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


431 


432  GETBITS(type, 12)


433  SKIPBITS(12)


434  /* id */


435  char sys;


436  if (type >= 1121)


437  sys = 'C';


438  else if (type >= 1111)


439  sys = 'J';


440  else if (type >= 1101)


441  sys = 'S';


442  else if (type >= 1091)


443  sys = 'E';


444  else if (type >= 1081)


445  sys = 'R';


446  else


447  sys = 'G';


448 


449  bncTime CurrentObsTime;


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


451  GETBITS(i, 30)


452  CurrentObsTime.setBDS(i);


453  }


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


455  SKIPBITS(3)


456  GETBITS(i, 27)


457  /* tk */


458  CurrentObsTime.setTk(i);


459  }


460  else /* GPS style date */ {


461  GETBITS(i, 30)


462  CurrentObsTime.set(i);


463  }


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


465  decoded = true;


466  _obsList.append(_CurrentObsList);


467  _CurrentObsList.clear();


468  }


469  _CurrentTime = CurrentObsTime;


470 


471  GETBITS(syncf, 1)


472  /**


473  * Ignore unknown types except for sync flag


474  *


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


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


477  */


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


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


480  uint64_t satmask, cellmask, ui;


481  double rrmod[RTCM3_MSM_NUMSAT];


482  int rrint[RTCM3_MSM_NUMSAT], rdop[RTCM3_MSM_NUMSAT],


483  extsat[RTCM3_MSM_NUMSAT];


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


485  double cnr[RTCM3_MSM_NUMCELLS];


486  double cp[RTCM3_MSM_NUMCELLS], psr[RTCM3_MSM_NUMCELLS],


487  dop[RTCM3_MSM_NUMCELLS];


488 


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


490  GETBITS64(satmask, RTCM3_MSM_NUMSAT)


491 


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


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


494  ++numsat;


495  GETBITS(sigmask, RTCM3_MSM_NUMSIG)


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


497  ++numsig;


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


499  extsat[i] = 15;


500 


501  i = numsat * numsig;


502  GETBITS64(cellmask, (unsigned )i)


503 


504  switch (type % 10) {


505  case 1:


506  case 2:


507  case 3:


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


509  for (int j = numsat; j;)


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


511  break;


512  case 4:


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


520  case 7:


521  for (int j = numsat; j;)


522  GETBITS(rrint[j], 8)


523  for (int j = numsat; j;)


524  GETBITS(extsat[j], 4)


525  for (int j = numsat; j;)


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


527  for (int j = numsat; j;)


528  GETBITSSIGN(rdop[j], 14)


529  break;


530  }


531 


532  int numcells = numsat * numsig;


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


534  * when that happens. */


535  if (numcells <= RTCM3_MSM_NUMCELLS) {


536  switch (type % 10) {


537  case 1:


538  for (int count = numcells; count;)


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


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


541  break;


542  case 2:


543  for (int count = numcells; count;)


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


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


546  for (int count = numcells; count;)


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


548  GETBITS(ll[count], 4)


549  for (int count = numcells; count;)


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


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


552  break;


553  case 3:


554  for (int count = numcells; count;)


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


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


557  for (int count = numcells; count;)


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


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


560  for (int count = numcells; count;)


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


562  GETBITS(ll[count], 4)


563  for (int count = numcells; count;)


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


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


566  break;


567  case 4:


568  for (int count = numcells; count;)


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


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


571  for (int count = numcells; count;)


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


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


574  for (int count = numcells; count;)


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


576  GETBITS(ll[count], 4)


577  for (int count = numcells; count;)


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


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


580  for (int count = numcells; count;)


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


582  GETBITS(cnr[count], 6)


583  break;


584  case 5:


585  for (int count = numcells; count;)


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


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


588  for (int count = numcells; count;)


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


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


591  for (int count = numcells; count;)


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


593  GETBITS(ll[count], 4)


594  for (int count = numcells; count;)


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


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


597  for (int count = numcells; count;)


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


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


600  for (int count = numcells; count;)


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


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


603  break;


604  case 6:


605  for (int count = numcells; count;)


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


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


608  for (int count = numcells; count;)


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


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


611  for (int count = numcells; count;)


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


613  GETBITS(ll[count], 10)


614  for (int count = numcells; count;)


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


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


617  for (int count = numcells; count;)


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


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


620  break;


621  case 7:


622  for (int count = numcells; count;)


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


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


625  for (int count = numcells; count;)


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


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


628  for (int count = numcells; count;)


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


630  GETBITS(ll[count], 10)


631  for (int count = numcells; count;)


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


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


634  for (int count = numcells; count;)


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


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


637  for (int count = numcells; count;)


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


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


640  break;


641  }


642  i = RTCM3_MSM_NUMSAT;


643  int j = 1;


644  t_satObs CurrentObs;


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


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


647  ;


648  if (j < 0) {


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


650  /* next satellite */


651  ;


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


653  _CurrentObsList.push_back(CurrentObs);


654  CurrentObs.clear();


655  CurrentObs._time = CurrentObsTime;


656  if (sys == 'S')


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


658  else


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


660  j = RTCM3_MSM_NUMSIG;


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


662  ;


663  numsat;


664  }


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


666  struct CodeData cd = {0.0, 0};


667  switch (sys) {


668  case 'J':


669  cd = qzss[RTCM3_MSM_NUMSIG  j  1];


670  break;


671  case 'C':


672  cd = bds[RTCM3_MSM_NUMSIG  j  1];


673  break;


674  case 'G':


675  case 'S':


676  cd = gps[RTCM3_MSM_NUMSIG  j  1];


677  break;


678  case 'R':


679  cd = glo[RTCM3_MSM_NUMSIG  j  1];


680  {


681  int k = GLOFreq[RTCM3_MSM_NUMSAT  i  1];


682  if (extsat[numsat] < 14) {


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


684   7;


685  }


686  if (k)


687  cd.wl = (


688  cd.wl == 0.0 ?


689  GLO_WAVELENGTH_L1(k  100) :


690  GLO_WAVELENGTH_L2(k  100));


691  else


692  cd.code = 0;


693  }


694  break;


695  case 'E':


696  cd = gal[RTCM3_MSM_NUMSIG  j  1];


697  break;


698  }


699  if (cd.code) {


700  t_frqObs *frqObs = new t_frqObs;


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


702 


703  switch (type % 10) {


704  case 1:


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


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


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


708  frqObs>_codeValid = true;


709  }


710  break;


711  case 2:


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


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


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


715  frqObs>_phaseValid = true;


716  frqObs>_slipCounter = ll[count];


717  }


718  break;


719  case 3:


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


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


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


723  frqObs>_codeValid = true;


724  }


725 


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


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


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


729  frqObs>_phaseValid = true;


730  frqObs>_slipCounter = ll[count];


731  }


732  break;


733  case 4:


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


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


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


737  frqObs>_codeValid = true;


738  }


739 


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


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


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


743  / cd.wl;


744  frqObs>_phaseValid = true;


745  frqObs>_slipCounter = ll[count];


746  }


747 


748  frqObs>_snr = cnr[count];


749  frqObs>_snrValid = true;


750  break;


751  case 5:


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


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


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


755  frqObs>_codeValid = true;


756  }


757 


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


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


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


761  / cd.wl;


762  frqObs>_phaseValid = true;


763  frqObs>_slipCounter = ll[count];


764  }


765 


766  frqObs>_snr = cnr[count];


767  frqObs>_snrValid = true;


768 


769  if (dop[count] > 1.6384) {


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


771  frqObs>_dopplerValid = true;


772  }


773  break;


774  case 6:


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


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


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


778  frqObs>_codeValid = true;


779  }


780 


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


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


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


784  / cd.wl;


785  frqObs>_phaseValid = true;


786  frqObs>_slipCounter = ll[count];


787  }


788 


789  frqObs>_snr = cnr[count];


790  frqObs>_snrValid = true;


791  break;


792  case 7:


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


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


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


796  frqObs>_codeValid = true;


797  }


798 


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


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


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


802  / cd.wl;


803  frqObs>_phaseValid = true;


804  frqObs>_slipCounter = ll[count];


805  }


806 


807  frqObs>_snr = cnr[count];


808  frqObs>_snrValid = true;


809 


810  if (dop[count] > 1.6384) {


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


812  frqObs>_dopplerValid = true;


813  }


814  break;


815  }


816  CurrentObs._obs.push_back(frqObs);


817  }


818  }


819  }


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


821  _CurrentObsList.push_back(CurrentObs);


822  }


823  }


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


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


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


827  }


828  if (!syncf) {


829  decoded = true;


830  _obsList.append(_CurrentObsList);


831  _CurrentTime.reset();


832  _CurrentObsList.clear();


833  }


834  return decoded;


835  }


836 


837  //


838  ////////////////////////////////////////////////////////////////////////////


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


840  bool decoded = false;


841  bncTime CurrentObsTime;


842  int i, numsats, syncf, type;


843  uint64_t numbits = 0, bitfield = 0;


844 


845  data += 3; /* header */


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


847 


848  GETBITS(type, 12)


849  SKIPBITS(12)


850  /* id */


851  GETBITS(i, 27)


852  /* tk */


853 


854  CurrentObsTime.setTk(i);


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


856  decoded = true;


857  _obsList.append(_CurrentObsList);


858  _CurrentObsList.clear();


859  }


860  _CurrentTime = CurrentObsTime;


861 


862  GETBITS(syncf, 1)


863  /* sync */


864  GETBITS(numsats, 5)


865  SKIPBITS(4)


866  /* smind, smint */


867 


868  while (numsats) {


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


870  t_satObs CurrentObs;


871  CurrentObs._time = CurrentObsTime;


872 


873  GETBITS(sv, 6)


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


875  GETBITS(code, 1)


876  GETBITS(freq, 5)


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


878 


879  t_frqObs *frqObs = new t_frqObs;


880  /* L1 */


881  (code) ?


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


883  GETBITS(l1range, 25);


884  GETBITSSIGN(i, 20);


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


886  frqObs>_code = l1range * 0.02;


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


888  / GLO_WAVELENGTH_L1(freq  7);


889  frqObs>_codeValid = frqObs>_phaseValid = true;


890  }


891  GETBITS(i, 7);


892  frqObs>_slipCounter = i;


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


894  GETBITS(amb, 7);


895  if (amb) {


896  frqObs>_code += amb * 599584.916;


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


898  }


899  GETBITS(i, 8);


900  if (i) {


901  frqObs>_snr = i * 0.25;


902  frqObs>_snrValid = true;


903  }


904  }


905  CurrentObs._obs.push_back(frqObs);


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


907  frqObs = new t_frqObs;


908  /* L2 */


909  GETBITS(code, 2);


910  switch (code) {


911  case 3:


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


913  break;


914  case 2:


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


916  break;


917  case 1:


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


919  break;


920  case 0:


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


922  break;


923  }


924  GETBITSSIGN(i, 14);


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


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


927  frqObs>_codeValid = true;


928  }


929  GETBITSSIGN(i, 20);


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


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


932  / GLO_WAVELENGTH_L2(freq  7);


933  frqObs>_phaseValid = true;


934  }


935  GETBITS(i, 7);


936  frqObs>_slipCounter = i;


937  if (type == 1012) {


938  GETBITS(i, 8);


939  if (i) {


940  frqObs>_snr = i * 0.25;


941  frqObs>_snrValid = true;


942  }


943  }


944  CurrentObs._obs.push_back(frqObs);


945  }


946  _CurrentObsList.push_back(CurrentObs);


947  }


948  if (!syncf) {


949  decoded = true;


950  _obsList.append(_CurrentObsList);


951  _CurrentTime.reset();


952  _CurrentObsList.clear();


953  }


954  return decoded;


955  }


956 


957  //


958  ////////////////////////////////////////////////////////////////////////////


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


960  bool decoded = false;


961 


962  if (size == 67) {


963  t_ephGPS eph;


964  int i, week;


965  uint64_t numbits = 0, bitfield = 0;


966 


967  data += 3; /* header */


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


969  SKIPBITS(12)


970 


971  eph._receptDateTime = currentDateAndTimeGPS();


972 


973  GETBITS(i, 6)


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


975  GETBITS(week, 10)


976  week += 1024;


977  GETBITS(i, 4)


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


979  GETBITS(eph._L2Codes, 2)


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


981  GETBITS(eph._IODE, 8)


982  GETBITS(i, 16)


983  i <<= 4;


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


985  GETFLOATSIGN(eph._clock_driftrate, 8,


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


987  GETFLOATSIGN(eph._clock_drift, 16,


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


989  GETFLOATSIGN(eph._clock_bias, 22,


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


991  GETBITS(eph._IODC, 10)


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


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


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


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


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


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


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


999  GETBITS(i, 16)


1000  i <<= 4;


1001  eph._TOEsec = i;


1002  bncTime t;


1003  t.set(i * 1000);


1004  eph._TOEweek = t.gpsw();


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


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


1007  return false;


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


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


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


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


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


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


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


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


1016  GETBITS(eph._health, 6)


1017  GETBITS(eph._L2PFlag, 1)


1018  GETBITS(eph._fitInterval, 1)


1019  eph._TOT = 0.9999e9;


1020 


1021  emit newGPSEph(eph);


1022  decoded = true;


1023  }


1024  return decoded;


1025  }


1026 


1027  //


1028  ////////////////////////////////////////////////////////////////////////////


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


1030  bool decoded = false;


1031 


1032  if (size == 51) {


1033  t_ephGlo eph;


1034  int sv, i, tk;


1035  uint64_t numbits = 0, bitfield = 0;


1036 


1037  data += 3; /* header */


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


1039  SKIPBITS(12)


1040 


1041  eph._receptDateTime = currentDateAndTimeGPS();


1042 


1043  GETBITS(sv, 6)


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


1045 


1046  GETBITS(i, 5)


1047  eph._frequency_number = i  7;


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


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


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


1051  GETBITS(i, 5)


1052  tk = i * 60 * 60;


1053  GETBITS(i, 6)


1054  tk += i * 60;


1055  GETBITS(i, 1)


1056  tk += i * 30;


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


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


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


1060  GETBITS(i, 7)


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


1062 


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


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


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


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


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


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


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


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


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


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


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


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


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


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


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


1078  GETBITS(eph._E, 5)


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


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


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


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


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


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


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


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


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


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


1089 


1090  unsigned year, month, day;


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


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


1093  eph._tt = eph._TOC;


1094 


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


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


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


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


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


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


1101 


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


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


1104 


1105  emit newGlonassEph(eph);


1106  decoded = true;


1107  }


1108  return decoded;


1109  }


1110 


1111  //


1112  ////////////////////////////////////////////////////////////////////////////


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


1114  bool decoded = false;


1115 


1116  if (size == 67) {


1117  t_ephGPS eph;


1118  int i, week;


1119  uint64_t numbits = 0, bitfield = 0;


1120 


1121  data += 3; /* header */


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


1123  SKIPBITS(12)


1124 


1125  eph._receptDateTime = currentDateAndTimeGPS();


1126 


1127  GETBITS(i, 4)


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


1129 


1130  GETBITS(i, 16)


1131  i <<= 4;


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


1133 


1134  GETFLOATSIGN(eph._clock_driftrate, 8,


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


1136  GETFLOATSIGN(eph._clock_drift, 16,


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


1138  GETFLOATSIGN(eph._clock_bias, 22,


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


1140  GETBITS(eph._IODE, 8)


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


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


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


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


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


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


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


1148  GETBITS(i, 16)


1149  i <<= 4;


1150  eph._TOEsec = i;


1151  bncTime t;


1152  t.set(i*1000);


1153  eph._TOEweek = t.gpsw();


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


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


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


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


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


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


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


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


1162  GETBITS(eph._L2Codes, 2)


1163  GETBITS(week, 10)


1164  week += 1024;


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


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


1167  return false;


1168 


1169  GETBITS(i, 4)


1170  if (i <= 6)


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


1172  else


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


1174  GETBITS(eph._health, 6)


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


1176  GETBITS(eph._IODC, 10)


1177  GETBITS(eph._fitInterval, 1)


1178  eph._TOT = 0.9999e9;


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


1180 


1181  emit newGPSEph(eph);


1182  decoded = true;


1183  }


1184  return decoded;


1185  }


1186 


1187  //


1188  ////////////////////////////////////////////////////////////////////////////


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


1190  bool decoded = false;


1191 


1192  if (size == 35) {


1193  t_ephSBAS eph;


1194  int i;


1195  uint64_t numbits = 0, bitfield = 0;


1196 


1197  data += 3; /* header */


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


1199  SKIPBITS(12)


1200 


1201  eph._receptDateTime = currentDateAndTimeGPS();


1202 


1203  GETBITS(i, 6)


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


1205  GETBITS(eph._IODN, 8)


1206  GETBITS(i, 13)


1207  i <<= 4;


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


1209  GETBITS(i, 4)


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


1211  GETFLOATSIGN(eph._x_pos, 30, 0.08)


1212  GETFLOATSIGN(eph._y_pos, 30, 0.08)


1213  GETFLOATSIGN(eph._z_pos, 25, 0.4)


1214  GETFLOATSIGN(eph._x_velocity, 17, 0.000625)


1215  GETFLOATSIGN(eph._y_velocity, 17, 0.000625)


1216  GETFLOATSIGN(eph._z_velocity, 18, 0.004)


1217  GETFLOATSIGN(eph._x_acceleration, 10, 0.0000125)


1218  GETFLOATSIGN(eph._y_acceleration, 10, 0.0000125)


1219  GETFLOATSIGN(eph._z_acceleration, 10, 0.0000625)


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


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


1222 


1223  eph._TOT = 0.9999E9;


1224  eph._health = 0;


1225 


1226  emit newSBASEph(eph);


1227  decoded = true;


1228  }


1229  return decoded;


1230  }


1231 


1232  //


1233  ////////////////////////////////////////////////////////////////////////////


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


1235  bool decoded = false;


1236  uint64_t numbits = 0, bitfield = 0;


1237  int i;


1238 


1239  data += 3; /* header */


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


1241  GETBITS(i, 12)


1242 


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


1244  t_ephGal eph;


1245 


1246  eph._receptDateTime = currentDateAndTimeGPS();


1247 


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


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


1250  GETBITS(i, 6)


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


1252 


1253  GETBITS(eph._TOEweek, 12)


1254  GETBITS(eph._IODnav, 10)


1255  GETBITS(i, 8)


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


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


1258  GETBITSFACTOR(i, 14, 60)


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


1260  GETFLOATSIGN(eph._clock_driftrate, 6,


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


1262  GETFLOATSIGN(eph._clock_drift, 21,


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


1264  GETFLOATSIGN(eph._clock_bias, 31,


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


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


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


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


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


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


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


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


1273  GETBITSFACTOR(eph._TOEsec, 14, 60)


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


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


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


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


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


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


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


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


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


1283  GETFLOATSIGN(eph._BGD_1_5A, 10,


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


1285  if (eph._inav) {


1286  /* set unused F/NAV values */


1287  eph._E5aHS = 0.0;


1288  eph._e5aDataInValid = false;


1289 


1290  GETFLOATSIGN(eph._BGD_1_5B, 10,


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


1292  GETBITS(eph._E5bHS, 2)


1293  GETBITS(eph._e5bDataInValid, 1)


1294  GETBITS(eph._E1_bHS, 2)


1295  GETBITS(eph._e1DataInValid, 1)


1296  }


1297  else {


1298  /* set unused I/NAV values */


1299  eph._BGD_1_5B = 0.0;


1300  eph._E5bHS = 0.0;


1301  eph._E1_bHS = 0.0;


1302  eph._e1DataInValid = false;


1303  eph._e5bDataInValid = false;


1304 


1305  GETBITS(eph._E5aHS, 2)


1306  GETBITS(eph._e5aDataInValid, 1)


1307  }


1308  eph._TOT = 0.9999e9;


1309 


1310  emit newGalileoEph(eph);


1311  decoded = true;


1312  }


1313  return decoded;


1314  }


1315 


1316  //


1317  ////////////////////////////////////////////////////////////////////////////


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


1319  bool decoded = false;


1320 


1321  if (size == 70) {


1322  t_ephBDS eph;


1323  int i;


1324  uint64_t numbits = 0, bitfield = 0;


1325 


1326  data += 3; /* header */


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


1328  SKIPBITS(12)


1329 


1330  eph._receptDateTime = currentDateAndTimeGPS();


1331 


1332  GETBITS(i, 6)


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


1334 


1335  SKIPBITS(13)


1336  /* week */


1337  GETBITS(i, 4)


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


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


1340  GETBITS(eph._AODE, 5)


1341  GETBITS(i, 17)


1342  i <<= 3;


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


1344  GETFLOATSIGN(eph._clock_driftrate, 11,


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


1346  GETFLOATSIGN(eph._clock_drift, 22,


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


1348  GETFLOATSIGN(eph._clock_bias, 24,


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


1350  GETBITS(eph._AODC, 5)


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


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


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


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


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


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


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


1358  GETBITS(i, 17)


1359  i <<= 3;


1360  eph._TOEsec = i;


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


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


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


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


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


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


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


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


1369  GETFLOATSIGN(eph._TGD1, 10, 0.0000000001)


1370  GETFLOATSIGN(eph._TGD2, 10, 0.0000000001)


1371  GETBITS(eph._SatH1, 1)


1372 


1373  eph._TOT = 0.9999E9;


1374  emit newBDSEph(eph);


1375  decoded = true;


1376  }


1377  return decoded;


1378  }


1379 


1380  //


1381  ////////////////////////////////////////////////////////////////////////////


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


1383  char *antenna, anttype[256];


1384  char *dummy;


1385  char *receiver, rectype[256];


1386 


1387  int type;


1388 


1389  int dnum = 1;


1390  int antnum = 1;


1391  int recnum = 1;


1392  uint64_t numbits = 0, bitfield = 0;


1393 


1394  data += 3; /* header*/


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


1396 


1397  GETBITS(type, 12)


1398  SKIPBITS(12)


1399  GETSTRING(antnum, antenna)


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


1401  memcpy(anttype, antenna, antnum);


1402  anttype[antnum] = 0;


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


1404  _antType.push_back(anttype);


1405  }


1406  }


1407  if (type == 1033) {


1408  SKIPBITS(8)


1409  GETSTRING(dnum, dummy)


1410  GETSTRING(recnum, receiver)


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


1412  memcpy(rectype, receiver, recnum);


1413  rectype[recnum] = 0;


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


1415  _recType.push_back(rectype);


1416  }


1417  }


1418  }


1419  return true;


1420  }


1421 


1422  //


1423  ////////////////////////////////////////////////////////////////////////////


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


1425  int type;


1426  uint64_t numbits = 0, bitfield = 0;


1427  double x, y, z;


1428 


1429  data += 3; /* header */


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


1431 


1432  GETBITS(type, 12)


1433  _antList.push_back(t_antInfo());


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


1435  SKIPBITS(22)


1436  GETBITSSIGN(x, 38)


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


1438  SKIPBITS(2)


1439  GETBITSSIGN(y, 38)


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


1441  SKIPBITS(2)


1442  GETBITSSIGN(z, 38)


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


1444  if (type == 1006)


1445  {


1446  double h;


1447  GETBITS(h, 16)


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


1449  _antList.back().height_f = true;


1450  }


1451  _antList.back().message = type;


1452 


1453  return true;


1454  }


1455 


1456  //


1457  ////////////////////////////////////////////////////////////////////////////


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


1459  bool decoded = false;


1460 


1461  errmsg.clear();


1462 


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


1464  int l = sizeof(_Message)  _MessageSize;


1465  if (l > bufLen)


1466  l = bufLen;


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


1468  _MessageSize += l;


1469  bufLen = l;


1470  buffer += l;


1471  int id;


1472  while ((id = GetMessage())) {


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


1474  if (_rawFile)


1475  _staID = _rawFile>staID();


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


1477  _typeList.push_back(id);


1478 


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


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


1481  * else. */


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


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


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


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


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


1487  errmsg) == success) {


1488  decoded = true;


1489  }


1490  }


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


1492  if (DecodeRTCM3MSM(_Message, _BlockSize))


1493  decoded = true;


1494  }


1495  else {


1496  switch (id) {


1497  case 1001:


1498  case 1003:


1499  emit(newMessage(


1500  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 1002:


1504  case 1004:


1505  if (DecodeRTCM3GPS(_Message, _BlockSize))


1506  decoded = true;


1507  break;


1508  case 1009:


1509  case 1011:


1510  emit(newMessage(


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


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


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


1514  case 1010:


1515  case 1012:


1516  if (DecodeRTCM3GLONASS(_Message, _BlockSize))


1517  decoded = true;


1518  break;


1519  case 1019:


1520  if (DecodeGPSEphemeris(_Message, _BlockSize))


1521  decoded = true;


1522  break;


1523  case 1020:


1524  if (DecodeGLONASSEphemeris(_Message, _BlockSize))


1525  decoded = true;


1526  break;


1527  case 1043:


1528  if (DecodeSBASEphemeris(_Message, _BlockSize))


1529  decoded = true;


1530  break;


1531  case 1044:


1532  if (DecodeQZSSEphemeris(_Message, _BlockSize))


1533  decoded = true;


1534  break;


1535  case 1045:


1536  case 1046:


1537  if (DecodeGalileoEphemeris(_Message, _BlockSize))


1538  decoded = true;


1539  break;


1540  case RTCM3ID_BDS:


1541  if (DecodeBDSEphemeris(_Message, _BlockSize))


1542  decoded = true;


1543  break;


1544  case 1007:


1545  case 1008:


1546  case 1033:


1547  DecodeAntennaReceiver(_Message, _BlockSize);


1548  break;


1549  case 1005:


1550  case 1006:


1551  DecodeAntennaPosition(_Message, _BlockSize);


1552  break;


1553  }


1554  }


1555  }


1556  }


1557  return decoded ? success : failure;


1558  }


1559  ;


1560 


1561  //


1562  ////////////////////////////////////////////////////////////////////////////


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


1564  uint32_t crc = 0;


1565  int i;


1566 


1567  while (size) {


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


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


1570  {


1571  crc <<= 1;


1572  if (crc & 0x1000000)


1573  crc ^= 0x01864cfb;


1574  }


1575  }


1576  return crc;


1577  }


1578 


1579  //


1580  ////////////////////////////////////////////////////////////////////////////


1581  int RTCM3Decoder::GetMessage(void) {


1582  unsigned char *m, *e;


1583  int i;


1584 


1585  m = _Message + _SkipBytes;


1586  e = _Message + _MessageSize;


1587  _NeedBytes = _SkipBytes = 0;


1588  while (e  m >= 3) {


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


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


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


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


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


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


1595  _BlockSize += 6;


1596  _SkipBytes = _BlockSize;


1597  break;


1598  }


1599  else


1600  ++m;


1601  }


1602  else {


1603  _NeedBytes = _BlockSize;


1604  break;


1605  }


1606  }


1607  else


1608  ++m;


1609  }


1610  if (e  m < 3)


1611  _NeedBytes = 3;


1612 


1613  /* copy buffer to front */


1614  i = m  _Message;


1615  if (i && m < e)


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


1617  _MessageSize = i;


1618 


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


1620  }


1621 


1622  // Time of Corrections


1623  //////////////////////////////////////////////////////////////////////////////


1624  int RTCM3Decoder::corrGPSEpochTime() const {


1625  return


1626  _coDecoders.size() > 0 ?


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


1628  }

