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

Last change on this file since 8616 was 8616, 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: 51.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).toAscii(), true));
843  }
844  if (!syncf) {
845    decoded = true;
846    _obsList.append(_CurrentObsList);
847    _CurrentTime.reset();
848    _CurrentObsList.clear();
849  }
850  return decoded;
851}
852
853//
854////////////////////////////////////////////////////////////////////////////
855bool RTCM3Decoder::DecodeRTCM3GLONASS(unsigned char* data, int size) {
856  bool decoded = false;
857  bncTime CurrentObsTime;
858  int i, numsats, syncf, type;
859  uint64_t numbits = 0, bitfield = 0;
860
861  data += 3; /* header */
862  size -= 6; /* header + crc */
863
864  GETBITS(type, 12)
865  SKIPBITS(12)
866  /* id */
867  GETBITS(i, 27)
868  /* tk */
869
870  CurrentObsTime.setTk(i);
871  if (_CurrentTime.valid() && CurrentObsTime != _CurrentTime) {
872    decoded = true;
873    _obsList.append(_CurrentObsList);
874    _CurrentObsList.clear();
875  }
876  _CurrentTime = CurrentObsTime;
877
878  GETBITS(syncf, 1)
879  /* sync */
880  GETBITS(numsats, 5)
881  SKIPBITS(4)
882  /* smind, smint */
883
884  while (numsats--) {
885    int sv, code, l1range, amb = 0, freq;
886    t_satObs CurrentObs;
887    CurrentObs._time = CurrentObsTime;
888
889    GETBITS(sv, 6)
890    CurrentObs._prn.set('R', sv);
891    GETBITS(code, 1)
892    GETBITS(freq, 5)
893    GLOFreq[sv - 1] = 100 + freq - 7; /* store frequency for other users (MSM) */
894
895    t_frqObs *frqObs = new t_frqObs;
896    /* L1 */
897    (code) ?
898        frqObs->_rnxType2ch.assign("1P") : frqObs->_rnxType2ch.assign("1C");
899    GETBITS(l1range, 25);
900    GETBITSSIGN(i, 20);
901    if ((i & ((1 << 20) - 1)) != 0x80000) {
902      frqObs->_code = l1range * 0.02;
903      frqObs->_phase = (l1range * 0.02 + i * 0.0005)
904          / GLO_WAVELENGTH_L1(freq - 7);
905      frqObs->_codeValid = frqObs->_phaseValid = true;
906    }
907    GETBITS(i, 7);
908    frqObs->_lockTime = lti2sec(type,i);
909    frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0 && frqObs->_phaseValid);
910    frqObs->_slipCounter = i;
911    if (type == 1010 || type == 1012) {
912      GETBITS(amb, 7);
913      if (amb) {
914        frqObs->_code += amb * 599584.916;
915        frqObs->_phase += (amb * 599584.916) / GLO_WAVELENGTH_L1(freq - 7);
916      }
917      GETBITS(i, 8);
918      if (i) {
919        frqObs->_snr = i * 0.25;
920        frqObs->_snrValid = true;
921      }
922    }
923    CurrentObs._obs.push_back(frqObs);
924    if (type == 1011 || type == 1012) {
925      frqObs = new t_frqObs;
926      /* L2 */
927      GETBITS(code, 2);
928      switch (code) {
929        case 3:
930          frqObs->_rnxType2ch.assign("2P");
931          break;
932        case 2:
933          frqObs->_rnxType2ch.assign("2P");
934          break;
935        case 1:
936          frqObs->_rnxType2ch.assign("2P");
937          break;
938        case 0:
939          frqObs->_rnxType2ch.assign("2C");
940          break;
941      }
942      GETBITSSIGN(i, 14);
943      if ((i & ((1 << 14) - 1)) != 0x2000) {
944        frqObs->_code = l1range * 0.02 + i * 0.02 + amb * 599584.916;
945        frqObs->_codeValid = true;
946      }
947      GETBITSSIGN(i, 20);
948      if ((i & ((1 << 20) - 1)) != 0x80000) {
949        frqObs->_phase = (l1range * 0.02 + i * 0.0005 + amb * 599584.916)
950            / GLO_WAVELENGTH_L2(freq - 7);
951        frqObs->_phaseValid = true;
952      }
953      GETBITS(i, 7);
954      frqObs->_lockTime = lti2sec(type,i);
955      frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0 && frqObs->_phaseValid);
956      frqObs->_slipCounter = i;
957      if (type == 1012) {
958        GETBITS(i, 8);
959        if (i) {
960          frqObs->_snr = i * 0.25;
961          frqObs->_snrValid = true;
962        }
963      }
964      CurrentObs._obs.push_back(frqObs);
965    }
966    _CurrentObsList.push_back(CurrentObs);
967  }
968  if (!syncf) {
969    decoded = true;
970    _obsList.append(_CurrentObsList);
971    _CurrentTime.reset();
972    _CurrentObsList.clear();
973  }
974  return decoded;
975}
976
977//
978////////////////////////////////////////////////////////////////////////////
979bool RTCM3Decoder::DecodeGPSEphemeris(unsigned char* data, int size) {
980  bool decoded = false;
981
982  if (size == 67) {
983    t_ephGPS eph;
984    int i, week;
985    uint64_t numbits = 0, bitfield = 0;
986
987    data += 3; /* header */
988    size -= 6; /* header + crc */
989    SKIPBITS(12)
990
991    eph._receptDateTime = currentDateAndTimeGPS();
992
993    GETBITS(i, 6)
994    eph._prn.set('G', i);
995    GETBITS(week, 10)
996    GETBITS(i, 4)
997    eph._ura = accuracyFromIndex(i, eph.type());
998    GETBITS(eph._L2Codes, 2)
999    GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
1000    GETBITS(eph._IODE, 8)
1001    GETBITS(i, 16)
1002    i <<= 4;
1003    eph._TOC.set(i * 1000);
1004    GETFLOATSIGN(eph._clock_driftrate, 8,
1005        1.0 / (double )(1 << 30) / (double )(1 << 25))
1006    GETFLOATSIGN(eph._clock_drift, 16,
1007        1.0 / (double )(1 << 30) / (double )(1 << 13))
1008    GETFLOATSIGN(eph._clock_bias, 22,
1009        1.0 / (double )(1 << 30) / (double )(1 << 1))
1010    GETBITS(eph._IODC, 10)
1011    GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5))
1012    GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
1013    GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1014    GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29))
1015    GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))
1016    GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29))
1017    GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))
1018    GETBITS(i, 16)
1019    i <<= 4;
1020    eph._TOEsec = i;
1021    bncTime t;
1022    t.set(i * 1000);
1023    eph._TOEweek = t.gpsw();
1024    int numOfRollOvers = int(floor(t.gpsw()/1024.0));
1025    week += (numOfRollOvers * 1024);
1026    /* week from HOW, differs from TOC, TOE week, we use adapted value instead */
1027    if (eph._TOEweek > week + 1 || eph._TOEweek < week - 1) /* invalid week */
1028      return false;
1029    GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29))
1030    GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1031    GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29))
1032    GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1033    GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5))
1034    GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1035    GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
1036    GETFLOATSIGN(eph._TGD, 8, 1.0 / (double )(1 << 30) / (double )(1 << 1))
1037    GETBITS(eph._health, 6)
1038    GETBITS(eph._L2PFlag, 1)
1039    GETBITS(eph._fitInterval, 1)
1040    eph._TOT = 0.9999e9;
1041
1042    emit newGPSEph(eph);
1043    decoded = true;
1044  }
1045  return decoded;
1046}
1047
1048//
1049////////////////////////////////////////////////////////////////////////////
1050bool RTCM3Decoder::DecodeGLONASSEphemeris(unsigned char* data, int size) {
1051  bool decoded = false;
1052
1053  if (size == 51) {
1054    t_ephGlo eph;
1055    int sv, i, tk;
1056    uint64_t numbits = 0, bitfield = 0;
1057
1058    data += 3; /* header */
1059    size -= 6; /* header + crc */
1060    SKIPBITS(12)
1061
1062    eph._receptDateTime = currentDateAndTimeGPS();
1063
1064    GETBITS(sv, 6)
1065    eph._prn.set('R', sv);
1066
1067    GETBITS(i, 5)
1068    eph._frequency_number = i - 7;
1069    GETBITS(eph._almanac_health, 1) /* almanac healthy */
1070    GETBITS(eph._almanac_health_availablility_indicator, 1) /* almanac health ok */
1071    GETBITS(eph._P1, 2) /*  P1 */
1072    GETBITS(i, 5)
1073    tk = i * 60 * 60;
1074    GETBITS(i, 6)
1075    tk += i * 60;
1076    GETBITS(i, 1)
1077    tk += i * 30;
1078    eph._tki = tk < 3 * 60 * 60 ? tk - 3 * 60 * 60 + 86400 : tk - 3 * 60 * 60;
1079    GETBITS(eph._health, 1) /* MSB of Bn*/
1080    GETBITS(eph._P2, 1)  /* P2 */
1081    GETBITS(i, 7)
1082    eph._TOC.setTk(i * 15 * 60 * 1000); /* tb */
1083
1084    GETFLOATSIGNM(eph._x_velocity, 24, 1.0 / (double )(1 << 20))
1085    GETFLOATSIGNM(eph._x_pos, 27, 1.0 / (double )(1 << 11))
1086    GETFLOATSIGNM(eph._x_acceleration, 5, 1.0 / (double )(1 << 30))
1087    GETFLOATSIGNM(eph._y_velocity, 24, 1.0 / (double )(1 << 20))
1088    GETFLOATSIGNM(eph._y_pos, 27, 1.0 / (double )(1 << 11))
1089    GETFLOATSIGNM(eph._y_acceleration, 5, 1.0 / (double )(1 << 30))
1090    GETFLOATSIGNM(eph._z_velocity, 24, 1.0 / (double )(1 << 20))
1091    GETFLOATSIGNM(eph._z_pos, 27, 1.0 / (double )(1 << 11))
1092    GETFLOATSIGNM(eph._z_acceleration, 5, 1.0 / (double )(1 << 30))
1093
1094    eph._xv(1) = eph._x_pos * 1.e3;
1095    eph._xv(2) = eph._y_pos * 1.e3;
1096    eph._xv(3) = eph._z_pos * 1.e3;
1097    eph._xv(4) = eph._x_velocity * 1.e3;
1098    eph._xv(5) = eph._y_velocity * 1.e3;
1099    eph._xv(6) = eph._z_velocity * 1.e3;
1100
1101    GETBITS(eph._P3, 1)    /* P3 */
1102    GETFLOATSIGNM(eph._gamma, 11, 1.0 / (double )(1 << 30) / (double )(1 << 10))
1103    GETBITS(eph._M_P, 2) /* GLONASS-M P, */
1104    GETBITS(eph._M_l3, 1) /*GLONASS-M ln (third string) */
1105    GETFLOATSIGNM(eph._tau, 22, 1.0 / (double )(1 << 30)) /* GLONASS tau n(tb) */
1106    GETFLOATSIGNM(eph._M_delta_tau, 5, 1.0 / (double )(1 << 30))  /* GLONASS-M delta tau n(tb) */
1107    GETBITS(eph._E, 5)
1108    GETBITS(eph._M_P4, 1) /* GLONASS-M P4 */
1109    GETBITS(eph._M_FT, 4) /* GLONASS-M Ft */
1110    GETBITS(eph._M_NT, 11) /* GLONASS-M Nt */
1111    GETBITS(eph._M_M, 2) /* GLONASS-M M */
1112    GETBITS(eph._additional_data_availability, 1) /* GLONASS-M The Availability of Additional Data */
1113    GETBITS(eph._NA, 11) /* GLONASS-M Na */
1114    GETFLOATSIGNM(eph._tauC, 32, 1.0/(double)(1<<30)/(double)(1<<1)) /* GLONASS tau c */
1115    GETBITS(eph._M_N4, 5) /* GLONASS-M N4 */
1116    GETFLOATSIGNM(eph._M_tau_GPS, 22, 1.0/(double)(1<<30)) /* GLONASS-M tau GPS */
1117    GETBITS(eph._M_l5, 1) /* GLONASS-M ln (fifth string) */
1118
1119    unsigned year, month, day;
1120    eph._TOC.civil_date(year, month, day);
1121    eph._gps_utc = gnumleap(year, month, day);
1122    eph._tt = eph._TOC;
1123
1124    GLOFreq[sv - 1] = 100 + eph._frequency_number ; /* store frequency for other users (MSM) */
1125    _gloFrq = QString("%1 %2").arg(eph._prn.toString().c_str()).arg(eph._frequency_number, 2, 'f', 0);
1126
1127    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, anttype[256];
1407  char *dummy;
1408  char *receiver, rectype[256];
1409
1410  int type;
1411
1412  int dnum = -1;
1413  int antnum = -1;
1414  int recnum = -1;
1415  uint64_t numbits = 0, bitfield = 0;
1416
1417  data += 3; /* header*/
1418  size -= 6; /* header + crc */
1419
1420  GETBITS(type, 12)
1421  SKIPBITS(12)
1422  GETSTRING(antnum, antenna)
1423  if (antnum > -1 && antnum < 265) {
1424    memcpy(anttype, antenna, antnum);
1425    anttype[antnum] = 0;
1426    if (!_antType.contains(anttype)) {
1427      _antType.push_back(anttype);
1428    }
1429  }
1430  if (type == 1033) {
1431    SKIPBITS(8)
1432    GETSTRING(dnum, dummy)
1433    GETSTRING(recnum, receiver)
1434    if (recnum > -1 && recnum < 265) {
1435      memcpy(rectype, receiver, recnum);
1436      rectype[recnum] = 0;
1437      if (!_recType.contains(rectype)) {
1438        _recType.push_back(rectype);
1439      }
1440    }
1441  }
1442  return true;
1443}
1444
1445//
1446////////////////////////////////////////////////////////////////////////////
1447bool RTCM3Decoder::DecodeAntennaPosition(unsigned char* data, int size) {
1448  int type;
1449  uint64_t numbits = 0, bitfield = 0;
1450  double x, y, z;
1451
1452  data += 3; /* header */
1453  size -= 6; /* header + crc */
1454
1455  GETBITS(type, 12)
1456  _antList.push_back(t_antInfo());
1457  _antList.back().type = t_antInfo::ARP;
1458  SKIPBITS(22)
1459  GETBITSSIGN(x, 38)
1460  _antList.back().xx = x * 1e-4;
1461  SKIPBITS(2)
1462  GETBITSSIGN(y, 38)
1463  _antList.back().yy = y * 1e-4;
1464  SKIPBITS(2)
1465  GETBITSSIGN(z, 38)
1466  _antList.back().zz = z * 1e-4;
1467  if (type == 1006)
1468      {
1469    double h;
1470    GETBITS(h, 16)
1471    _antList.back().height = h * 1e-4;
1472    _antList.back().height_f = true;
1473  }
1474  _antList.back().message = type;
1475
1476  return true;
1477}
1478
1479//
1480////////////////////////////////////////////////////////////////////////////
1481t_irc RTCM3Decoder::Decode(char* buffer, int bufLen, vector<string>& errmsg) {
1482  bool decoded = false;
1483
1484  errmsg.clear();
1485
1486  while (bufLen && _MessageSize < sizeof(_Message)) {
1487    int l = sizeof(_Message) - _MessageSize;
1488    if (l > bufLen)
1489      l = bufLen;
1490    memcpy(_Message + _MessageSize, buffer, l);
1491    _MessageSize += l;
1492    bufLen -= l;
1493    buffer += l;
1494    int id;
1495    while ((id = GetMessage())) {
1496      /* reset station ID for file loading as it can change */
1497      if (_rawFile)
1498        _staID = _rawFile->staID();
1499      /* store the id into the list of loaded blocks */
1500      _typeList.push_back(id);
1501
1502      /* SSR I+II data handled in another function, already pass the
1503       * extracted data block. That does no harm, as it anyway skip everything
1504       * else. */
1505      if ((id >= 1057 && id <= 1068) || (id >= 1240 && id <= 1270)) {
1506        if (!_coDecoders.contains(_staID.toAscii()))
1507          _coDecoders[_staID.toAscii()] = new RTCM3coDecoder(_staID);
1508        RTCM3coDecoder* coDecoder = _coDecoders[_staID.toAscii()];
1509        if (coDecoder->Decode(reinterpret_cast<char *>(_Message), _BlockSize,
1510            errmsg) == success) {
1511          decoded = true;
1512        }
1513      }
1514      else if (id >= 1070 && id <= 1229) /* MSM */ {
1515        if (DecodeRTCM3MSM(_Message, _BlockSize))
1516          decoded = true;
1517      }
1518      else {
1519        switch (id) {
1520          case 1001:
1521          case 1003:
1522            emit(newMessage(
1523                QString("%1: Block %2 contain partial data! Ignored!")
1524                    .arg(_staID).arg(id).toAscii(), true));
1525            break; /* no use decoding partial data ATM, remove break when data can be used */
1526          case 1002:
1527          case 1004:
1528            if (DecodeRTCM3GPS(_Message, _BlockSize))
1529              decoded = true;
1530            break;
1531          case 1009:
1532          case 1011:
1533            emit(newMessage(
1534                QString("%1: Block %2 contain partial data! Ignored!")
1535                    .arg(_staID).arg(id).toAscii(), true));
1536            break; /* no use decoding partial data ATM, remove break when data can be used */
1537          case 1010:
1538          case 1012:
1539            if (DecodeRTCM3GLONASS(_Message, _BlockSize))
1540              decoded = true;
1541            break;
1542          case 1019:
1543            if (DecodeGPSEphemeris(_Message, _BlockSize))
1544              decoded = true;
1545            break;
1546          case 1020:
1547            if (DecodeGLONASSEphemeris(_Message, _BlockSize))
1548              decoded = true;
1549            break;
1550          case 1043:
1551            if (DecodeSBASEphemeris(_Message, _BlockSize))
1552              decoded = true;
1553            break;
1554          case 1044:
1555            if (DecodeQZSSEphemeris(_Message, _BlockSize))
1556              decoded = true;
1557            break;
1558          case 1045:
1559          case 1046:
1560            if (DecodeGalileoEphemeris(_Message, _BlockSize))
1561              decoded = true;
1562            break;
1563          case RTCM3ID_BDS:
1564            if (DecodeBDSEphemeris(_Message, _BlockSize))
1565              decoded = true;
1566            break;
1567          case 1007:
1568          case 1008:
1569          case 1033:
1570            DecodeAntennaReceiver(_Message, _BlockSize);
1571            break;
1572          case 1005:
1573          case 1006:
1574            DecodeAntennaPosition(_Message, _BlockSize);
1575            break;
1576        }
1577      }
1578    }
1579  }
1580  return decoded ? success : failure;
1581}
1582;
1583
1584//
1585////////////////////////////////////////////////////////////////////////////
1586uint32_t RTCM3Decoder::CRC24(long size, const unsigned char *buf) {
1587  uint32_t crc = 0;
1588  int i;
1589
1590  while (size--) {
1591    crc ^= (*buf++) << (16);
1592    for (i = 0; i < 8; i++)
1593        {
1594      crc <<= 1;
1595      if (crc & 0x1000000)
1596        crc ^= 0x01864cfb;
1597    }
1598  }
1599  return crc;
1600}
1601
1602//
1603////////////////////////////////////////////////////////////////////////////
1604int RTCM3Decoder::GetMessage(void) {
1605  unsigned char *m, *e;
1606  int i;
1607
1608  m = _Message + _SkipBytes;
1609  e = _Message + _MessageSize;
1610  _NeedBytes = _SkipBytes = 0;
1611  while (e - m >= 3) {
1612    if (m[0] == 0xD3) {
1613      _BlockSize = ((m[1] & 3) << 8) | m[2];
1614      if (e - m >= static_cast<int>(_BlockSize + 6)) {
1615        if (static_cast<uint32_t>((m[3 + _BlockSize] << 16)
1616            | (m[3 + _BlockSize + 1] << 8)
1617            | (m[3 + _BlockSize + 2])) == CRC24(_BlockSize + 3, m)) {
1618          _BlockSize += 6;
1619          _SkipBytes = _BlockSize;
1620          break;
1621        }
1622        else
1623          ++m;
1624      }
1625      else {
1626        _NeedBytes = _BlockSize;
1627        break;
1628      }
1629    }
1630    else
1631      ++m;
1632  }
1633  if (e - m < 3)
1634    _NeedBytes = 3;
1635
1636  /* copy buffer to front */
1637  i = m - _Message;
1638  if (i && m < e)
1639    memmove(_Message, m, static_cast<size_t>(_MessageSize - i));
1640  _MessageSize -= i;
1641
1642  return !_NeedBytes ? ((_Message[3] << 4) | (_Message[4] >> 4)) : 0;
1643}
1644
1645// Time of Corrections
1646//////////////////////////////////////////////////////////////////////////////
1647int RTCM3Decoder::corrGPSEpochTime() const {
1648  return
1649      _coDecoders.size() > 0 ?
1650          _coDecoders.begin().value()->corrGPSEpochTime() : -1;
1651}
Note: See TracBrowser for help on using the repository browser.