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


1153 


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  eph._TOEweek = t.gpsw();


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


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


1168  return false;


1169 


1170  GETBITS(i, 4)


1171  if (i <= 6)


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


1173  else


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


1175  GETBITS(eph._health, 6)


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


1177  GETBITS(eph._IODC, 10)


1178  GETBITS(eph._fitInterval, 1)


1179  eph._TOT = 0.9999e9;


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


1181 


1182  emit newGPSEph(eph);


1183  decoded = true;


1184  }


1185  return decoded;


1186  }


1187 


1188  //


1189  ////////////////////////////////////////////////////////////////////////////


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


1191  bool decoded = false;


1192 


1193  if (size == 35) {


1194  t_ephSBAS eph;


1195  int i;


1196  uint64_t numbits = 0, bitfield = 0;


1197 


1198  data += 3; /* header */


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


1200  SKIPBITS(12)


1201 


1202  eph._receptDateTime = currentDateAndTimeGPS();


1203 


1204  GETBITS(i, 6)


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


1206  GETBITS(eph._IODN, 8)


1207  GETBITS(i, 13)


1208  i <<= 4;


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


1210  GETBITS(i, 4)


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


1212  GETFLOATSIGN(eph._x_pos, 30, 0.08)


1213  GETFLOATSIGN(eph._y_pos, 30, 0.08)


1214  GETFLOATSIGN(eph._z_pos, 25, 0.4)


1215  GETFLOATSIGN(eph._x_velocity, 17, 0.000625)


1216  GETFLOATSIGN(eph._y_velocity, 17, 0.000625)


1217  GETFLOATSIGN(eph._z_velocity, 18, 0.004)


1218  GETFLOATSIGN(eph._x_acceleration, 10, 0.0000125)


1219  GETFLOATSIGN(eph._y_acceleration, 10, 0.0000125)


1220  GETFLOATSIGN(eph._z_acceleration, 10, 0.0000625)


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


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


1223 


1224  eph._TOW = 0.9999E9;


1225  eph._health = 0;


1226 


1227  emit newSBASEph(eph);


1228  decoded = true;


1229  }


1230  return decoded;


1231  }


1232 


1233  //


1234  ////////////////////////////////////////////////////////////////////////////


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


1236  bool decoded = false;


1237  uint64_t numbits = 0, bitfield = 0;


1238  int i;


1239 


1240  data += 3; /* header */


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


1242  GETBITS(i, 12)


1243 


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


1245  t_ephGal eph;


1246 


1247  eph._receptDateTime = currentDateAndTimeGPS();


1248 


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


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


1251  GETBITS(i, 6)


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


1253 


1254  GETBITS(eph._TOEweek, 12)


1255  GETBITS(eph._IODnav, 10)


1256  GETBITS(i, 8)


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


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


1259  GETBITSFACTOR(i, 14, 60)


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


1261  GETFLOATSIGN(eph._clock_driftrate, 6,


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


1263  GETFLOATSIGN(eph._clock_drift, 21,


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


1265  GETFLOATSIGN(eph._clock_bias, 31,


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


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


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


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


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


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


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


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


1274  GETBITSFACTOR(eph._TOEsec, 14, 60)


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


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


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


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


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


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


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


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


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


1284  GETFLOATSIGN(eph._BGD_1_5A, 10,


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


1286  if (eph._inav) {


1287  /* set unused F/NAV values */


1288  eph._E5aHS = 0.0;


1289  eph._e5aDataInValid = false;


1290 


1291  GETFLOATSIGN(eph._BGD_1_5B, 10,


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


1293  GETBITS(eph._E5bHS, 2)


1294  GETBITS(eph._e5bDataInValid, 1)


1295  GETBITS(eph._E1_bHS, 2)


1296  GETBITS(eph._e1DataInValid, 1)


1297  }


1298  else {


1299  /* set unused I/NAV values */


1300  eph._BGD_1_5B = 0.0;


1301  eph._E5bHS = 0.0;


1302  eph._E1_bHS = 0.0;


1303  eph._e1DataInValid = false;


1304  eph._e5bDataInValid = false;


1305 


1306  GETBITS(eph._E5aHS, 2)


1307  GETBITS(eph._e5aDataInValid, 1)


1308  }


1309  eph._TOT = 0.9999e9;


1310 


1311  emit newGalileoEph(eph);


1312  decoded = true;


1313  }


1314  return decoded;


1315  }


1316 


1317  //


1318  ////////////////////////////////////////////////////////////////////////////


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


1320  bool decoded = false;


1321 


1322  if (size == 70) {


1323  t_ephBDS eph;


1324  int i;


1325  uint64_t numbits = 0, bitfield = 0;


1326 


1327  data += 3; /* header */


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


1329  SKIPBITS(12)


1330 


1331  eph._receptDateTime = currentDateAndTimeGPS();


1332 


1333  GETBITS(i, 6)


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


1335 


1336  SKIPBITS(13)


1337  /* week */


1338  GETBITS(i, 4)


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


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


1341  GETBITS(eph._AODE, 5)


1342  GETBITS(i, 17)


1343  i <<= 3;


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


1345  GETFLOATSIGN(eph._clock_driftrate, 11,


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


1347  GETFLOATSIGN(eph._clock_drift, 22,


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


1349  GETFLOATSIGN(eph._clock_bias, 24,


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


1351  GETBITS(eph._AODC, 5)


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


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


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


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


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


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


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


1359  GETBITS(i, 17)


1360  i <<= 3;


1361  eph._TOEsec = i;


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


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


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


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


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


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


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


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


1370  GETFLOATSIGN(eph._TGD1, 10, 0.0000000001)


1371  GETFLOATSIGN(eph._TGD2, 10, 0.0000000001)


1372  GETBITS(eph._SatH1, 1)


1373 


1374  eph._TOW = 0.9999E9;


1375  emit newBDSEph(eph);


1376  decoded = true;


1377  }


1378  return decoded;


1379  }


1380 


1381  //


1382  ////////////////////////////////////////////////////////////////////////////


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


1384  char *antenna, anttype[256];


1385  char *dummy;


1386  char *receiver, rectype[256];


1387 


1388  int type;


1389 


1390  int dnum = 1;


1391  int antnum = 1;


1392  int recnum = 1;


1393  uint64_t numbits = 0, bitfield = 0;


1394 


1395  data += 3; /* header*/


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


1397 


1398  GETBITS(type, 12)


1399  SKIPBITS(12)


1400  GETSTRING(antnum, antenna)


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


1402  memcpy(anttype, antenna, antnum);


1403  anttype[antnum] = 0;


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


1405  _antType.push_back(anttype);


1406  }


1407  }


1408  if (type == 1033) {


1409  SKIPBITS(8)


1410  GETSTRING(dnum, dummy)


1411  GETSTRING(recnum, receiver)


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


1413  memcpy(rectype, receiver, recnum);


1414  rectype[recnum] = 0;


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


1416  _recType.push_back(rectype);


1417  }


1418  }


1419  }


1420  return true;


1421  }


1422 


1423  //


1424  ////////////////////////////////////////////////////////////////////////////


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


1426  int type;


1427  uint64_t numbits = 0, bitfield = 0;


1428  double x, y, z;


1429 


1430  data += 3; /* header */


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


1432 


1433  GETBITS(type, 12)


1434  _antList.push_back(t_antInfo());


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


1436  SKIPBITS(22)


1437  GETBITSSIGN(x, 38)


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


1439  SKIPBITS(2)


1440  GETBITSSIGN(y, 38)


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


1442  SKIPBITS(2)


1443  GETBITSSIGN(z, 38)


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


1445  if (type == 1006)


1446  {


1447  double h;


1448  GETBITS(h, 16)


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


1450  _antList.back().height_f = true;


1451  }


1452  _antList.back().message = type;


1453 


1454  return true;


1455  }


1456 


1457  //


1458  ////////////////////////////////////////////////////////////////////////////


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


1460  bool decoded = false;


1461 


1462  errmsg.clear();


1463 


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


1465  int l = sizeof(_Message)  _MessageSize;


1466  if (l > bufLen)


1467  l = bufLen;


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


1469  _MessageSize += l;


1470  bufLen = l;


1471  buffer += l;


1472  int id;


1473  while ((id = GetMessage())) {


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


1475  if (_rawFile)


1476  _staID = _rawFile>staID();


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


1478  _typeList.push_back(id);


1479 


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


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


1482  * else. */


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


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


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


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


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


1488  errmsg) == success) {


1489  decoded = true;


1490  }


1491  }


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


1493  if (DecodeRTCM3MSM(_Message, _BlockSize))


1494  decoded = true;


1495  }


1496  else {


1497  switch (id) {


1498  case 1001:


1499  case 1003:


1500  emit(newMessage(


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


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


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


1504  case 1002:


1505  case 1004:


1506  if (DecodeRTCM3GPS(_Message, _BlockSize))


1507  decoded = true;


1508  break;


1509  case 1009:


1510  case 1011:


1511  emit(newMessage(


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


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


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


1515  case 1010:


1516  case 1012:


1517  if (DecodeRTCM3GLONASS(_Message, _BlockSize))


1518  decoded = true;


1519  break;


1520  case 1019:


1521  if (DecodeGPSEphemeris(_Message, _BlockSize))


1522  decoded = true;


1523  break;


1524  case 1020:


1525  if (DecodeGLONASSEphemeris(_Message, _BlockSize))


1526  decoded = true;


1527  break;


1528  case 1043:


1529  if (DecodeSBASEphemeris(_Message, _BlockSize))


1530  decoded = true;


1531  break;


1532  case 1044:


1533  if (DecodeQZSSEphemeris(_Message, _BlockSize))


1534  decoded = true;


1535  break;


1536  case 1045:


1537  case 1046:


1538  if (DecodeGalileoEphemeris(_Message, _BlockSize))


1539  decoded = true;


1540  break;


1541  case RTCM3ID_BDS:


1542  if (DecodeBDSEphemeris(_Message, _BlockSize))


1543  decoded = true;


1544  break;


1545  case 1007:


1546  case 1008:


1547  case 1033:


1548  DecodeAntennaReceiver(_Message, _BlockSize);


1549  break;


1550  case 1005:


1551  case 1006:


1552  DecodeAntennaPosition(_Message, _BlockSize);


1553  break;


1554  }


1555  }


1556  }


1557  }


1558  return decoded ? success : failure;


1559  }


1560  ;


1561 


1562  //


1563  ////////////////////////////////////////////////////////////////////////////


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


1565  uint32_t crc = 0;


1566  int i;


1567 


1568  while (size) {


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


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


1571  {


1572  crc <<= 1;


1573  if (crc & 0x1000000)


1574  crc ^= 0x01864cfb;


1575  }


1576  }


1577  return crc;


1578  }


1579 


1580  //


1581  ////////////////////////////////////////////////////////////////////////////


1582  int RTCM3Decoder::GetMessage(void) {


1583  unsigned char *m, *e;


1584  int i;


1585 


1586  m = _Message + _SkipBytes;


1587  e = _Message + _MessageSize;


1588  _NeedBytes = _SkipBytes = 0;


1589  while (e  m >= 3) {


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


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


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


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


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


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


1596  _BlockSize += 6;


1597  _SkipBytes = _BlockSize;


1598  break;


1599  }


1600  else


1601  ++m;


1602  }


1603  else {


1604  _NeedBytes = _BlockSize;


1605  break;


1606  }


1607  }


1608  else


1609  ++m;


1610  }


1611  if (e  m < 3)


1612  _NeedBytes = 3;


1613 


1614  /* copy buffer to front */


1615  i = m  _Message;


1616  if (i && m < e)


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


1618  _MessageSize = i;


1619 


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


1621  }


1622 


1623  // Time of Corrections


1624  //////////////////////////////////////////////////////////////////////////////


1625  int RTCM3Decoder::corrGPSEpochTime() const {


1626  return


1627  _coDecoders.size() > 0 ?


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


1629  }

