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

Last change on this file since 6864 was 6864, checked in by stuerze, 5 years ago

scanning of GLONASS slots is added

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