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

Last change on this file since 8234 was 8234, checked in by stuerze, 18 months ago

antenna and receiver info from message type 1007, 1008 and 1033 completed

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