source: ntrip/trunk/BNC/src/RTCM3/RTCM3Decoder.cpp @ 7874

Last change on this file since 7874 was 7874, checked in by stuerze, 3 years ago

minor changes regarding RTCM antenna type descriptor

File size: 45.7 KB
Line 
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: euref-ip@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 02111-1307, 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:    24-Aug-2006
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
56using namespace std;
57
58// Error Handling
59////////////////////////////////////////////////////////////////////////////
60void RTCM3Error(const char*, ...) {
61}
62
63// Constructor
64////////////////////////////////////////////////////////////////////////////
65RTCM3Decoder::RTCM3Decoder(const QString& staID, bncRawFile* rawFile) :
66                GPSDecoder() {
67
68  _staID   = staID;
69  _rawFile = rawFile;
70
71  connect(this, SIGNAL(newGPSEph(t_ephGPS)),     BNC_CORE, SLOT(slotNewGPSEph(t_ephGPS)));
72  connect(this, SIGNAL(newGlonassEph(t_ephGlo)), BNC_CORE, SLOT(slotNewGlonassEph(t_ephGlo)));
73  connect(this, SIGNAL(newGalileoEph(t_ephGal)), BNC_CORE, SLOT(slotNewGalileoEph(t_ephGal)));
74  connect(this, SIGNAL(newSBASEph(t_ephSBAS)),   BNC_CORE, SLOT(slotNewSBASEph(t_ephSBAS)));
75  connect(this, SIGNAL(newBDSEph(t_ephBDS)),     BNC_CORE, SLOT(slotNewBDSEph(t_ephBDS)));
76
77  _MessageSize = _SkipBytes = _BlockSize = _NeedBytes = 0;
78}
79
80// Destructor
81////////////////////////////////////////////////////////////////////////////
82RTCM3Decoder::~RTCM3Decoder() {
83  QMapIterator<QByteArray, RTCM3coDecoder*> it(_coDecoders);
84  while(it.hasNext())
85  {
86    it.next();
87    delete it.value();
88  }
89}
90
91//
92////////////////////////////////////////////////////////////////////////////
93bool RTCM3Decoder::DecodeRTCM3GPS(unsigned char* data, int size)
94{
95  bool decoded = false;
96  bncTime CurrentObsTime;
97  int i, numsats, syncf, type;
98  uint64_t numbits = 0, bitfield = 0;
99
100  data += 3; /* header */
101  size -= 6; /* header + crc */
102
103  GETBITS(type, 12)
104  SKIPBITS(12) /* id */
105  GETBITS(i,30)
106
107  CurrentObsTime.set(i);
108  if(_CurrentTime.valid() && CurrentObsTime != _CurrentTime)
109  {
110    decoded = true;
111    _obsList.append(_CurrentObsList);
112    _CurrentObsList.clear();
113  }
114
115  _CurrentTime = CurrentObsTime;
116
117  GETBITS(syncf,1) /* sync */
118  GETBITS(numsats,5)
119  SKIPBITS(4) /* smind, smint */
120
121  while(numsats--)
122  {
123    int sv, code, l1range, amb=0;
124    t_satObs CurrentObs;
125    CurrentObs._time = CurrentObsTime;
126
127    GETBITS(sv, 6)
128    if(sv < 40)
129      CurrentObs._prn.set('G', sv);
130    else
131      CurrentObs._prn.set('S', sv-20);
132
133    t_frqObs *frqObs = new t_frqObs;
134    /* L1 */
135    GETBITS(code, 1);
136    (code) ? frqObs->_rnxType2ch.assign("1W") : frqObs->_rnxType2ch.assign("1C");
137    GETBITS(l1range, 24);
138    GETBITSSIGN(i, 20);
139    if((i&((1<<20)-1)) != 0x80000)
140    {
141      frqObs->_code = l1range*0.02;
142      frqObs->_phase = (l1range*0.02+i*0.0005)/GPS_WAVELENGTH_L1;
143      frqObs->_codeValid = frqObs->_phaseValid = true;
144    }
145    GETBITS(i, 7);
146    frqObs->_slipCounter = i;
147    if(type == 1002 || type == 1004)
148    {
149      GETBITS(amb,8);
150      if(amb)
151      {
152        frqObs->_code += amb*299792.458;
153        frqObs->_phase += (amb*299792.458)/GPS_WAVELENGTH_L1;
154      }
155      GETBITS(i, 8);
156      if(i)
157      {
158        frqObs->_snr = i*0.25;
159        frqObs->_snrValid = true;
160      }
161    }
162    CurrentObs._obs.push_back(frqObs);
163    if(type == 1003 || type == 1004)
164    {
165      frqObs = new t_frqObs;
166      /* L2 */
167      GETBITS(code,2);
168      switch(code)
169      {
170      case 3: frqObs->_rnxType2ch.assign("2W"); /* or "2Y"? */ break;
171      case 2: frqObs->_rnxType2ch.assign("2W"); break;
172      case 1: frqObs->_rnxType2ch.assign("2P"); break;
173      case 0: frqObs->_rnxType2ch.assign("2X"); /* or "2S" or "2L"? */ break;
174      }
175      GETBITSSIGN(i,14);
176      if((i&((1<<14)-1)) != 0x2000)
177      {
178        frqObs->_code = l1range*0.02+i*0.02+amb*299792.458;
179        frqObs->_codeValid = true;
180      }
181      GETBITSSIGN(i,20);
182      if((i&((1<<20)-1)) != 0x80000)
183      {
184        frqObs->_phase = (l1range*0.02+i*0.0005+amb*299792.458)/GPS_WAVELENGTH_L2;
185        frqObs->_phaseValid = true;
186      }
187      GETBITS(i,7);
188      frqObs->_slipCounter = i;
189      if(type == 1004)
190      {
191        GETBITS(i, 8);
192        if(i)
193        {
194          frqObs->_snr = i*0.25;
195          frqObs->_snrValid = true;
196        }
197      }
198      CurrentObs._obs.push_back(frqObs);
199    }
200    _CurrentObsList.push_back(CurrentObs);
201  }
202
203  if(!syncf)
204  {
205    decoded = true;
206    _obsList.append(_CurrentObsList);
207    _CurrentTime.reset();
208    _CurrentObsList.clear();
209  }
210  return decoded;
211}
212
213#define RTCM3_MSM_NUMSIG      32
214#define RTCM3_MSM_NUMSAT      64
215#define RTCM3_MSM_NUMCELLS    96 /* arbitrary limit */
216
217/**
218 * Frequency numbers of GLONASS with an offset of 100 to detect unset values.
219 * Gets filled by ephemeris and data blocks and shared between different streams.
220 */
221static int GLOFreq[RTCM3_MSM_NUMSAT];
222
223/*
224 * Storage structure to store frequency and RINEX ID assignment for MSM
225 * message */
226struct CodeData {
227  double wl;
228  const char *code; /* currently unused */
229};
230
231/** MSM signal types for GPS and SBAS */
232static struct CodeData gps[RTCM3_MSM_NUMSIG] =
233{
234  {0.0,0},
235  {GPS_WAVELENGTH_L1,"1C"},
236  {GPS_WAVELENGTH_L1,"1P"},
237  {GPS_WAVELENGTH_L1,"1W"},
238  {0.0,0}/*{GPS_WAVELENGTH_L1,"1Y"}*/,
239  {0.0,0},
240  {0.0,0},
241  {GPS_WAVELENGTH_L2,"2C"},
242  {GPS_WAVELENGTH_L2,"2P"},
243  {GPS_WAVELENGTH_L2,"2W"},
244  {0.0,0}/*{GPS_WAVELENGTH_L2,"2Y"}*/,
245  {0.0,0},
246  {0.0,0},
247  {0.0,0},
248  {GPS_WAVELENGTH_L2,"2S"},
249  {GPS_WAVELENGTH_L2,"2L"},
250  {GPS_WAVELENGTH_L2,"2X"},
251  {0.0,0},
252  {0.0,0},
253  {0.0,0},
254  {0.0,0},
255  {GPS_WAVELENGTH_L5,"5I"},
256  {GPS_WAVELENGTH_L5,"5Q"},
257  {GPS_WAVELENGTH_L5,"5X"},
258  {0.0,0},
259  {0.0,0},
260  {0.0,0},
261  {0.0,0},
262  {0.0,0},
263  {GPS_WAVELENGTH_L1,"1S"},
264  {GPS_WAVELENGTH_L1,"1L"},
265  {GPS_WAVELENGTH_L1,"1X"}
266};
267
268/**
269 * MSM signal types for GLONASS
270 *
271 * NOTE: Uses 0.0, 1.0 for wavelength as sat index dependence is done later!
272 */
273static struct CodeData glo[RTCM3_MSM_NUMSIG] =
274{
275  {0.0,0},
276  {0.0,"1C"},
277  {0.0,"1P"},
278  {0.0,0},
279  {0.0,0},
280  {0.0,0},
281  {0.0,0},
282  {1.0,"2C"},
283  {1.0,"2P"},
284  {0.0,0},
285  {0.0,0},
286  {0.0,0},
287  {0.0,0},
288  {0.0,0},
289  {0.0,0},
290  {0.0,0},
291  {0.0,0},
292  {0.0,0},
293  {0.0,0},
294  {0.0,0},
295  {0.0,0},
296  {0.0,0},
297  {0.0,0},
298  {0.0,0},
299  {0.0,0},
300  {0.0,0},
301  {0.0,0},
302  {0.0,0},
303  {0.0,0},
304  {0.0,0},
305  {0.0,0},
306  {0.0,0}
307};
308
309/** MSM signal types for Galileo */
310static struct CodeData gal[RTCM3_MSM_NUMSIG] =
311{
312  {0.0,0},
313  {GAL_WAVELENGTH_E1,"1C"},
314  {GAL_WAVELENGTH_E1,"1A"},
315  {GAL_WAVELENGTH_E1,"1B"},
316  {GAL_WAVELENGTH_E1,"1X"},
317  {GAL_WAVELENGTH_E1,"1Z"},
318  {0.0,0},
319  {GAL_WAVELENGTH_E6,"6C"},
320  {GAL_WAVELENGTH_E6,"6A"},
321  {GAL_WAVELENGTH_E6,"6B"},
322  {GAL_WAVELENGTH_E6,"6X"},
323  {GAL_WAVELENGTH_E6,"6Z"},
324  {0.0,0},
325  {GAL_WAVELENGTH_E5B,"7I"},
326  {GAL_WAVELENGTH_E5B,"7Q"},
327  {GAL_WAVELENGTH_E5B,"7X"},
328  {0.0,0},
329  {GAL_WAVELENGTH_E5AB,"8I"},
330  {GAL_WAVELENGTH_E5AB,"8Q"},
331  {GAL_WAVELENGTH_E5AB,"8X"},
332  {0.0,0},
333  {GAL_WAVELENGTH_E5A,"5I"},
334  {GAL_WAVELENGTH_E5A,"5Q"},
335  {GAL_WAVELENGTH_E5A,"5X"},
336  {0.0,0},
337  {0.0,0},
338  {0.0,0},
339  {0.0,0},
340  {0.0,0},
341  {0.0,0},
342  {0.0,0},
343  {0.0,0},
344};
345
346/** MSM signal types for QZSS */
347static struct CodeData qzss[RTCM3_MSM_NUMSIG] =
348{
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 */
384static struct CodeData bds[RTCM3_MSM_NUMSIG] =
385{
386  {0.0,0},
387  {BDS_WAVELENGTH_B1,"2I"},
388  {0.0,0},
389  {0.0,0},
390  {0.0,0},
391  {0.0,0},
392  {0.0,0},
393  {BDS_WAVELENGTH_B3,"6I"},
394  {0.0,0},
395  {0.0,0},
396  {0.0,0},
397  {0.0,0},
398  {0.0,0},
399  {BDS_WAVELENGTH_B2,"7I"},
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  {0.0,0},
418};
419
420#define UINT64(c) c ## ULL
421
422//
423////////////////////////////////////////////////////////////////////////////
424bool RTCM3Decoder::DecodeRTCM3MSM(unsigned char* data, int size)
425{
426  bool decoded = false;
427  int type, syncf, i;
428  uint64_t numbits = 0, bitfield = 0;
429
430  data += 3; /* header */
431  size -= 6; /* header + crc */
432
433  GETBITS(type, 12)
434  SKIPBITS(12) /* 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  {
452    GETBITS(i,30)
453    CurrentObsTime.setBDS(i);
454  }
455  else if(sys == 'R') /* GLONASS */
456  {
457    SKIPBITS(3)
458    GETBITS(i,27) /* tk */
459    CurrentObsTime.setTk(i);
460  }
461  else /* GPS style date */
462  {
463    GETBITS(i,30)
464    CurrentObsTime.set(i);
465  }
466  if(_CurrentTime.valid() && CurrentObsTime != _CurrentTime)
467  {
468    decoded = true;
469    _obsList = _CurrentObsList;
470    _CurrentObsList.clear();
471  }
472  _CurrentTime = CurrentObsTime;
473
474  GETBITS(syncf, 1)
475  /**
476   * Ignore unknown types except for sync flag
477   *
478   * We actually support types 1-3 in following code, but as they are missing
479   * the full cycles and can't be used later we skip interpretation here already.
480   */
481  if(type <= 1130 && (type % 10) >= 4 && (type % 10) <= 7)
482  {
483    int sigmask, numsat = 0, numsig = 0;
484    uint64_t satmask, cellmask, ui;
485    double rrmod[RTCM3_MSM_NUMSAT];
486    int rrint[RTCM3_MSM_NUMSAT], rdop[RTCM3_MSM_NUMSAT],
487    extsat[RTCM3_MSM_NUMSAT];
488    int ll[RTCM3_MSM_NUMCELLS]/*, hc[RTCM3_MSM_NUMCELLS]*/;
489    double cnr[RTCM3_MSM_NUMCELLS];
490    double cp[RTCM3_MSM_NUMCELLS], psr[RTCM3_MSM_NUMCELLS],
491    dop[RTCM3_MSM_NUMCELLS];
492
493    SKIPBITS(3+7+2+2+1+3)
494    GETBITS64(satmask, RTCM3_MSM_NUMSAT)
495
496    /* http://gurmeetsingh.wordpress.com/2008/08/05/fast-bit-counting-routines/ */
497    for(ui = satmask; ui; ui &= (ui - 1) /* remove rightmost bit */)
498      ++numsat;
499    GETBITS(sigmask, RTCM3_MSM_NUMSIG)
500    for(i = sigmask; i; i &= (i - 1) /* remove rightmost bit */)
501      ++numsig;
502    for(i = 0; i < RTCM3_MSM_NUMSAT; ++i)
503      extsat[i] = 15;
504
505    i = numsat*numsig;
506    GETBITS64(cellmask, (unsigned)i)
507
508    switch(type % 10)
509    {
510    case 1: case 2: case 3:
511      /* partial data, already skipped above, but implemented for future expansion ! */
512      for(int j = numsat; j--;)
513        GETFLOAT(rrmod[j], 10, 1.0/1024.0)
514      break;
515    case 4: case 6:
516      for(int j = numsat; j--;)
517        GETBITS(rrint[j], 8)
518      for(int j = numsat; j--;)
519        GETFLOAT(rrmod[j], 10, 1.0/1024.0)
520      break;
521    case 5: case 7:
522      for(int j = numsat; j--;)
523        GETBITS(rrint[j], 8)
524      for(int j = numsat; j--;)
525        GETBITS(extsat[j], 4)
526      for(int j = numsat; j--;)
527        GETFLOAT(rrmod[j], 10, 1.0/1024.0)
528      for(int j = numsat; j--;)
529        GETBITSSIGN(rdop[j], 14)
530      break;
531    }
532
533    int numcells = numsat*numsig;
534    /** Drop anything which exceeds our cell limit. Increase limit definition
535     * when that happens. */
536    if(numcells <= RTCM3_MSM_NUMCELLS)
537    {
538      switch(type % 10)
539      {
540      case 1:
541        for(int count = numcells; count--;)
542          if(cellmask & (UINT64(1)<<count))
543            GETFLOATSIGN(psr[count], 15, 1.0/(1<<24))
544        break;
545      case 2:
546        for(int count = numcells; count--;)
547          if(cellmask & (UINT64(1)<<count))
548            GETFLOATSIGN(cp[count], 22, 1.0/(1<<29))
549        for(int count = numcells; count--;)
550          if(cellmask & (UINT64(1)<<count))
551            GETBITS(ll[count], 4)
552        for(int count = numcells; count--;)
553          if(cellmask & (UINT64(1)<<count))
554            SKIPBITS(1)/*GETBITS(hc[count], 1)*/
555        break;
556      case 3:
557        for(int count = numcells; count--;)
558          if(cellmask & (UINT64(1)<<count))
559            GETFLOATSIGN(psr[count], 15, 1.0/(1<<24))
560        for(int count = numcells; count--;)
561          if(cellmask & (UINT64(1)<<count))
562            GETFLOATSIGN(cp[count], 22, 1.0/(1<<29))
563        for(int count = numcells; count--;)
564          if(cellmask & (UINT64(1)<<count))
565            GETBITS(ll[count], 4)
566        for(int count = numcells; count--;)
567          if(cellmask & (UINT64(1)<<count))
568            SKIPBITS(1)/*GETBITS(hc[count], 1)*/
569        break;
570      case 4:
571        for(int count = numcells; count--;)
572          if(cellmask & (UINT64(1)<<count))
573            GETFLOATSIGN(psr[count], 15, 1.0/(1<<24))
574        for(int count = numcells; count--;)
575          if(cellmask & (UINT64(1)<<count))
576            GETFLOATSIGN(cp[count], 22, 1.0/(1<<29))
577        for(int count = numcells; count--;)
578          if(cellmask & (UINT64(1)<<count))
579            GETBITS(ll[count], 4)
580        for(int count = numcells; count--;)
581          if(cellmask & (UINT64(1)<<count))
582            SKIPBITS(1)/*GETBITS(hc[count], 1)*/
583        for(int count = numcells; count--;)
584          if(cellmask & (UINT64(1)<<count))
585            GETBITS(cnr[count], 6)
586        break;
587      case 5:
588        for(int count = numcells; count--;)
589          if(cellmask & (UINT64(1)<<count))
590            GETFLOATSIGN(psr[count], 15, 1.0/(1<<24))
591        for(int count = numcells; count--;)
592          if(cellmask & (UINT64(1)<<count))
593            GETFLOATSIGN(cp[count], 22, 1.0/(1<<29))
594        for(int count = numcells; count--;)
595          if(cellmask & (UINT64(1)<<count))
596            GETBITS(ll[count], 4)
597        for(int count = numcells; count--;)
598          if(cellmask & (UINT64(1)<<count))
599            SKIPBITS(1)/*GETBITS(hc[count], 1)*/
600        for(int count = numcells; count--;)
601          if(cellmask & (UINT64(1)<<count))
602            GETFLOAT(cnr[count], 6, 1.0)
603        for(int count = numcells; count--;)
604          if(cellmask & (UINT64(1)<<count))
605            GETFLOATSIGN(dop[count], 15, 0.0001)
606        break;
607      case 6:
608        for(int count = numcells; count--;)
609          if(cellmask & (UINT64(1)<<count))
610            GETFLOATSIGN(psr[count], 20, 1.0/(1<<29))
611        for(int count = numcells; count--;)
612          if(cellmask & (UINT64(1)<<count))
613            GETFLOATSIGN(cp[count], 24, 1.0/(1U<<31))
614        for(int count = numcells; count--;)
615          if(cellmask & (UINT64(1)<<count))
616            GETBITS(ll[count], 10)
617        for(int count = numcells; count--;)
618          if(cellmask & (UINT64(1)<<count))
619            SKIPBITS(1)/*GETBITS(hc[count], 1)*/
620        for(int count = numcells; count--;)
621          if(cellmask & (UINT64(1)<<count))
622            GETFLOAT(cnr[count], 10, 1.0/(1<<4))
623        break;
624      case 7:
625        for(int count = numcells; count--;)
626          if(cellmask & (UINT64(1)<<count))
627            GETFLOATSIGN(psr[count], 20, 1.0/(1<<29))
628        for(int count = numcells; count--;)
629          if(cellmask & (UINT64(1)<<count))
630            GETFLOATSIGN(cp[count], 24, 1.0/(1U<<31))
631        for(int count = numcells; count--;)
632          if(cellmask & (UINT64(1)<<count))
633            GETBITS(ll[count], 10)
634        for(int count = numcells; count--;)
635          if(cellmask & (UINT64(1)<<count))
636            SKIPBITS(1)/*GETBITS(hc[count], 1)*/
637        for(int count = numcells; count--;)
638          if(cellmask & (UINT64(1)<<count))
639            GETFLOAT(cnr[count], 10, 1.0/(1<<4))
640        for(int count = numcells; count--;)
641          if(cellmask & (UINT64(1)<<count))
642            GETFLOATSIGN(dop[count], 15, 0.0001)
643        break;
644      }
645      i = RTCM3_MSM_NUMSAT;
646      int j = -1;
647      t_satObs CurrentObs;
648      for(int count = numcells; count--;)
649      {
650        while(j >= 0 && !(sigmask&(1<<--j)))
651          ;
652        if(j < 0)
653        {
654          while(!(satmask&(UINT64(1)<<(--i)))) /* next satellite */
655            ;
656          if(CurrentObs._obs.size() > 0)
657            _CurrentObsList.push_back(CurrentObs);
658          CurrentObs.clear();
659          CurrentObs._time = CurrentObsTime;
660          if(sys == 'S')
661            CurrentObs._prn.set(sys, 20-1+RTCM3_MSM_NUMSAT-i);
662          else
663            CurrentObs._prn.set(sys, RTCM3_MSM_NUMSAT-i);
664          j = RTCM3_MSM_NUMSIG;
665          while(!(sigmask&(1<<--j)))
666            ;
667          --numsat;
668        }
669        if(cellmask & (UINT64(1)<<count))
670        {
671          struct CodeData cd = {0.0,0};
672          switch(sys)
673          {
674          case 'J':
675            cd = qzss[RTCM3_MSM_NUMSIG-j-1];
676            break;
677          case 'C':
678            cd = bds[RTCM3_MSM_NUMSIG-j-1];
679            break;
680          case 'G': case 'S':
681            cd = gps[RTCM3_MSM_NUMSIG-j-1];
682            break;
683          case 'R':
684            cd = glo[RTCM3_MSM_NUMSIG-j-1];
685            {
686              int k = GLOFreq[RTCM3_MSM_NUMSAT-i-1];
687              if(extsat[numsat] < 14)
688              {
689                k = GLOFreq[RTCM3_MSM_NUMSAT-i-1] = 100+extsat[numsat]-7;
690              }
691              if(k)
692                cd.wl = (cd.wl == 0.0 ? GLO_WAVELENGTH_L1(k-100) : GLO_WAVELENGTH_L2(k-100));
693              else
694                cd.code = 0;
695            }
696            break;
697          case 'E':
698            cd = gal[RTCM3_MSM_NUMSIG-j-1];
699            break;
700          }
701          if(cd.code)
702          {
703            t_frqObs *frqObs = new t_frqObs;
704            frqObs->_rnxType2ch.assign(cd.code);
705
706            switch(type % 10)
707            {
708            case 1:
709              if(psr[count] > -1.0/(1<<10))
710              {
711                frqObs->_code = psr[count]*LIGHTSPEED/1000.0
712                +(rrmod[numsat])*LIGHTSPEED/1000.0;
713                frqObs->_codeValid = true;
714              }
715              break;
716            case 2:
717              if(cp[count] > -1.0/(1<<8))
718              {
719                frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl
720                +(rrmod[numsat])*LIGHTSPEED/1000.0/cd.wl;
721                frqObs->_phaseValid = true;
722                frqObs->_slipCounter = ll[count];
723              }
724              break;
725            case 3:
726              if(psr[count] > -1.0/(1<<10))
727              {
728                frqObs->_code = psr[count]*LIGHTSPEED/1000.0
729                +(rrmod[numsat])*LIGHTSPEED/1000.0;
730                frqObs->_codeValid = true;
731              }
732
733              if(cp[count] > -1.0/(1<<8))
734              {
735                frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl
736                +rrmod[numsat]*LIGHTSPEED/1000.0/cd.wl;
737                frqObs->_phaseValid = true;
738                frqObs->_slipCounter = ll[count];
739              }
740              break;
741            case 4:
742              if(psr[count] > -1.0/(1<<10))
743              {
744                frqObs->_code = psr[count]*LIGHTSPEED/1000.0
745                +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0;
746                frqObs->_codeValid = true;
747              }
748
749              if(cp[count] > -1.0/(1<<8))
750              {
751                frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl
752                +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0/cd.wl;
753                frqObs->_phaseValid = true;
754                frqObs->_slipCounter = ll[count];
755              }
756
757              frqObs->_snr = cnr[count];
758              frqObs->_snrValid = true;
759              break;
760            case 5:
761              if(psr[count] > -1.0/(1<<10))
762              {
763                frqObs->_code = psr[count]*LIGHTSPEED/1000.0
764                +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0;
765                frqObs->_codeValid = true;
766              }
767
768              if(cp[count] > -1.0/(1<<8))
769              {
770                frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl
771                +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0/cd.wl;
772                frqObs->_phaseValid = true;
773                frqObs->_slipCounter = ll[count];
774              }
775
776              frqObs->_snr = cnr[count];
777              frqObs->_snrValid = true;
778
779              if(dop[count] > -1.6384)
780              {
781                frqObs->_doppler = -(dop[count]+rdop[numsat])/cd.wl;
782                frqObs->_dopplerValid = true;
783              }
784              break;
785            case 6:
786              if(psr[count] > -1.0/(1<<10))
787              {
788                frqObs->_code = psr[count]*LIGHTSPEED/1000.0
789                +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0;
790                frqObs->_codeValid = true;
791              }
792
793              if(cp[count] > -1.0/(1<<8))
794              {
795                frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl
796                +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0/cd.wl;
797                frqObs->_phaseValid = true;
798                frqObs->_slipCounter = ll[count];
799              }
800
801              frqObs->_snr = cnr[count];
802              frqObs->_snrValid = true;
803              break;
804            case 7:
805              if(psr[count] > -1.0/(1<<10))
806              {
807                frqObs->_code = psr[count]*LIGHTSPEED/1000.0
808                +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0;
809                frqObs->_codeValid = true;
810              }
811
812              if(cp[count] > -1.0/(1<<8))
813              {
814                frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl
815                +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0/cd.wl;
816                frqObs->_phaseValid = true;
817                frqObs->_slipCounter = ll[count];
818              }
819
820              frqObs->_snr = cnr[count];
821              frqObs->_snrValid = true;
822
823              if(dop[count] > -1.6384)
824              {
825                frqObs->_doppler = -(dop[count]+rdop[numsat])/cd.wl;
826                frqObs->_dopplerValid = true;
827              }
828              break;
829            }
830            CurrentObs._obs.push_back(frqObs);
831          }
832        }
833      }
834      if(CurrentObs._obs.size() > 0)
835        _CurrentObsList.push_back(CurrentObs);
836    }
837  }
838  else if((type % 10) < 3)
839  {
840    emit(newMessage(QString("%1: Block %2 contain partial data! Ignored!")
841    .arg(_staID).arg(type).toAscii(), true));
842  }
843  if(!syncf)
844  {
845    decoded = true;
846    _obsList = _CurrentObsList;
847    _CurrentTime.reset();
848    _CurrentObsList.clear();
849  }
850  return decoded;
851}
852
853//
854////////////////////////////////////////////////////////////////////////////
855bool RTCM3Decoder::DecodeRTCM3GLONASS(unsigned char* data, int size)
856{
857  bool decoded = false;
858  bncTime CurrentObsTime;
859  int i, numsats, syncf, type;
860  uint64_t numbits = 0, bitfield = 0;
861
862  data += 3; /* header */
863  size -= 6; /* header + crc */
864
865  GETBITS(type, 12)
866  SKIPBITS(12) /* id */
867  GETBITS(i,27) /* tk */
868
869  CurrentObsTime.setTk(i);
870  if(_CurrentTime.valid() && CurrentObsTime != _CurrentTime)
871  {
872    decoded = true;
873    _obsList.append(_CurrentObsList);
874    _CurrentObsList.clear();
875  }
876  _CurrentTime = CurrentObsTime;
877
878  GETBITS(syncf,1) /* sync */
879  GETBITS(numsats,5)
880  SKIPBITS(4) /* smind, smint */
881
882  while(numsats--)
883  {
884    int sv, code, l1range, amb=0, freq;
885    t_satObs CurrentObs;
886    CurrentObs._time = CurrentObsTime;
887
888    GETBITS(sv, 6)
889    CurrentObs._prn.set('R', sv);
890    GETBITS(code, 1)
891    GETBITS(freq, 5)
892    GLOFreq[sv-1] = 100+freq-7; /* store frequency for other users (MSM) */
893
894    t_frqObs *frqObs = new t_frqObs;
895    /* L1 */
896    (code) ? frqObs->_rnxType2ch.assign("1P") : frqObs->_rnxType2ch.assign("1C");
897    GETBITS(l1range, 25);
898    GETBITSSIGN(i, 20);
899    if((i&((1<<20)-1)) != 0x80000)
900    {
901      frqObs->_code = l1range*0.02;
902      frqObs->_phase = (l1range*0.02+i*0.0005)/GLO_WAVELENGTH_L1(freq-7);
903      frqObs->_codeValid = frqObs->_phaseValid = true;
904    }
905    GETBITS(i, 7);
906    frqObs->_slipCounter = i;
907    if(type == 1010 || type == 1012)
908    {
909      GETBITS(amb,7);
910      if(amb)
911      {
912        frqObs->_code += amb*599584.916;
913        frqObs->_phase += (amb*599584.916)/GLO_WAVELENGTH_L1(freq-7);
914      }
915      GETBITS(i, 8);
916      if(i)
917      {
918        frqObs->_snr = i*0.25;
919        frqObs->_snrValid = true;
920      }
921    }
922    CurrentObs._obs.push_back(frqObs);
923    if(type == 1011 || type == 1012)
924    {
925      frqObs = new t_frqObs;
926      /* L2 */
927      GETBITS(code,2);
928      switch(code)
929      {
930      case 3: frqObs->_rnxType2ch.assign("2P"); break;
931      case 2: frqObs->_rnxType2ch.assign("2P"); break;
932      case 1: frqObs->_rnxType2ch.assign("2P"); break;
933      case 0: frqObs->_rnxType2ch.assign("2C"); break;
934      }
935      GETBITSSIGN(i,14);
936      if((i&((1<<14)-1)) != 0x2000)
937      {
938        frqObs->_code = l1range*0.02+i*0.02+amb*599584.916;
939        frqObs->_codeValid = true;
940      }
941      GETBITSSIGN(i,20);
942      if((i&((1<<20)-1)) != 0x80000)
943      {
944        frqObs->_phase = (l1range*0.02+i*0.0005+amb*599584.916)/GLO_WAVELENGTH_L2(freq-7);
945        frqObs->_phaseValid = true;
946      }
947      GETBITS(i,7);
948      frqObs->_slipCounter = i;
949      if(type == 1012)
950      {
951        GETBITS(i, 8);
952        if(i)
953        {
954          frqObs->_snr = i*0.25;
955          frqObs->_snrValid = true;
956        }
957      }
958      CurrentObs._obs.push_back(frqObs);
959    }
960    _CurrentObsList.push_back(CurrentObs);
961  }
962  if(!syncf)
963  {
964    decoded = true;
965    _obsList.append(_CurrentObsList);
966    _CurrentTime.reset();
967    _CurrentObsList.clear();
968  }
969  return decoded;
970}
971
972//
973////////////////////////////////////////////////////////////////////////////
974bool RTCM3Decoder::DecodeGPSEphemeris(unsigned char* data, int size)
975{
976  bool decoded = false;
977
978  if(size == 67)
979  {
980    t_ephGPS eph;
981    int i, week;
982    uint64_t numbits = 0, bitfield = 0;
983
984    data += 3; /* header */
985    size -= 6; /* header + crc */
986    SKIPBITS(12)
987
988    eph._receptDateTime = currentDateAndTimeGPS();
989
990    GETBITS(i, 6)
991    eph._prn.set('G', i);
992    GETBITS(week, 10)
993    week += 1024;
994    GETBITS(i, 4)
995    eph._ura = accuracyFromIndex(i, eph.type());
996    GETBITS(eph._L2Codes, 2)
997    GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
998    GETBITS(eph._IODE, 8)
999    GETBITS(i, 16)
1000    i <<= 4;
1001    eph._TOC.set(i*1000);
1002    GETFLOATSIGN(eph._clock_driftrate, 8, 1.0/(double)(1<<30)/(double)(1<<25))
1003    GETFLOATSIGN(eph._clock_drift, 16, 1.0/(double)(1<<30)/(double)(1<<13))
1004    GETFLOATSIGN(eph._clock_bias, 22, 1.0/(double)(1<<30)/(double)(1<<1))
1005    GETBITS(eph._IODC, 10)
1006    GETFLOATSIGN(eph._Crs, 16, 1.0/(double)(1<<5))
1007    GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
1008    GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1009    GETFLOATSIGN(eph._Cuc, 16, 1.0/(double)(1<<29))
1010    GETFLOAT(eph._e, 32, 1.0/(double)(1<<30)/(double)(1<<3))
1011    GETFLOATSIGN(eph._Cus, 16, 1.0/(double)(1<<29))
1012    GETFLOAT(eph._sqrt_A, 32, 1.0/(double)(1<<19))
1013    GETBITS(i, 16)
1014    i <<= 4;
1015    eph._TOEsec = i;
1016    bncTime t;
1017    t.set(i*1000);
1018    eph._TOEweek = t.gpsw();
1019    /* week from HOW, differs from TOC, TOE week, we use adapted value instead */
1020    if(eph._TOEweek > week + 1 || eph._TOEweek < week-1) /* invalid week */
1021      return false;
1022    GETFLOATSIGN(eph._Cic, 16, 1.0/(double)(1<<29))
1023    GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1024    GETFLOATSIGN(eph._Cis, 16, 1.0/(double)(1<<29))
1025    GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1026    GETFLOATSIGN(eph._Crc, 16, 1.0/(double)(1<<5))
1027    GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1028    GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
1029    GETFLOATSIGN(eph._TGD, 8, 1.0/(double)(1<<30)/(double)(1<<1))
1030    GETBITS(eph._health, 6)
1031    GETBITS(eph._L2PFlag, 1)
1032    GETBITS(eph._fitInterval, 1)
1033    eph._TOT = 0.9999e9;
1034
1035    emit newGPSEph(eph);
1036    decoded = true;
1037  }
1038  return decoded;
1039}
1040
1041//
1042////////////////////////////////////////////////////////////////////////////
1043bool RTCM3Decoder::DecodeGLONASSEphemeris(unsigned char* data, int size)
1044{
1045  bool decoded = false;
1046
1047  if(size == 51)
1048  {
1049    t_ephGlo eph;
1050    int sv, i, tk;
1051    uint64_t numbits = 0, bitfield = 0;
1052
1053    data += 3; /* header */
1054    size -= 6; /* header + crc */
1055    SKIPBITS(12)
1056
1057    eph._receptDateTime = currentDateAndTimeGPS();
1058
1059    GETBITS(sv, 6)
1060    eph._prn.set('R', sv);
1061
1062    GETBITS(i, 5)
1063    eph._frequency_number = i-7;
1064    GLOFreq[sv-1] = 100+i-7; /* store frequency for other users (MSM) */
1065    _gloFrq = QString("%1 %2").arg(eph._prn.toString().c_str()).arg(eph._frequency_number,2,'f',0);
1066    SKIPBITS(4) /* almanac healthy, almanac health ok, P1 */
1067    GETBITS(i, 5)
1068    tk = i*60*60;
1069    GETBITS(i, 6)
1070    tk += i*60;
1071    GETBITS(i, 1)
1072    tk += i*30;
1073    eph._tki = tk < 3*60*60 ? tk-3*60*60+86400 : tk-3*60*60;
1074    GETBITS(eph._health, 1)
1075    SKIPBITS(1) /* P2 */
1076    GETBITS(i, 7)
1077    eph._TOC.setTk(i*15*60*1000); /* tb */
1078
1079    GETFLOATSIGNM(eph._x_velocity, 24, 1.0/(double)(1<<20))
1080    GETFLOATSIGNM(eph._x_pos, 27, 1.0/(double)(1<<11))
1081    GETFLOATSIGNM(eph._x_acceleration, 5, 1.0/(double)(1<<30))
1082    GETFLOATSIGNM(eph._y_velocity, 24, 1.0/(double)(1<<20))
1083    GETFLOATSIGNM(eph._y_pos, 27, 1.0/(double)(1<<11))
1084    GETFLOATSIGNM(eph._y_acceleration, 5, 1.0/(double)(1<<30))
1085    GETFLOATSIGNM(eph._z_velocity, 24, 1.0/(double)(1<<20))
1086    GETFLOATSIGNM(eph._z_pos, 27, 1.0/(double)(1<<11))
1087    GETFLOATSIGNM(eph._z_acceleration, 5, 1.0/(double)(1<<30))
1088    SKIPBITS(1) /* P3 */
1089    GETFLOATSIGNM(eph._gamma, 11, 1.0/(double)(1<<30)/(double)(1<<10))
1090    SKIPBITS(3) /* GLONASS-M P, GLONASS-M ln (third string) */
1091    GETFLOATSIGNM(eph._tau, 22, 1.0/(double)(1<<30)) /* GLONASS tau n(tb) */
1092    SKIPBITS(5) /* GLONASS-M delta tau n(tb) */
1093    GETBITS(eph._E, 5)
1094    /* GETBITS(i, 1) / * GLONASS-M P4 */
1095    /* GETBITS(i, 4) / * GLONASS-M Ft */
1096    /* GETBITS(i, 11) / * GLONASS-M Nt */
1097    /* GETBITS(i, 2) / * GLONASS-M M */
1098    /* GETBITS(i, 1) / * GLONASS-M The Availability of Additional Data */
1099    /* GETBITS(i, 11) / * GLONASS-M Na */
1100    /* GETFLOATSIGNM(i, 32, 1.0/(double)(1<<30)/(double)(1<<1)) / * GLONASS tau c */
1101    /* GETBITS(i, 5) / * GLONASS-M N4 */
1102    /* GETFLOATSIGNM(i, 22, 1.0/(double)(1<<30)) / * GLONASS-M tau GPS */
1103    /* GETBITS(i, 1) / * GLONASS-M ln (fifth string) */
1104
1105    unsigned year, month, day;
1106    eph._TOC.civil_date(year, month, day);
1107    eph._gps_utc = gnumleap(year, month, day);
1108    eph._tt = eph._TOC;
1109
1110    eph._xv(1) = eph._x_pos * 1.e3;
1111    eph._xv(2) = eph._y_pos * 1.e3;
1112    eph._xv(3) = eph._z_pos * 1.e3;
1113    eph._xv(4) = eph._x_velocity * 1.e3;
1114    eph._xv(5) = eph._y_velocity * 1.e3;
1115    eph._xv(6) = eph._z_velocity * 1.e3;
1116
1117    emit newGlonassEph(eph);
1118    decoded = true;
1119  }
1120  return decoded;
1121}
1122
1123//
1124////////////////////////////////////////////////////////////////////////////
1125bool RTCM3Decoder::DecodeQZSSEphemeris(unsigned char* data, int size)
1126{
1127  bool decoded = false;
1128
1129  if(size == 67)
1130  {
1131    t_ephGPS eph;
1132    int i, week;
1133    uint64_t numbits = 0, bitfield = 0;
1134
1135    data += 3; /* header */
1136    size -= 6; /* header + crc */
1137    SKIPBITS(12)
1138
1139    eph._receptDateTime = currentDateAndTimeGPS();
1140
1141    GETBITS(i, 4)
1142    eph._prn.set('J', i);
1143
1144    GETBITS(i, 16)
1145    i <<= 4;
1146    eph._TOC.set(i*1000);
1147
1148    GETFLOATSIGN(eph._clock_driftrate, 8, 1.0/(double)(1<<30)/(double)(1<<25))
1149    GETFLOATSIGN(eph._clock_drift, 16, 1.0/(double)(1<<30)/(double)(1<<13))
1150    GETFLOATSIGN(eph._clock_bias, 22, 1.0/(double)(1<<30)/(double)(1<<1))
1151    GETBITS(eph._IODE, 8)
1152    GETFLOATSIGN(eph._Crs, 16, 1.0/(double)(1<<5))
1153    GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
1154    GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1155    GETFLOATSIGN(eph._Cuc, 16, 1.0/(double)(1<<29))
1156    GETFLOAT(eph._e, 32, 1.0/(double)(1<<30)/(double)(1<<3))
1157    GETFLOATSIGN(eph._Cus, 16, 1.0/(double)(1<<29))
1158    GETFLOAT(eph._sqrt_A, 32, 1.0/(double)(1<<19))
1159    GETBITS(i, 16)
1160    i <<= 4;
1161    eph._TOEsec = i;
1162    bncTime t;
1163    t.set(i);
1164
1165    GETFLOATSIGN(eph._Cic, 16, 1.0/(double)(1<<29))
1166    GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1167    GETFLOATSIGN(eph._Cis, 16, 1.0/(double)(1<<29))
1168    GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1169    GETFLOATSIGN(eph._Crc, 16, 1.0/(double)(1<<5))
1170    GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1171    GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
1172    GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
1173    GETBITS(eph._L2Codes, 2)
1174    GETBITS(week, 10)
1175    week += 1024;
1176    eph._TOEweek = t.gpsw();
1177    /* week from HOW, differs from TOC, TOE week, we use adapted value instead */
1178    if(eph._TOEweek > week + 1 || eph._TOEweek < week-1) /* invalid week */
1179      return false;
1180
1181    GETBITS(i, 4)
1182    if(i <= 6)
1183      eph._ura = ceil(10.0*pow(2.0, 1.0+i/2.0))/10.0;
1184    else
1185      eph._ura = ceil(10.0*pow(2.0, i/2.0))/10.0;
1186    GETBITS(eph._health, 6)
1187    GETFLOATSIGN(eph._TGD, 8, 1.0/(double)(1<<30)/(double)(1<<1))
1188    GETBITS(eph._IODC, 10)
1189    GETBITS(eph._fitInterval, 1)
1190    eph._TOT = 0.9999e9;
1191    eph._L2PFlag = 0; /* does not exist for QZSS */
1192
1193    emit newGPSEph(eph);
1194    decoded = true;
1195  }
1196  return decoded;
1197}
1198
1199//
1200////////////////////////////////////////////////////////////////////////////
1201bool RTCM3Decoder::DecodeSBASEphemeris(unsigned char* data, int size)
1202{
1203  bool decoded = false;
1204
1205  if(size == 35)
1206  {
1207    t_ephSBAS eph;
1208    int i;
1209    uint64_t numbits = 0, bitfield = 0;
1210
1211    data += 3; /* header */
1212    size -= 6; /* header + crc */
1213    SKIPBITS(12)
1214
1215    eph._receptDateTime = currentDateAndTimeGPS();
1216
1217    GETBITS(i, 6)
1218    eph._prn.set('S', 20+i);
1219    GETBITS(eph._IODN, 8)
1220    GETBITS(i, 13)
1221    i <<= 4;
1222    eph._TOC.setTOD(i*1000);
1223    GETBITS(i, 4)
1224    eph._ura = accuracyFromIndex(i, eph.type());
1225    GETFLOATSIGN(eph._x_pos, 30, 0.08)
1226    GETFLOATSIGN(eph._y_pos, 30, 0.08)
1227    GETFLOATSIGN(eph._z_pos, 25, 0.4)
1228    GETFLOATSIGN(eph._x_velocity, 17, 0.000625)
1229    GETFLOATSIGN(eph._y_velocity, 17, 0.000625)
1230    GETFLOATSIGN(eph._z_velocity, 18, 0.004)
1231    GETFLOATSIGN(eph._x_acceleration, 10, 0.0000125)
1232    GETFLOATSIGN(eph._y_acceleration, 10, 0.0000125)
1233    GETFLOATSIGN(eph._z_acceleration, 10, 0.0000625)
1234    GETFLOATSIGN(eph._agf0, 12, 1.0/(1<<30)/(1<<1))
1235    GETFLOATSIGN(eph._agf1, 8, 1.0/(1<<30)/(1<<10))
1236
1237    eph._TOW = 0.9999E9;
1238    eph._health = 0;
1239
1240    emit newSBASEph(eph);
1241    decoded = true;
1242  }
1243  return decoded;
1244}
1245
1246//
1247////////////////////////////////////////////////////////////////////////////
1248bool RTCM3Decoder::DecodeGalileoEphemeris(unsigned char* data, int size)
1249{
1250  bool decoded = false;
1251  uint64_t numbits = 0, bitfield = 0;
1252  int i;
1253
1254  data += 3; /* header */
1255  size -= 6; /* header + crc */
1256  GETBITS(i, 12)
1257
1258  if((i == 1046 && size == 61) || (i == 1045 && size == 60))
1259  {
1260    t_ephGal eph;
1261
1262    eph._receptDateTime = currentDateAndTimeGPS();
1263
1264    eph._inav = (i == 1046);
1265    eph._fnav = (i == 1045);
1266    GETBITS(i, 6)
1267    eph._prn.set('E', i, eph._inav ? 1 : 0);
1268
1269    GETBITS(eph._TOEweek, 12)
1270    GETBITS(eph._IODnav, 10)
1271    GETBITS(i, 8)
1272    eph._SISA = accuracyFromIndex(i, eph.type());
1273    GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
1274    GETBITSFACTOR(i, 14, 60)
1275    eph._TOC.set(1024+eph._TOEweek, i);
1276    GETFLOATSIGN(eph._clock_driftrate, 6, 1.0/(double)(1<<30)/(double)(1<<29))
1277    GETFLOATSIGN(eph._clock_drift, 21, 1.0/(double)(1<<30)/(double)(1<<16))
1278    GETFLOATSIGN(eph._clock_bias, 31, 1.0/(double)(1<<30)/(double)(1<<4))
1279    GETFLOATSIGN(eph._Crs, 16, 1.0/(double)(1<<5))
1280    GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
1281    GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1282    GETFLOATSIGN(eph._Cuc, 16, 1.0/(double)(1<<29))
1283    GETFLOAT(eph._e, 32, 1.0/(double)(1<<30)/(double)(1<<3))
1284    GETFLOATSIGN(eph._Cus, 16, 1.0/(double)(1<<29))
1285    GETFLOAT(eph._sqrt_A, 32, 1.0/(double)(1<<19))
1286    GETBITSFACTOR(eph._TOEsec, 14, 60)
1287    /* FIXME: overwrite value, copied from old code */
1288    eph._TOEsec = eph._TOC.gpssec();
1289    GETFLOATSIGN(eph._Cic, 16, 1.0/(double)(1<<29))
1290    GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1291    GETFLOATSIGN(eph._Cis, 16, 1.0/(double)(1<<29))
1292    GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1293    GETFLOATSIGN(eph._Crc, 16, 1.0/(double)(1<<5))
1294    GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1295    GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
1296    GETFLOATSIGN(eph._BGD_1_5A, 10, 1.0/(double)(1<<30)/(double)(1<<2))
1297    if(eph._inav)
1298    {
1299      /* set unused F/NAV values */
1300      eph._E5aHS = 0.0;
1301      eph._e5aDataInValid = false;
1302
1303      GETFLOATSIGN(eph._BGD_1_5B, 10, 1.0/(double)(1<<30)/(double)(1<<2))
1304      GETBITS(eph._E5bHS, 2)
1305      GETBITS(eph._e5bDataInValid, 1)
1306      GETBITS(eph._E1_bHS, 2)
1307      GETBITS(eph._e1DataInValid, 1)
1308    }
1309    else
1310    {
1311      /* set unused I/NAV values */
1312      eph._BGD_1_5B = 0.0;
1313      eph._E5bHS = 0.0;
1314      eph._E1_bHS = 0.0;
1315      eph._e1DataInValid = false;
1316      eph._e5bDataInValid = false;
1317
1318      GETBITS(eph._E5aHS, 2)
1319      GETBITS(eph._e5aDataInValid, 1)
1320    }
1321    eph._TOT = 0.9999e9;
1322
1323    emit newGalileoEph(eph);
1324    decoded = true;
1325  }
1326  return decoded;
1327}
1328
1329//
1330////////////////////////////////////////////////////////////////////////////
1331bool RTCM3Decoder::DecodeBDSEphemeris(unsigned char* data, int size)
1332{
1333  bool decoded = false;
1334
1335  if(size == 70)
1336  {
1337    t_ephBDS eph;
1338    int i;
1339    uint64_t numbits = 0, bitfield = 0;
1340
1341    data += 3; /* header */
1342    size -= 6; /* header + crc */
1343    SKIPBITS(12)
1344
1345    eph._receptDateTime = currentDateAndTimeGPS();
1346
1347    GETBITS(i, 6)
1348    eph._prn.set('C', i);
1349
1350    SKIPBITS(13) /* week */
1351    GETBITS(i, 4)
1352    eph._URA = accuracyFromIndex(i, eph.type());
1353    GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
1354    GETBITS(eph._AODE, 5)
1355    GETBITS(i, 17)
1356    i <<= 3;
1357    eph._TOC.setBDS(i*1000);
1358    GETFLOATSIGN(eph._clock_driftrate, 11, 1.0/(double)(1<<30)/(double)(1<<30)/(double)(1<<6))
1359    GETFLOATSIGN(eph._clock_drift, 22, 1.0/(double)(1<<30)/(double)(1<<20))
1360    GETFLOATSIGN(eph._clock_bias, 24, 1.0/(double)(1<<30)/(double)(1<<3))
1361    GETBITS(eph._AODC, 5)
1362    GETFLOATSIGN(eph._Crs, 18, 1.0/(double)(1<<6))
1363    GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
1364    GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1365    GETFLOATSIGN(eph._Cuc, 18, 1.0/(double)(1<<30)/(double)(1<<1))
1366    GETFLOAT(eph._e, 32, 1.0/(double)(1<<30)/(double)(1<<3))
1367    GETFLOATSIGN(eph._Cus, 18, 1.0/(double)(1<<30)/(double)(1<<1))
1368    GETFLOAT(eph._sqrt_A, 32, 1.0/(double)(1<<19))
1369    GETBITS(i, 17)
1370    i <<= 3;
1371    eph._TOEsec = i;
1372    eph._TOE.setBDS(i*1000);
1373    GETFLOATSIGN(eph._Cic, 18, 1.0/(double)(1<<30)/(double)(1<<1))
1374    GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1375    GETFLOATSIGN(eph._Cis, 18, 1.0/(double)(1<<30)/(double)(1<<1))
1376    GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1377    GETFLOATSIGN(eph._Crc, 18, 1.0/(double)(1<<6))
1378    GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1379    GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
1380    GETFLOATSIGN(eph._TGD1, 10, 0.0000000001)
1381    GETFLOATSIGN(eph._TGD2, 10, 0.0000000001)
1382    GETBITS(eph._SatH1, 1)
1383
1384    eph._TOW = 0.9999E9;
1385    emit newBDSEph(eph);
1386    decoded = true;
1387  }
1388  return decoded;
1389}
1390
1391//
1392////////////////////////////////////////////////////////////////////////////
1393bool RTCM3Decoder::DecodeAntenna(unsigned char* data, int size)
1394{
1395  char *antenna, type[256];
1396  int antnum = -1;
1397  uint64_t numbits = 0, bitfield = 0;
1398
1399  data += 4; /* header */
1400  size -= 6; /* header + crc */
1401
1402  SKIPBITS(12)
1403  GETSTRING(antnum, antenna)
1404  if (antnum > -1 && antnum < 265) {
1405    memcpy(type, antenna, antnum);
1406    type[antnum] = 0;
1407    if (!_antType.contains(type)) {
1408      _antType.push_back(type);
1409    }
1410  }
1411  return true;
1412}
1413
1414//
1415////////////////////////////////////////////////////////////////////////////
1416bool RTCM3Decoder::DecodeAntennaPosition(unsigned char* data, int size)
1417{
1418  int type;
1419  uint64_t numbits = 0, bitfield = 0;
1420  double x, y, z;
1421
1422  data += 3; /* header */
1423  size -= 6; /* header + crc */
1424
1425  GETBITS(type, 12)
1426  _antList.push_back(t_antInfo());
1427  _antList.back().type = t_antInfo::ARP;
1428  SKIPBITS(22)
1429  GETBITSSIGN(x, 38)
1430  _antList.back().xx = x * 1e-4;
1431  SKIPBITS(2)
1432  GETBITSSIGN(y, 38)
1433  _antList.back().yy = y * 1e-4;
1434  SKIPBITS(2)
1435  GETBITSSIGN(z, 38)
1436  _antList.back().zz = z * 1e-4;
1437  if(type == 1006)
1438  {
1439    double h;
1440    GETBITS(h, 16)
1441    _antList.back().height = h * 1e-4;
1442    _antList.back().height_f = true;
1443  }
1444  _antList.back().message  = type;
1445
1446  return true;
1447}
1448
1449//
1450////////////////////////////////////////////////////////////////////////////
1451t_irc RTCM3Decoder::Decode(char* buffer, int bufLen, vector<string>& errmsg)
1452{
1453  bool decoded = false;
1454
1455  errmsg.clear();
1456
1457  while(bufLen && _MessageSize < sizeof(_Message))
1458  {
1459    int l = sizeof(_Message) - _MessageSize;
1460    if(l > bufLen)
1461      l = bufLen;
1462    memcpy(_Message+_MessageSize, buffer, l);
1463    _MessageSize += l;
1464    bufLen -= l;
1465    buffer += l;
1466    int id;
1467    while((id = GetMessage()))
1468    {
1469      /* reset station ID for file loading as it can change */
1470      if(_rawFile)
1471        _staID = _rawFile->staID();
1472      /* store the id into the list of loaded blocks */
1473      _typeList.push_back(id);
1474
1475      /* SSR I+II data handled in another function, already pass the
1476       * extracted data block. That does no harm, as it anyway skip everything
1477       * else. */
1478      if((id >= 1057 && id <= 1068) || (id >= 1240 && id <= 1270))
1479      {
1480        if (!_coDecoders.contains(_staID.toAscii()))
1481          _coDecoders[_staID.toAscii()] = new RTCM3coDecoder(_staID);
1482        RTCM3coDecoder* coDecoder = _coDecoders[_staID.toAscii()];
1483        if(coDecoder->Decode(reinterpret_cast<char *>(_Message), _BlockSize,
1484        errmsg) == success)
1485        {
1486          decoded = true;
1487        }
1488      }
1489      else if(id >= 1070 && id <= 1229) /* MSM */
1490      {
1491        if(DecodeRTCM3MSM(_Message, _BlockSize))
1492          decoded = true;
1493      }
1494      else
1495      {
1496        switch(id)
1497        {
1498        case 1001: case 1003:
1499          emit(newMessage(QString("%1: Block %2 contain partial data! Ignored!")
1500          .arg(_staID).arg(id).toAscii(), true));
1501          break; /* no use decoding partial data ATM, remove break when data can be used */
1502        case 1002: case 1004:
1503          if(DecodeRTCM3GPS(_Message, _BlockSize))
1504            decoded = true;
1505          break;
1506        case 1009: case 1011:
1507          emit(newMessage(QString("%1: Block %2 contain partial data! Ignored!")
1508          .arg(_staID).arg(id).toAscii(), true));
1509          break; /* no use decoding partial data ATM, remove break when data can be used */
1510        case 1010: case 1012:
1511          if(DecodeRTCM3GLONASS(_Message, _BlockSize))
1512            decoded = true;
1513          break;
1514        case 1019:
1515          if(DecodeGPSEphemeris(_Message, _BlockSize))
1516            decoded = true;
1517          break;
1518        case 1020:
1519          if(DecodeGLONASSEphemeris(_Message, _BlockSize))
1520            decoded = true;
1521          break;
1522        case 1043:
1523          if(DecodeSBASEphemeris(_Message, _BlockSize))
1524            decoded = true;
1525          break;
1526        case 1044:
1527          if(DecodeQZSSEphemeris(_Message, _BlockSize))
1528            decoded = true;
1529          break;
1530        case 1045: case 1046:
1531          if(DecodeGalileoEphemeris(_Message, _BlockSize))
1532            decoded = true;
1533          break;
1534        case RTCM3ID_BDS:
1535          if(DecodeBDSEphemeris(_Message, _BlockSize))
1536            decoded = true;
1537          break;
1538        case 1007: case 1008: case 1033:
1539          DecodeAntenna(_Message, _BlockSize);
1540          break;
1541        case 1005: case 1006:
1542          DecodeAntennaPosition(_Message, _BlockSize);
1543          break;
1544        }
1545      }
1546    }
1547  }
1548  return decoded ? success : failure;
1549};
1550
1551//
1552////////////////////////////////////////////////////////////////////////////
1553uint32_t RTCM3Decoder::CRC24(long size, const unsigned char *buf)
1554{
1555  uint32_t crc = 0;
1556  int i;
1557
1558  while(size--)
1559  {
1560    crc ^= (*buf++) << (16);
1561    for(i = 0; i < 8; i++)
1562    {
1563      crc <<= 1;
1564      if(crc & 0x1000000)
1565        crc ^= 0x01864cfb;
1566    }
1567  }
1568  return crc;
1569}
1570
1571//
1572////////////////////////////////////////////////////////////////////////////
1573int RTCM3Decoder::GetMessage(void)
1574{
1575  unsigned char *m, *e;
1576  int i;
1577
1578  m = _Message+_SkipBytes;
1579  e = _Message+_MessageSize;
1580  _NeedBytes = _SkipBytes = 0;
1581  while(e-m >= 3)
1582  {
1583    if(m[0] == 0xD3)
1584    {
1585      _BlockSize = ((m[1]&3)<<8)|m[2];
1586      if(e-m >= static_cast<int>(_BlockSize+6))
1587      {
1588        if(static_cast<uint32_t>((m[3+_BlockSize]<<16)|(m[3+_BlockSize+1]<<8)
1589        |(m[3+_BlockSize+2])) == CRC24(_BlockSize+3, m))
1590        {
1591          _BlockSize +=6;
1592          _SkipBytes = _BlockSize;
1593          break;
1594        }
1595        else
1596          ++m;
1597      }
1598      else
1599      {
1600        _NeedBytes = _BlockSize;
1601        break;
1602      }
1603    }
1604    else
1605      ++m;
1606  }
1607  if(e-m < 3)
1608    _NeedBytes = 3;
1609
1610  /* copy buffer to front */
1611  i = m - _Message;
1612  if(i && m < e)
1613    memmove(_Message, m, static_cast<size_t>(_MessageSize-i));
1614  _MessageSize -= i;
1615
1616  return !_NeedBytes ? ((_Message[3]<<4)|(_Message[4]>>4)) : 0;
1617}
1618
1619// Time of Corrections
1620//////////////////////////////////////////////////////////////////////////////
1621int RTCM3Decoder::corrGPSEpochTime() const {
1622  return _coDecoders.size() > 0 ? _coDecoders.begin().value()->corrGPSEpochTime() : -1;
1623}
Note: See TracBrowser for help on using the repository browser.