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

Last change on this file since 8470 was 8470, checked in by stuerze, 12 months ago

bug fixed to have now consistent QZSS weeks in clock and orbit broadcast ephemerides

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