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

Last change on this file since 8617 was 8617, checked in by stuerze, 8 weeks ago

a computed lock time is added for RTCM3 phase observations as an optional feed engine output

File size: 52.2 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"},
359        {0.0, 0},
360        {0.0, 0},
361        {QZSS_WAVELENGTH_LEX, "6S"},
362        {QZSS_WAVELENGTH_LEX, "6L"},
363        {QZSS_WAVELENGTH_LEX, "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, "1D"},
383        {GPS_WAVELENGTH_L1, "1P"},
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        {0.0, 0},
392        {0.0, 0},
393        {0.0, 0},
394        {0.0, 0},
395        {0.0, 0},
396        {BDS_WAVELENGTH_B3, "6I"},
397        {0.0, 0},
398        {0.0, 0},
399        {0.0, 0},
400        {0.0, 0},
401        {0.0, 0},
402        {BDS_WAVELENGTH_B2, "7I"},
403        {0.0, 0},
404        {0.0, 0},
405        {0.0, 0},
406        {0.0, 0},
407        {0.0, 0},
408        {0.0, 0},
409        {0.0, 0},
410        {0.0, 0},
411        {0.0, 0},
412        {0.0, 0},
413        {0.0, 0},
414        {0.0, 0},
415        {0.0, 0},
416        {0.0, 0},
417        {0.0, 0},
418        {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).toLatin1(), 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    emit newGlonassEph(eph);
1128    decoded = true;
1129  }
1130  return decoded;
1131}
1132
1133//
1134////////////////////////////////////////////////////////////////////////////
1135bool RTCM3Decoder::DecodeQZSSEphemeris(unsigned char* data, int size) {
1136  bool decoded = false;
1137
1138  if (size == 67) {
1139    t_ephGPS eph;
1140    int i, week;
1141    uint64_t numbits = 0, bitfield = 0;
1142
1143    data += 3; /* header */
1144    size -= 6; /* header + crc */
1145    SKIPBITS(12)
1146
1147    eph._receptDateTime = currentDateAndTimeGPS();
1148
1149    GETBITS(i, 4)
1150    eph._prn.set('J', i);
1151
1152    GETBITS(i, 16)
1153    i <<= 4;
1154    eph._TOC.set(i * 1000);
1155
1156    GETFLOATSIGN(eph._clock_driftrate, 8,
1157        1.0 / (double )(1 << 30) / (double )(1 << 25))
1158    GETFLOATSIGN(eph._clock_drift, 16,
1159        1.0 / (double )(1 << 30) / (double )(1 << 13))
1160    GETFLOATSIGN(eph._clock_bias, 22,
1161        1.0 / (double )(1 << 30) / (double )(1 << 1))
1162    GETBITS(eph._IODE, 8)
1163    GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5))
1164    GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
1165    GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1166    GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29))
1167    GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))
1168    GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29))
1169    GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))
1170    GETBITS(i, 16)
1171    i <<= 4;
1172    eph._TOEsec = i;
1173    bncTime t;
1174    t.set(i*1000);
1175    eph._TOEweek = t.gpsw();
1176    GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29))
1177    GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1178    GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29))
1179    GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1180    GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5))
1181    GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1182    GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
1183    GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
1184    GETBITS(eph._L2Codes, 2)
1185    GETBITS(week, 10)
1186    int numOfRollOvers = int(floor(t.gpsw()/1024.0));
1187    week += (numOfRollOvers * 1024);
1188    /* week from HOW, differs from TOC, TOE week, we use adapted value instead */
1189    if (eph._TOEweek > week + 1 || eph._TOEweek < week - 1) /* invalid week */
1190      return false;
1191
1192    GETBITS(i, 4)
1193    if (i <= 6)
1194      eph._ura = ceil(10.0 * pow(2.0, 1.0 + i / 2.0)) / 10.0;
1195    else
1196      eph._ura = ceil(10.0 * pow(2.0, i / 2.0)) / 10.0;
1197    GETBITS(eph._health, 6)
1198    GETFLOATSIGN(eph._TGD, 8, 1.0 / (double )(1 << 30) / (double )(1 << 1))
1199    GETBITS(eph._IODC, 10)
1200    GETBITS(eph._fitInterval, 1)
1201    eph._TOT = 0.9999e9;
1202    eph._L2PFlag = 0; /* does not exist for QZSS */
1203
1204    emit newGPSEph(eph);
1205    decoded = true;
1206  }
1207  return decoded;
1208}
1209
1210//
1211////////////////////////////////////////////////////////////////////////////
1212bool RTCM3Decoder::DecodeSBASEphemeris(unsigned char* data, int size) {
1213  bool decoded = false;
1214
1215  if (size == 35) {
1216    t_ephSBAS eph;
1217    int i;
1218    uint64_t numbits = 0, bitfield = 0;
1219
1220    data += 3; /* header */
1221    size -= 6; /* header + crc */
1222    SKIPBITS(12)
1223
1224    eph._receptDateTime = currentDateAndTimeGPS();
1225
1226    GETBITS(i, 6)
1227    eph._prn.set('S', 20 + i);
1228    GETBITS(eph._IODN, 8)
1229    GETBITS(i, 13)
1230    i <<= 4;
1231    eph._TOC.setTOD(i * 1000);
1232    GETBITS(i, 4)
1233    eph._ura = accuracyFromIndex(i, eph.type());
1234    GETFLOATSIGN(eph._x_pos, 30, 0.08)
1235    GETFLOATSIGN(eph._y_pos, 30, 0.08)
1236    GETFLOATSIGN(eph._z_pos, 25, 0.4)
1237    GETFLOATSIGN(eph._x_velocity, 17, 0.000625)
1238    GETFLOATSIGN(eph._y_velocity, 17, 0.000625)
1239    GETFLOATSIGN(eph._z_velocity, 18, 0.004)
1240    GETFLOATSIGN(eph._x_acceleration, 10, 0.0000125)
1241    GETFLOATSIGN(eph._y_acceleration, 10, 0.0000125)
1242    GETFLOATSIGN(eph._z_acceleration, 10, 0.0000625)
1243    GETFLOATSIGN(eph._agf0, 12, 1.0 / (1 << 30) / (1 << 1))
1244    GETFLOATSIGN(eph._agf1, 8, 1.0 / (1 << 30) / (1 << 10))
1245
1246    eph._TOT = 0.9999E9;
1247    eph._health = 0;
1248
1249    emit newSBASEph(eph);
1250    decoded = true;
1251  }
1252  return decoded;
1253}
1254
1255//
1256////////////////////////////////////////////////////////////////////////////
1257bool RTCM3Decoder::DecodeGalileoEphemeris(unsigned char* data, int size) {
1258  bool decoded = false;
1259  uint64_t numbits = 0, bitfield = 0;
1260  int i;
1261
1262  data += 3; /* header */
1263  size -= 6; /* header + crc */
1264  GETBITS(i, 12)
1265
1266  if ((i == 1046 && size == 61) || (i == 1045 && size == 60)) {
1267    t_ephGal eph;
1268
1269    eph._receptDateTime = currentDateAndTimeGPS();
1270
1271    eph._inav = (i == 1046);
1272    eph._fnav = (i == 1045);
1273    GETBITS(i, 6)
1274    eph._prn.set('E', i, eph._inav ? 1 : 0);
1275
1276    GETBITS(eph._TOEweek, 12)
1277    GETBITS(eph._IODnav, 10)
1278    GETBITS(i, 8)
1279    eph._SISA = accuracyFromIndex(i, eph.type());
1280    GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
1281    GETBITSFACTOR(i, 14, 60)
1282    eph._TOC.set(1024 + eph._TOEweek, i);
1283    GETFLOATSIGN(eph._clock_driftrate, 6,
1284        1.0 / (double )(1 << 30) / (double )(1 << 29))
1285    GETFLOATSIGN(eph._clock_drift, 21,
1286        1.0 / (double )(1 << 30) / (double )(1 << 16))
1287    GETFLOATSIGN(eph._clock_bias, 31,
1288        1.0 / (double )(1 << 30) / (double )(1 << 4))
1289    GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5))
1290    GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
1291    GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1292    GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29))
1293    GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))
1294    GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29))
1295    GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))
1296    GETBITSFACTOR(eph._TOEsec, 14, 60)
1297    /* FIXME: overwrite value, copied from old code */
1298    eph._TOEsec = eph._TOC.gpssec();
1299    GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29))
1300    GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1301    GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29))
1302    GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1303    GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5))
1304    GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1305    GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
1306    GETFLOATSIGN(eph._BGD_1_5A, 10,
1307        1.0 / (double )(1 << 30) / (double )(1 << 2))
1308    if (eph._inav) {
1309      /* set unused F/NAV values */
1310      eph._E5aHS = 0.0;
1311      eph._e5aDataInValid = false;
1312
1313      GETFLOATSIGN(eph._BGD_1_5B, 10,
1314          1.0 / (double )(1 << 30) / (double )(1 << 2))
1315      GETBITS(eph._E5bHS, 2)
1316      GETBITS(eph._e5bDataInValid, 1)
1317      GETBITS(eph._E1_bHS, 2)
1318      GETBITS(eph._e1DataInValid, 1)
1319    }
1320    else {
1321      /* set unused I/NAV values */
1322      eph._BGD_1_5B = 0.0;
1323      eph._E5bHS = 0.0;
1324      eph._E1_bHS = 0.0;
1325      eph._e1DataInValid = false;
1326      eph._e5bDataInValid = false;
1327
1328      GETBITS(eph._E5aHS, 2)
1329      GETBITS(eph._e5aDataInValid, 1)
1330    }
1331    eph._TOT = 0.9999e9;
1332
1333    emit newGalileoEph(eph);
1334    decoded = true;
1335  }
1336  return decoded;
1337}
1338
1339//
1340////////////////////////////////////////////////////////////////////////////
1341bool RTCM3Decoder::DecodeBDSEphemeris(unsigned char* data, int size) {
1342  bool decoded = false;
1343
1344  if (size == 70) {
1345    t_ephBDS eph;
1346    int i;
1347    uint64_t numbits = 0, bitfield = 0;
1348
1349    data += 3; /* header */
1350    size -= 6; /* header + crc */
1351    SKIPBITS(12)
1352
1353    eph._receptDateTime = currentDateAndTimeGPS();
1354
1355    GETBITS(i, 6)
1356    eph._prn.set('C', i);
1357
1358    SKIPBITS(13)
1359    /* week */
1360    GETBITS(i, 4)
1361    eph._URA = accuracyFromIndex(i, eph.type());
1362    GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
1363    GETBITS(eph._AODE, 5)
1364    GETBITS(i, 17)
1365    i <<= 3;
1366    eph._TOC.setBDS(i * 1000);
1367    GETFLOATSIGN(eph._clock_driftrate, 11,
1368        1.0 / (double )(1 << 30) / (double )(1 << 30) / (double )(1 << 6))
1369    GETFLOATSIGN(eph._clock_drift, 22,
1370        1.0 / (double )(1 << 30) / (double )(1 << 20))
1371    GETFLOATSIGN(eph._clock_bias, 24,
1372        1.0 / (double )(1 << 30) / (double )(1 << 3))
1373    GETBITS(eph._AODC, 5)
1374    GETFLOATSIGN(eph._Crs, 18, 1.0 / (double )(1 << 6))
1375    GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
1376    GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1377    GETFLOATSIGN(eph._Cuc, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1))
1378    GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))
1379    GETFLOATSIGN(eph._Cus, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1))
1380    GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))
1381    GETBITS(i, 17)
1382    i <<= 3;
1383    eph._TOEsec = i;
1384    eph._TOE.setBDS(i * 1000);
1385    GETFLOATSIGN(eph._Cic, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1))
1386    GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1387    GETFLOATSIGN(eph._Cis, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1))
1388    GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1389    GETFLOATSIGN(eph._Crc, 18, 1.0 / (double )(1 << 6))
1390    GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1391    GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
1392    GETFLOATSIGN(eph._TGD1, 10, 0.0000000001)
1393    GETFLOATSIGN(eph._TGD2, 10, 0.0000000001)
1394    GETBITS(eph._SatH1, 1)
1395
1396    eph._TOT = 0.9999E9;
1397    emit newBDSEph(eph);
1398    decoded = true;
1399  }
1400  return decoded;
1401}
1402
1403//
1404////////////////////////////////////////////////////////////////////////////
1405bool RTCM3Decoder::DecodeAntennaReceiver(unsigned char* data, int size) {
1406  char *antenna;
1407  char *antserialnum;
1408  char *receiver;
1409  char *recfirmware;
1410  char *recserialnum;
1411  int type;
1412  int antsernum = -1;
1413  int antnum = -1;
1414  int recnum = -1;
1415  int recsernum = -1;
1416  int recfirnum = -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) /* reference station ID */
1424  GETSTRING(antnum, antenna)
1425  if ((antnum > -1 && antnum < 265) &&
1426      (_antType.empty() || strncmp(_antType.back().descriptor, antenna, recnum) != 0)) {
1427    _antType.push_back(t_antInfo());
1428    memcpy(_antType.back().descriptor, antenna, antnum);
1429    _antType.back().descriptor[antnum] = 0;
1430  }
1431  SKIPBITS(8) /* antenna setup ID */
1432  if (type == 1008 || type == 1033 ) {
1433    GETSTRING(antsernum, antserialnum)
1434    if ((antsernum > -1 && antsernum < 265)) {
1435      memcpy(_antType.back().serialnumber, antserialnum, antsernum);
1436      _antType.back().serialnumber[antsernum] = 0;
1437    }
1438  }
1439
1440  if (type == 1033) {
1441    GETSTRING(recnum, receiver)
1442    GETSTRING(recfirnum, recfirmware)
1443    GETSTRING(recsernum, recserialnum)
1444    if ((recnum > -1 && recnum < 265) &&
1445        (_recType.empty() || strncmp(_recType.back().descriptor, receiver, recnum) != 0)) {
1446      _recType.push_back(t_recInfo());
1447      memcpy(_recType.back().descriptor, receiver, recnum);
1448      _recType.back().descriptor[recnum] = 0;
1449      if (recfirnum > -1 && recfirnum < 265) {
1450        memcpy(_recType.back().firmware, recfirmware, recfirnum);
1451        _recType.back().firmware[recfirnum] = 0;
1452      }
1453      if (recsernum > -1 && recsernum < 265) {
1454        memcpy(_recType.back().serialnumber, recserialnum, recsernum);
1455        _recType.back().serialnumber[recsernum] = 0;
1456      }
1457    }
1458  }
1459  return true;
1460}
1461
1462//
1463////////////////////////////////////////////////////////////////////////////
1464bool RTCM3Decoder::DecodeAntennaPosition(unsigned char* data, int size) {
1465  int type;
1466  uint64_t numbits = 0, bitfield = 0;
1467  double x, y, z;
1468
1469  data += 3; /* header */
1470  size -= 6; /* header + crc */
1471
1472  GETBITS(type, 12)
1473  _antList.push_back(t_antRefPoint());
1474  _antList.back().type = t_antRefPoint::ARP;
1475  SKIPBITS(22)
1476  GETBITSSIGN(x, 38)
1477  _antList.back().xx = x * 1e-4;
1478  SKIPBITS(2)
1479  GETBITSSIGN(y, 38)
1480  _antList.back().yy = y * 1e-4;
1481  SKIPBITS(2)
1482  GETBITSSIGN(z, 38)
1483  _antList.back().zz = z * 1e-4;
1484  if (type == 1006)
1485      {
1486    double h;
1487    GETBITS(h, 16)
1488    _antList.back().height = h * 1e-4;
1489    _antList.back().height_f = true;
1490  }
1491  _antList.back().message = type;
1492
1493  return true;
1494}
1495
1496//
1497////////////////////////////////////////////////////////////////////////////
1498t_irc RTCM3Decoder::Decode(char* buffer, int bufLen, vector<string>& errmsg) {
1499  bool decoded = false;
1500
1501  errmsg.clear();
1502
1503  while (bufLen && _MessageSize < sizeof(_Message)) {
1504    int l = sizeof(_Message) - _MessageSize;
1505    if (l > bufLen)
1506      l = bufLen;
1507    memcpy(_Message + _MessageSize, buffer, l);
1508    _MessageSize += l;
1509    bufLen -= l;
1510    buffer += l;
1511    int id;
1512    while ((id = GetMessage())) {
1513      /* reset station ID for file loading as it can change */
1514      if (_rawFile)
1515        _staID = _rawFile->staID();
1516      /* store the id into the list of loaded blocks */
1517      _typeList.push_back(id);
1518
1519      /* SSR I+II data handled in another function, already pass the
1520       * extracted data block. That does no harm, as it anyway skip everything
1521       * else. */
1522      if ((id >= 1057 && id <= 1068) || (id >= 1240 && id <= 1270)) {
1523        if (!_coDecoders.contains(_staID.toLatin1()))
1524          _coDecoders[_staID.toLatin1()] = new RTCM3coDecoder(_staID);
1525        RTCM3coDecoder* coDecoder = _coDecoders[_staID.toLatin1()];
1526        if (coDecoder->Decode(reinterpret_cast<char *>(_Message), _BlockSize,
1527            errmsg) == success) {
1528          decoded = true;
1529        }
1530      }
1531      else if (id >= 1070 && id <= 1229) /* MSM */ {
1532        if (DecodeRTCM3MSM(_Message, _BlockSize))
1533          decoded = true;
1534      }
1535      else {
1536        switch (id) {
1537          case 1001:
1538          case 1003:
1539            emit(newMessage(
1540                QString("%1: Block %2 contain partial data! Ignored!")
1541                    .arg(_staID).arg(id).toLatin1(), true));
1542            break; /* no use decoding partial data ATM, remove break when data can be used */
1543          case 1002:
1544          case 1004:
1545            if (DecodeRTCM3GPS(_Message, _BlockSize))
1546              decoded = true;
1547            break;
1548          case 1009:
1549          case 1011:
1550            emit(newMessage(
1551                QString("%1: Block %2 contain partial data! Ignored!")
1552                    .arg(_staID).arg(id).toLatin1(), true));
1553            break; /* no use decoding partial data ATM, remove break when data can be used */
1554          case 1010:
1555          case 1012:
1556            if (DecodeRTCM3GLONASS(_Message, _BlockSize))
1557              decoded = true;
1558            break;
1559          case 1019:
1560            if (DecodeGPSEphemeris(_Message, _BlockSize))
1561              decoded = true;
1562            break;
1563          case 1020:
1564            if (DecodeGLONASSEphemeris(_Message, _BlockSize))
1565              decoded = true;
1566            break;
1567          case 1043:
1568            if (DecodeSBASEphemeris(_Message, _BlockSize))
1569              decoded = true;
1570            break;
1571          case 1044:
1572            if (DecodeQZSSEphemeris(_Message, _BlockSize))
1573              decoded = true;
1574            break;
1575          case 1045:
1576          case 1046:
1577            if (DecodeGalileoEphemeris(_Message, _BlockSize))
1578              decoded = true;
1579            break;
1580          case RTCM3ID_BDS:
1581            if (DecodeBDSEphemeris(_Message, _BlockSize))
1582              decoded = true;
1583            break;
1584          case 1007:
1585          case 1008:
1586          case 1033:
1587            DecodeAntennaReceiver(_Message, _BlockSize);
1588            break;
1589          case 1005:
1590          case 1006:
1591            DecodeAntennaPosition(_Message, _BlockSize);
1592            break;
1593        }
1594      }
1595    }
1596  }
1597  return decoded ? success : failure;
1598}
1599;
1600
1601//
1602////////////////////////////////////////////////////////////////////////////
1603uint32_t RTCM3Decoder::CRC24(long size, const unsigned char *buf) {
1604  uint32_t crc = 0;
1605  int i;
1606
1607  while (size--) {
1608    crc ^= (*buf++) << (16);
1609    for (i = 0; i < 8; i++)
1610        {
1611      crc <<= 1;
1612      if (crc & 0x1000000)
1613        crc ^= 0x01864cfb;
1614    }
1615  }
1616  return crc;
1617}
1618
1619//
1620////////////////////////////////////////////////////////////////////////////
1621int RTCM3Decoder::GetMessage(void) {
1622  unsigned char *m, *e;
1623  int i;
1624
1625  m = _Message + _SkipBytes;
1626  e = _Message + _MessageSize;
1627  _NeedBytes = _SkipBytes = 0;
1628  while (e - m >= 3) {
1629    if (m[0] == 0xD3) {
1630      _BlockSize = ((m[1] & 3) << 8) | m[2];
1631      if (e - m >= static_cast<int>(_BlockSize + 6)) {
1632        if (static_cast<uint32_t>((m[3 + _BlockSize] << 16)
1633            | (m[3 + _BlockSize + 1] << 8)
1634            | (m[3 + _BlockSize + 2])) == CRC24(_BlockSize + 3, m)) {
1635          _BlockSize += 6;
1636          _SkipBytes = _BlockSize;
1637          break;
1638        }
1639        else
1640          ++m;
1641      }
1642      else {
1643        _NeedBytes = _BlockSize;
1644        break;
1645      }
1646    }
1647    else
1648      ++m;
1649  }
1650  if (e - m < 3)
1651    _NeedBytes = 3;
1652
1653  /* copy buffer to front */
1654  i = m - _Message;
1655  if (i && m < e)
1656    memmove(_Message, m, static_cast<size_t>(_MessageSize - i));
1657  _MessageSize -= i;
1658
1659  return !_NeedBytes ? ((_Message[3] << 4) | (_Message[4] >> 4)) : 0;
1660}
1661
1662// Time of Corrections
1663//////////////////////////////////////////////////////////////////////////////
1664int RTCM3Decoder::corrGPSEpochTime() const {
1665  return
1666      _coDecoders.size() > 0 ?
1667          _coDecoders.begin().value()->corrGPSEpochTime() : -1;
1668}
Note: See TracBrowser for help on using the repository browser.