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  GLOFreq[sv  1] = 100 + i  7; /* store frequency for other users (MSM) */


1049  _gloFrq = QString("%1 %2").arg(eph._prn.toString().c_str()).arg(


1050  eph._frequency_number, 2, 'f', 0);


1051  SKIPBITS(4)


1052  /* almanac healthy, almanac health ok, P1 */


1053  GETBITS(i, 5)


1054  tk = i * 60 * 60;


1055  GETBITS(i, 6)


1056  tk += i * 60;


1057  GETBITS(i, 1)


1058  tk += i * 30;


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


1060  GETBITS(eph._health, 1)


1061  SKIPBITS(1)


1062  /* P2 */


1063  GETBITS(i, 7)


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


1065 


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


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


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


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


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


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


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


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


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


1075  SKIPBITS(1)


1076  /* P3 */


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


1078  SKIPBITS(3)


1079  /* GLONASSM P, GLONASSM ln (third string) */


1080  GETFLOATSIGNM(eph._tau, 22, 1.0 / (double )(1 << 30))


1081  /* GLONASS tau n(tb) */


1082  SKIPBITS(5)


1083  /* GLONASSM delta tau n(tb) */


1084  GETBITS(eph._E, 5)


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


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


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


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


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


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


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


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


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


1094  /* GETBITS(i, 1) / * GLONASSM ln (fifth string) */


1095 


1096  unsigned year, month, day;


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


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


1099  eph._tt = eph._TOC;


1100 


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


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


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


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


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


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


1107 


1108  emit newGlonassEph(eph);


1109  decoded = true;


1110  }


1111  return decoded;


1112  }


1113 


1114  //


1115  ////////////////////////////////////////////////////////////////////////////


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


1117  bool decoded = false;


1118 


1119  if (size == 67) {


1120  t_ephGPS eph;


1121  int i, week;


1122  uint64_t numbits = 0, bitfield = 0;


1123 


1124  data += 3; /* header */


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


1126  SKIPBITS(12)


1127 


1128  eph._receptDateTime = currentDateAndTimeGPS();


1129 


1130  GETBITS(i, 4)


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


1132 


1133  GETBITS(i, 16)


1134  i <<= 4;


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


1136 


1137  GETFLOATSIGN(eph._clock_driftrate, 8,


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


1139  GETFLOATSIGN(eph._clock_drift, 16,


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


1141  GETFLOATSIGN(eph._clock_bias, 22,


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


1143  GETBITS(eph._IODE, 8)


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


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


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


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


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


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


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


1151  GETBITS(i, 16)


1152  i <<= 4;


1153  eph._TOEsec = i;


1154  bncTime t;


1155  t.set(i);


1156 


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


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


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


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


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


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


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


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


1165  GETBITS(eph._L2Codes, 2)


1166  GETBITS(week, 10)


1167  week += 1024;


1168  eph._TOEweek = t.gpsw();


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


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


1171  return false;


1172 


1173  GETBITS(i, 4)


1174  if (i <= 6)


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


1176  else


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


1178  GETBITS(eph._health, 6)


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


1180  GETBITS(eph._IODC, 10)


1181  GETBITS(eph._fitInterval, 1)


1182  eph._TOT = 0.9999e9;


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


1184 


1185  emit newGPSEph(eph);


1186  decoded = true;


1187  }


1188  return decoded;


1189  }


1190 


1191  //


1192  ////////////////////////////////////////////////////////////////////////////


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


1194  bool decoded = false;


1195 


1196  if (size == 35) {


1197  t_ephSBAS eph;


1198  int i;


1199  uint64_t numbits = 0, bitfield = 0;


1200 


1201  data += 3; /* header */


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


1203  SKIPBITS(12)


1204 


1205  eph._receptDateTime = currentDateAndTimeGPS();


1206 


1207  GETBITS(i, 6)


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


1209  GETBITS(eph._IODN, 8)


1210  GETBITS(i, 13)


1211  i <<= 4;


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


1213  GETBITS(i, 4)


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


1215  GETFLOATSIGN(eph._x_pos, 30, 0.08)


1216  GETFLOATSIGN(eph._y_pos, 30, 0.08)


1217  GETFLOATSIGN(eph._z_pos, 25, 0.4)


1218  GETFLOATSIGN(eph._x_velocity, 17, 0.000625)


1219  GETFLOATSIGN(eph._y_velocity, 17, 0.000625)


1220  GETFLOATSIGN(eph._z_velocity, 18, 0.004)


1221  GETFLOATSIGN(eph._x_acceleration, 10, 0.0000125)


1222  GETFLOATSIGN(eph._y_acceleration, 10, 0.0000125)


1223  GETFLOATSIGN(eph._z_acceleration, 10, 0.0000625)


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


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


1226 


1227  eph._TOW = 0.9999E9;


1228  eph._health = 0;


1229 


1230  emit newSBASEph(eph);


1231  decoded = true;


1232  }


1233  return decoded;


1234  }


1235 


1236  //


1237  ////////////////////////////////////////////////////////////////////////////


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


1239  bool decoded = false;


1240  uint64_t numbits = 0, bitfield = 0;


1241  int i;


1242 


1243  data += 3; /* header */


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


1245  GETBITS(i, 12)


1246 


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


1248  t_ephGal eph;


1249 


1250  eph._receptDateTime = currentDateAndTimeGPS();


1251 


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


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


1254  GETBITS(i, 6)


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


1256 


1257  GETBITS(eph._TOEweek, 12)


1258  GETBITS(eph._IODnav, 10)


1259  GETBITS(i, 8)


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


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


1262  GETBITSFACTOR(i, 14, 60)


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


1264  GETFLOATSIGN(eph._clock_driftrate, 6,


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


1266  GETFLOATSIGN(eph._clock_drift, 21,


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


1268  GETFLOATSIGN(eph._clock_bias, 31,


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


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


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


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


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


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


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


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


1277  GETBITSFACTOR(eph._TOEsec, 14, 60)


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


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


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


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


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


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


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


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


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


1287  GETFLOATSIGN(eph._BGD_1_5A, 10,


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


1289  if (eph._inav) {


1290  /* set unused F/NAV values */


1291  eph._E5aHS = 0.0;


1292  eph._e5aDataInValid = false;


1293 


1294  GETFLOATSIGN(eph._BGD_1_5B, 10,


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


1296  GETBITS(eph._E5bHS, 2)


1297  GETBITS(eph._e5bDataInValid, 1)


1298  GETBITS(eph._E1_bHS, 2)


1299  GETBITS(eph._e1DataInValid, 1)


1300  }


1301  else {


1302  /* set unused I/NAV values */


1303  eph._BGD_1_5B = 0.0;


1304  eph._E5bHS = 0.0;


1305  eph._E1_bHS = 0.0;


1306  eph._e1DataInValid = false;


1307  eph._e5bDataInValid = false;


1308 


1309  GETBITS(eph._E5aHS, 2)


1310  GETBITS(eph._e5aDataInValid, 1)


1311  }


1312  eph._TOT = 0.9999e9;


1313 


1314  emit newGalileoEph(eph);


1315  decoded = true;


1316  }


1317  return decoded;


1318  }


1319 


1320  //


1321  ////////////////////////////////////////////////////////////////////////////


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


1323  bool decoded = false;


1324 


1325  if (size == 70) {


1326  t_ephBDS eph;


1327  int i;


1328  uint64_t numbits = 0, bitfield = 0;


1329 


1330  data += 3; /* header */


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


1332  SKIPBITS(12)


1333 


1334  eph._receptDateTime = currentDateAndTimeGPS();


1335 


1336  GETBITS(i, 6)


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


1338 


1339  SKIPBITS(13)


1340  /* week */


1341  GETBITS(i, 4)


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


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


1344  GETBITS(eph._AODE, 5)


1345  GETBITS(i, 17)


1346  i <<= 3;


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


1348  GETFLOATSIGN(eph._clock_driftrate, 11,


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


1350  GETFLOATSIGN(eph._clock_drift, 22,


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


1352  GETFLOATSIGN(eph._clock_bias, 24,


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


1354  GETBITS(eph._AODC, 5)


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


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


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


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


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


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


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


1362  GETBITS(i, 17)


1363  i <<= 3;


1364  eph._TOEsec = i;


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


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


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


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


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


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


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


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


1373  GETFLOATSIGN(eph._TGD1, 10, 0.0000000001)


1374  GETFLOATSIGN(eph._TGD2, 10, 0.0000000001)


1375  GETBITS(eph._SatH1, 1)


1376 


1377  eph._TOW = 0.9999E9;


1378  emit newBDSEph(eph);


1379  decoded = true;


1380  }


1381  return decoded;


1382  }


1383 


1384  //


1385  ////////////////////////////////////////////////////////////////////////////


1386  bool RTCM3Decoder::DecodeAntenna(unsigned char* data, int size) {


1387  char *antenna, type[256];


1388  int antnum = 1;


1389  uint64_t numbits = 0, bitfield = 0;


1390 


1391  data += 4; /* header */


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


1393 


1394  SKIPBITS(12)


1395  GETSTRING(antnum, antenna)


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


1397  memcpy(type, antenna, antnum);


1398  type[antnum] = 0;


1399  if (!_antType.contains(type)) {


1400  _antType.push_back(type);


1401  }


1402  }


1403  return true;


1404  }


1405 


1406  //


1407  ////////////////////////////////////////////////////////////////////////////


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


1409  int type;


1410  uint64_t numbits = 0, bitfield = 0;


1411  double x, y, z;


1412 


1413  data += 3; /* header */


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


1415 


1416  GETBITS(type, 12)


1417  _antList.push_back(t_antInfo());


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


1419  SKIPBITS(22)


1420  GETBITSSIGN(x, 38)


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


1422  SKIPBITS(2)


1423  GETBITSSIGN(y, 38)


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


1425  SKIPBITS(2)


1426  GETBITSSIGN(z, 38)


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


1428  if (type == 1006)


1429  {


1430  double h;


1431  GETBITS(h, 16)


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


1433  _antList.back().height_f = true;


1434  }


1435  _antList.back().message = type;


1436 


1437  return true;


1438  }


1439 


1440  //


1441  ////////////////////////////////////////////////////////////////////////////


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


1443  bool decoded = false;


1444 


1445  errmsg.clear();


1446 


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


1448  int l = sizeof(_Message)  _MessageSize;


1449  if (l > bufLen)


1450  l = bufLen;


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


1452  _MessageSize += l;


1453  bufLen = l;


1454  buffer += l;


1455  int id;


1456  while ((id = GetMessage())) {


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


1458  if (_rawFile)


1459  _staID = _rawFile>staID();


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


1461  _typeList.push_back(id);


1462 


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


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


1465  * else. */


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


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


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


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


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


1471  errmsg) == success) {


1472  decoded = true;


1473  }


1474  }


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


1476  if (DecodeRTCM3MSM(_Message, _BlockSize))


1477  decoded = true;


1478  }


1479  else {


1480  switch (id) {


1481  case 1001:


1482  case 1003:


1483  emit(newMessage(


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


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


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


1487  case 1002:


1488  case 1004:


1489  if (DecodeRTCM3GPS(_Message, _BlockSize))


1490  decoded = true;


1491  break;


1492  case 1009:


1493  case 1011:


1494  emit(newMessage(


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


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


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


1498  case 1010:


1499  case 1012:


1500  if (DecodeRTCM3GLONASS(_Message, _BlockSize))


1501  decoded = true;


1502  break;


1503  case 1019:


1504  if (DecodeGPSEphemeris(_Message, _BlockSize))


1505  decoded = true;


1506  break;


1507  case 1020:


1508  if (DecodeGLONASSEphemeris(_Message, _BlockSize))


1509  decoded = true;


1510  break;


1511  case 1043:


1512  if (DecodeSBASEphemeris(_Message, _BlockSize))


1513  decoded = true;


1514  break;


1515  case 1044:


1516  if (DecodeQZSSEphemeris(_Message, _BlockSize))


1517  decoded = true;


1518  break;


1519  case 1045:


1520  case 1046:


1521  if (DecodeGalileoEphemeris(_Message, _BlockSize))


1522  decoded = true;


1523  break;


1524  case RTCM3ID_BDS:


1525  if (DecodeBDSEphemeris(_Message, _BlockSize))


1526  decoded = true;


1527  break;


1528  case 1007:


1529  case 1008:


1530  case 1033:


1531  DecodeAntenna(_Message, _BlockSize);


1532  break;


1533  case 1005:


1534  case 1006:


1535  DecodeAntennaPosition(_Message, _BlockSize);


1536  break;


1537  }


1538  }


1539  }


1540  }


1541  return decoded ? success : failure;


1542  }


1543  ;


1544 


1545  //


1546  ////////////////////////////////////////////////////////////////////////////


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


1548  uint32_t crc = 0;


1549  int i;


1550 


1551  while (size) {


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


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


1554  {


1555  crc <<= 1;


1556  if (crc & 0x1000000)


1557  crc ^= 0x01864cfb;


1558  }


1559  }


1560  return crc;


1561  }


1562 


1563  //


1564  ////////////////////////////////////////////////////////////////////////////


1565  int RTCM3Decoder::GetMessage(void) {


1566  unsigned char *m, *e;


1567  int i;


1568 


1569  m = _Message + _SkipBytes;


1570  e = _Message + _MessageSize;


1571  _NeedBytes = _SkipBytes = 0;


1572  while (e  m >= 3) {


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


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


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


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


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


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


1579  _BlockSize += 6;


1580  _SkipBytes = _BlockSize;


1581  break;


1582  }


1583  else


1584  ++m;


1585  }


1586  else {


1587  _NeedBytes = _BlockSize;


1588  break;


1589  }


1590  }


1591  else


1592  ++m;


1593  }


1594  if (e  m < 3)


1595  _NeedBytes = 3;


1596 


1597  /* copy buffer to front */


1598  i = m  _Message;


1599  if (i && m < e)


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


1601  _MessageSize = i;


1602 


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


1604  }


1605 


1606  // Time of Corrections


1607  //////////////////////////////////////////////////////////////////////////////


1608  int RTCM3Decoder::corrGPSEpochTime() const {


1609  return


1610  _coDecoders.size() > 0 ?


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


1612  }

