source: ntrip/branches/BNC_2.12/src/RTCM3/RTCM3Decoder.cpp @ 8681

Last change on this file since 8681 was 8681, checked in by stuerze, 6 weeks ago

minor changes in RTCM3 Decoder with respect to MSM obs codes

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