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

Last change on this file since 8158 was 8158, checked in by stuerze, 20 months ago

file format is adapted to the format, which is used in the overall BNC project

File size: 49.4 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  CurrentObsTime.set(i);
112
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 = _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                qDebug() << "MSM4";
735                if (psr[count] > -1.0 / (1 << 10)) {
736                  frqObs->_code = psr[count] * LIGHTSPEED / 1000.0
737                      + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0;
738                  frqObs->_codeValid = true;
739                  qDebug() << "frqObs->_codeValid";
740                }
741
742                if (cp[count] > -1.0 / (1 << 8)) {
743                  frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl
744                      + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0
745                          / cd.wl;
746                  frqObs->_phaseValid = true;
747                  qDebug() << "frqObs->_phaseValid";
748                  frqObs->_slipCounter = ll[count];
749                }
750
751                frqObs->_snr = cnr[count];
752                frqObs->_snrValid = true;
753                qDebug() << "frqObs->_snrValid";
754                break;
755              case 5:
756                if (psr[count] > -1.0 / (1 << 10)) {
757                  frqObs->_code = psr[count] * LIGHTSPEED / 1000.0
758                      + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0;
759                  frqObs->_codeValid = true;
760                }
761
762                if (cp[count] > -1.0 / (1 << 8)) {
763                  frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl
764                      + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0
765                          / cd.wl;
766                  frqObs->_phaseValid = true;
767                  frqObs->_slipCounter = ll[count];
768                }
769
770                frqObs->_snr = cnr[count];
771                frqObs->_snrValid = true;
772
773                if (dop[count] > -1.6384) {
774                  frqObs->_doppler = -(dop[count] + rdop[numsat]) / cd.wl;
775                  frqObs->_dopplerValid = true;
776                }
777                break;
778              case 6:
779                if (psr[count] > -1.0 / (1 << 10)) {
780                  frqObs->_code = psr[count] * LIGHTSPEED / 1000.0
781                      + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0;
782                  frqObs->_codeValid = true;
783                }
784
785                if (cp[count] > -1.0 / (1 << 8)) {
786                  frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl
787                      + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0
788                          / cd.wl;
789                  frqObs->_phaseValid = true;
790                  frqObs->_slipCounter = ll[count];
791                }
792
793                frqObs->_snr = cnr[count];
794                frqObs->_snrValid = true;
795                break;
796              case 7:
797                if (psr[count] > -1.0 / (1 << 10)) {
798                  frqObs->_code = psr[count] * LIGHTSPEED / 1000.0
799                      + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0;
800                  frqObs->_codeValid = true;
801                }
802
803                if (cp[count] > -1.0 / (1 << 8)) {
804                  frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl
805                      + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0
806                          / cd.wl;
807                  frqObs->_phaseValid = true;
808                  frqObs->_slipCounter = ll[count];
809                }
810
811                frqObs->_snr = cnr[count];
812                frqObs->_snrValid = true;
813
814                if (dop[count] > -1.6384) {
815                  frqObs->_doppler = -(dop[count] + rdop[numsat]) / cd.wl;
816                  frqObs->_dopplerValid = true;
817                }
818                break;
819            }
820            CurrentObs._obs.push_back(frqObs);
821          }
822        }
823      }
824      qDebug() << CurrentObs._time.datestr().c_str() << " "
825          << CurrentObs._time.timestr().c_str();
826      qDebug() << "CurrentObs._obs.size(): " << CurrentObs._obs.size();
827      if (CurrentObs._obs.size() > 0) {
828        _CurrentObsList.push_back(CurrentObs);
829        qDebug() << "_CurrentObsList.push_back(CurrentObs)";
830      }
831    }
832  }
833  else if ((type % 10) < 3) {
834    emit(newMessage(QString("%1: Block %2 contain partial data! Ignored!")
835        .arg(_staID).arg(type).toAscii(), true));
836  }
837  if (!syncf) {
838    decoded = true;
839    _obsList = _CurrentObsList;
840    _CurrentTime.reset();
841    _CurrentObsList.clear();
842  }
843  return decoded;
844}
845
846//
847////////////////////////////////////////////////////////////////////////////
848bool RTCM3Decoder::DecodeRTCM3GLONASS(unsigned char* data, int size) {
849  bool decoded = false;
850  bncTime CurrentObsTime;
851  int i, numsats, syncf, type;
852  uint64_t numbits = 0, bitfield = 0;
853
854  data += 3; /* header */
855  size -= 6; /* header + crc */
856
857  GETBITS(type, 12)
858  SKIPBITS(12)
859  /* id */
860  GETBITS(i, 27)
861  /* tk */
862
863  CurrentObsTime.setTk(i);
864  if (_CurrentTime.valid() && CurrentObsTime != _CurrentTime) {
865    decoded = true;
866    _obsList.append(_CurrentObsList);
867    _CurrentObsList.clear();
868  }
869  _CurrentTime = CurrentObsTime;
870
871  GETBITS(syncf, 1)
872  /* sync */
873  GETBITS(numsats, 5)
874  SKIPBITS(4)
875  /* smind, smint */
876
877  while (numsats--) {
878    int sv, code, l1range, amb = 0, freq;
879    t_satObs CurrentObs;
880    CurrentObs._time = CurrentObsTime;
881
882    GETBITS(sv, 6)
883    CurrentObs._prn.set('R', sv);
884    GETBITS(code, 1)
885    GETBITS(freq, 5)
886    GLOFreq[sv - 1] = 100 + freq - 7; /* store frequency for other users (MSM) */
887
888    t_frqObs *frqObs = new t_frqObs;
889    /* L1 */
890    (code) ?
891        frqObs->_rnxType2ch.assign("1P") : frqObs->_rnxType2ch.assign("1C");
892    GETBITS(l1range, 25);
893    GETBITSSIGN(i, 20);
894    if ((i & ((1 << 20) - 1)) != 0x80000) {
895      frqObs->_code = l1range * 0.02;
896      frqObs->_phase = (l1range * 0.02 + i * 0.0005)
897          / GLO_WAVELENGTH_L1(freq - 7);
898      frqObs->_codeValid = frqObs->_phaseValid = true;
899    }
900    GETBITS(i, 7);
901    frqObs->_slipCounter = i;
902    if (type == 1010 || type == 1012) {
903      GETBITS(amb, 7);
904      if (amb) {
905        frqObs->_code += amb * 599584.916;
906        frqObs->_phase += (amb * 599584.916) / GLO_WAVELENGTH_L1(freq - 7);
907      }
908      GETBITS(i, 8);
909      if (i) {
910        frqObs->_snr = i * 0.25;
911        frqObs->_snrValid = true;
912      }
913    }
914    CurrentObs._obs.push_back(frqObs);
915    if (type == 1011 || type == 1012) {
916      frqObs = new t_frqObs;
917      /* L2 */
918      GETBITS(code, 2);
919      switch (code) {
920        case 3:
921          frqObs->_rnxType2ch.assign("2P");
922          break;
923        case 2:
924          frqObs->_rnxType2ch.assign("2P");
925          break;
926        case 1:
927          frqObs->_rnxType2ch.assign("2P");
928          break;
929        case 0:
930          frqObs->_rnxType2ch.assign("2C");
931          break;
932      }
933      GETBITSSIGN(i, 14);
934      if ((i & ((1 << 14) - 1)) != 0x2000) {
935        frqObs->_code = l1range * 0.02 + i * 0.02 + amb * 599584.916;
936        frqObs->_codeValid = true;
937      }
938      GETBITSSIGN(i, 20);
939      if ((i & ((1 << 20) - 1)) != 0x80000) {
940        frqObs->_phase = (l1range * 0.02 + i * 0.0005 + amb * 599584.916)
941            / GLO_WAVELENGTH_L2(freq - 7);
942        frqObs->_phaseValid = true;
943      }
944      GETBITS(i, 7);
945      frqObs->_slipCounter = i;
946      if (type == 1012) {
947        GETBITS(i, 8);
948        if (i) {
949          frqObs->_snr = i * 0.25;
950          frqObs->_snrValid = true;
951        }
952      }
953      CurrentObs._obs.push_back(frqObs);
954    }
955    _CurrentObsList.push_back(CurrentObs);
956  }
957  if (!syncf) {
958    decoded = true;
959    _obsList.append(_CurrentObsList);
960    _CurrentTime.reset();
961    _CurrentObsList.clear();
962  }
963  return decoded;
964}
965
966//
967////////////////////////////////////////////////////////////////////////////
968bool RTCM3Decoder::DecodeGPSEphemeris(unsigned char* data, int size) {
969  bool decoded = false;
970
971  if (size == 67) {
972    t_ephGPS eph;
973    int i, week;
974    uint64_t numbits = 0, bitfield = 0;
975
976    data += 3; /* header */
977    size -= 6; /* header + crc */
978    SKIPBITS(12)
979
980    eph._receptDateTime = currentDateAndTimeGPS();
981
982    GETBITS(i, 6)
983    eph._prn.set('G', i);
984    GETBITS(week, 10)
985    week += 1024;
986    GETBITS(i, 4)
987    eph._ura = accuracyFromIndex(i, eph.type());
988    GETBITS(eph._L2Codes, 2)
989    GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
990    GETBITS(eph._IODE, 8)
991    GETBITS(i, 16)
992    i <<= 4;
993    eph._TOC.set(i * 1000);
994    GETFLOATSIGN(eph._clock_driftrate, 8,
995        1.0 / (double )(1 << 30) / (double )(1 << 25))
996    GETFLOATSIGN(eph._clock_drift, 16,
997        1.0 / (double )(1 << 30) / (double )(1 << 13))
998    GETFLOATSIGN(eph._clock_bias, 22,
999        1.0 / (double )(1 << 30) / (double )(1 << 1))
1000    GETBITS(eph._IODC, 10)
1001    GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5))
1002    GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
1003    GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1004    GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29))
1005    GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))
1006    GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29))
1007    GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))
1008    GETBITS(i, 16)
1009    i <<= 4;
1010    eph._TOEsec = i;
1011    bncTime t;
1012    t.set(i * 1000);
1013    eph._TOEweek = t.gpsw();
1014    /* week from HOW, differs from TOC, TOE week, we use adapted value instead */
1015    if (eph._TOEweek > week + 1 || eph._TOEweek < week - 1) /* invalid week */
1016      return false;
1017    GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29))
1018    GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1019    GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29))
1020    GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1021    GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5))
1022    GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1023    GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
1024    GETFLOATSIGN(eph._TGD, 8, 1.0 / (double )(1 << 30) / (double )(1 << 1))
1025    GETBITS(eph._health, 6)
1026    GETBITS(eph._L2PFlag, 1)
1027    GETBITS(eph._fitInterval, 1)
1028    eph._TOT = 0.9999e9;
1029
1030    emit newGPSEph(eph);
1031    decoded = true;
1032  }
1033  return decoded;
1034}
1035
1036//
1037////////////////////////////////////////////////////////////////////////////
1038bool RTCM3Decoder::DecodeGLONASSEphemeris(unsigned char* data, int size) {
1039  bool decoded = false;
1040
1041  if (size == 51) {
1042    t_ephGlo eph;
1043    int sv, i, tk;
1044    uint64_t numbits = 0, bitfield = 0;
1045
1046    data += 3; /* header */
1047    size -= 6; /* header + crc */
1048    SKIPBITS(12)
1049
1050    eph._receptDateTime = currentDateAndTimeGPS();
1051
1052    GETBITS(sv, 6)
1053    eph._prn.set('R', sv);
1054
1055    GETBITS(i, 5)
1056    eph._frequency_number = i - 7;
1057    GLOFreq[sv - 1] = 100 + i - 7; /* store frequency for other users (MSM) */
1058    _gloFrq = QString("%1 %2").arg(eph._prn.toString().c_str()).arg(
1059        eph._frequency_number, 2, 'f', 0);
1060    SKIPBITS(4)
1061    /* almanac healthy, almanac health ok, P1 */
1062    GETBITS(i, 5)
1063    tk = i * 60 * 60;
1064    GETBITS(i, 6)
1065    tk += i * 60;
1066    GETBITS(i, 1)
1067    tk += i * 30;
1068    eph._tki = tk < 3 * 60 * 60 ? tk - 3 * 60 * 60 + 86400 : tk - 3 * 60 * 60;
1069    GETBITS(eph._health, 1)
1070    SKIPBITS(1)
1071    /* P2 */
1072    GETBITS(i, 7)
1073    eph._TOC.setTk(i * 15 * 60 * 1000); /* tb */
1074
1075    GETFLOATSIGNM(eph._x_velocity, 24, 1.0 / (double )(1 << 20))
1076    GETFLOATSIGNM(eph._x_pos, 27, 1.0 / (double )(1 << 11))
1077    GETFLOATSIGNM(eph._x_acceleration, 5, 1.0 / (double )(1 << 30))
1078    GETFLOATSIGNM(eph._y_velocity, 24, 1.0 / (double )(1 << 20))
1079    GETFLOATSIGNM(eph._y_pos, 27, 1.0 / (double )(1 << 11))
1080    GETFLOATSIGNM(eph._y_acceleration, 5, 1.0 / (double )(1 << 30))
1081    GETFLOATSIGNM(eph._z_velocity, 24, 1.0 / (double )(1 << 20))
1082    GETFLOATSIGNM(eph._z_pos, 27, 1.0 / (double )(1 << 11))
1083    GETFLOATSIGNM(eph._z_acceleration, 5, 1.0 / (double )(1 << 30))
1084    SKIPBITS(1)
1085    /* P3 */
1086    GETFLOATSIGNM(eph._gamma, 11, 1.0 / (double )(1 << 30) / (double )(1 << 10))
1087    SKIPBITS(3)
1088    /* GLONASS-M P, GLONASS-M ln (third string) */
1089    GETFLOATSIGNM(eph._tau, 22, 1.0 / (double )(1 << 30))
1090    /* GLONASS tau n(tb) */
1091    SKIPBITS(5)
1092    /* GLONASS-M delta tau n(tb) */
1093    GETBITS(eph._E, 5)
1094    /* GETBITS(i, 1) / * GLONASS-M P4 */
1095    /* GETBITS(i, 4) / * GLONASS-M Ft */
1096    /* GETBITS(i, 11) / * GLONASS-M Nt */
1097    /* GETBITS(i, 2) / * GLONASS-M M */
1098    /* GETBITS(i, 1) / * GLONASS-M The Availability of Additional Data */
1099    /* GETBITS(i, 11) / * GLONASS-M Na */
1100    /* GETFLOATSIGNM(i, 32, 1.0/(double)(1<<30)/(double)(1<<1)) / * GLONASS tau c */
1101    /* GETBITS(i, 5) / * GLONASS-M N4 */
1102    /* GETFLOATSIGNM(i, 22, 1.0/(double)(1<<30)) / * GLONASS-M tau GPS */
1103    /* GETBITS(i, 1) / * GLONASS-M ln (fifth string) */
1104
1105    unsigned year, month, day;
1106    eph._TOC.civil_date(year, month, day);
1107    eph._gps_utc = gnumleap(year, month, day);
1108    eph._tt = eph._TOC;
1109
1110    eph._xv(1) = eph._x_pos * 1.e3;
1111    eph._xv(2) = eph._y_pos * 1.e3;
1112    eph._xv(3) = eph._z_pos * 1.e3;
1113    eph._xv(4) = eph._x_velocity * 1.e3;
1114    eph._xv(5) = eph._y_velocity * 1.e3;
1115    eph._xv(6) = eph._z_velocity * 1.e3;
1116
1117    emit newGlonassEph(eph);
1118    decoded = true;
1119  }
1120  return decoded;
1121}
1122
1123//
1124////////////////////////////////////////////////////////////////////////////
1125bool RTCM3Decoder::DecodeQZSSEphemeris(unsigned char* data, int size) {
1126  bool decoded = false;
1127
1128  if (size == 67) {
1129    t_ephGPS eph;
1130    int i, week;
1131    uint64_t numbits = 0, bitfield = 0;
1132
1133    data += 3; /* header */
1134    size -= 6; /* header + crc */
1135    SKIPBITS(12)
1136
1137    eph._receptDateTime = currentDateAndTimeGPS();
1138
1139    GETBITS(i, 4)
1140    eph._prn.set('J', i);
1141
1142    GETBITS(i, 16)
1143    i <<= 4;
1144    eph._TOC.set(i * 1000);
1145
1146    GETFLOATSIGN(eph._clock_driftrate, 8,
1147        1.0 / (double )(1 << 30) / (double )(1 << 25))
1148    GETFLOATSIGN(eph._clock_drift, 16,
1149        1.0 / (double )(1 << 30) / (double )(1 << 13))
1150    GETFLOATSIGN(eph._clock_bias, 22,
1151        1.0 / (double )(1 << 30) / (double )(1 << 1))
1152    GETBITS(eph._IODE, 8)
1153    GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5))
1154    GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
1155    GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1156    GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29))
1157    GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))
1158    GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29))
1159    GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))
1160    GETBITS(i, 16)
1161    i <<= 4;
1162    eph._TOEsec = i;
1163    bncTime t;
1164    t.set(i);
1165
1166    GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29))
1167    GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1168    GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29))
1169    GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1170    GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5))
1171    GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1172    GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
1173    GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
1174    GETBITS(eph._L2Codes, 2)
1175    GETBITS(week, 10)
1176    week += 1024;
1177    eph._TOEweek = t.gpsw();
1178    /* week from HOW, differs from TOC, TOE week, we use adapted value instead */
1179    if (eph._TOEweek > week + 1 || eph._TOEweek < week - 1) /* invalid week */
1180      return false;
1181
1182    GETBITS(i, 4)
1183    if (i <= 6)
1184      eph._ura = ceil(10.0 * pow(2.0, 1.0 + i / 2.0)) / 10.0;
1185    else
1186      eph._ura = ceil(10.0 * pow(2.0, i / 2.0)) / 10.0;
1187    GETBITS(eph._health, 6)
1188    GETFLOATSIGN(eph._TGD, 8, 1.0 / (double )(1 << 30) / (double )(1 << 1))
1189    GETBITS(eph._IODC, 10)
1190    GETBITS(eph._fitInterval, 1)
1191    eph._TOT = 0.9999e9;
1192    eph._L2PFlag = 0; /* does not exist for QZSS */
1193
1194    emit newGPSEph(eph);
1195    decoded = true;
1196  }
1197  return decoded;
1198}
1199
1200//
1201////////////////////////////////////////////////////////////////////////////
1202bool RTCM3Decoder::DecodeSBASEphemeris(unsigned char* data, int size) {
1203  bool decoded = false;
1204
1205  if (size == 35) {
1206    t_ephSBAS eph;
1207    int i;
1208    uint64_t numbits = 0, bitfield = 0;
1209
1210    data += 3; /* header */
1211    size -= 6; /* header + crc */
1212    SKIPBITS(12)
1213
1214    eph._receptDateTime = currentDateAndTimeGPS();
1215
1216    GETBITS(i, 6)
1217    eph._prn.set('S', 20 + i);
1218    GETBITS(eph._IODN, 8)
1219    GETBITS(i, 13)
1220    i <<= 4;
1221    eph._TOC.setTOD(i * 1000);
1222    GETBITS(i, 4)
1223    eph._ura = accuracyFromIndex(i, eph.type());
1224    GETFLOATSIGN(eph._x_pos, 30, 0.08)
1225    GETFLOATSIGN(eph._y_pos, 30, 0.08)
1226    GETFLOATSIGN(eph._z_pos, 25, 0.4)
1227    GETFLOATSIGN(eph._x_velocity, 17, 0.000625)
1228    GETFLOATSIGN(eph._y_velocity, 17, 0.000625)
1229    GETFLOATSIGN(eph._z_velocity, 18, 0.004)
1230    GETFLOATSIGN(eph._x_acceleration, 10, 0.0000125)
1231    GETFLOATSIGN(eph._y_acceleration, 10, 0.0000125)
1232    GETFLOATSIGN(eph._z_acceleration, 10, 0.0000625)
1233    GETFLOATSIGN(eph._agf0, 12, 1.0 / (1 << 30) / (1 << 1))
1234    GETFLOATSIGN(eph._agf1, 8, 1.0 / (1 << 30) / (1 << 10))
1235
1236    eph._TOW = 0.9999E9;
1237    eph._health = 0;
1238
1239    emit newSBASEph(eph);
1240    decoded = true;
1241  }
1242  return decoded;
1243}
1244
1245//
1246////////////////////////////////////////////////////////////////////////////
1247bool RTCM3Decoder::DecodeGalileoEphemeris(unsigned char* data, int size) {
1248  bool decoded = false;
1249  uint64_t numbits = 0, bitfield = 0;
1250  int i;
1251
1252  data += 3; /* header */
1253  size -= 6; /* header + crc */
1254  GETBITS(i, 12)
1255
1256  if ((i == 1046 && size == 61) || (i == 1045 && size == 60)) {
1257    t_ephGal eph;
1258
1259    eph._receptDateTime = currentDateAndTimeGPS();
1260
1261    eph._inav = (i == 1046);
1262    eph._fnav = (i == 1045);
1263    GETBITS(i, 6)
1264    eph._prn.set('E', i, eph._inav ? 1 : 0);
1265
1266    GETBITS(eph._TOEweek, 12)
1267    GETBITS(eph._IODnav, 10)
1268    GETBITS(i, 8)
1269    eph._SISA = accuracyFromIndex(i, eph.type());
1270    GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
1271    GETBITSFACTOR(i, 14, 60)
1272    eph._TOC.set(1024 + eph._TOEweek, i);
1273    GETFLOATSIGN(eph._clock_driftrate, 6,
1274        1.0 / (double )(1 << 30) / (double )(1 << 29))
1275    GETFLOATSIGN(eph._clock_drift, 21,
1276        1.0 / (double )(1 << 30) / (double )(1 << 16))
1277    GETFLOATSIGN(eph._clock_bias, 31,
1278        1.0 / (double )(1 << 30) / (double )(1 << 4))
1279    GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5))
1280    GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
1281    GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1282    GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29))
1283    GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))
1284    GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29))
1285    GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))
1286    GETBITSFACTOR(eph._TOEsec, 14, 60)
1287    /* FIXME: overwrite value, copied from old code */
1288    eph._TOEsec = eph._TOC.gpssec();
1289    GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29))
1290    GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1291    GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29))
1292    GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1293    GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5))
1294    GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1295    GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
1296    GETFLOATSIGN(eph._BGD_1_5A, 10,
1297        1.0 / (double )(1 << 30) / (double )(1 << 2))
1298    if (eph._inav) {
1299      /* set unused F/NAV values */
1300      eph._E5aHS = 0.0;
1301      eph._e5aDataInValid = false;
1302
1303      GETFLOATSIGN(eph._BGD_1_5B, 10,
1304          1.0 / (double )(1 << 30) / (double )(1 << 2))
1305      GETBITS(eph._E5bHS, 2)
1306      GETBITS(eph._e5bDataInValid, 1)
1307      GETBITS(eph._E1_bHS, 2)
1308      GETBITS(eph._e1DataInValid, 1)
1309    }
1310    else {
1311      /* set unused I/NAV values */
1312      eph._BGD_1_5B = 0.0;
1313      eph._E5bHS = 0.0;
1314      eph._E1_bHS = 0.0;
1315      eph._e1DataInValid = false;
1316      eph._e5bDataInValid = false;
1317
1318      GETBITS(eph._E5aHS, 2)
1319      GETBITS(eph._e5aDataInValid, 1)
1320    }
1321    eph._TOT = 0.9999e9;
1322
1323    emit newGalileoEph(eph);
1324    decoded = true;
1325  }
1326  return decoded;
1327}
1328
1329//
1330////////////////////////////////////////////////////////////////////////////
1331bool RTCM3Decoder::DecodeBDSEphemeris(unsigned char* data, int size) {
1332  bool decoded = false;
1333
1334  if (size == 70) {
1335    t_ephBDS eph;
1336    int i;
1337    uint64_t numbits = 0, bitfield = 0;
1338
1339    data += 3; /* header */
1340    size -= 6; /* header + crc */
1341    SKIPBITS(12)
1342
1343    eph._receptDateTime = currentDateAndTimeGPS();
1344
1345    GETBITS(i, 6)
1346    eph._prn.set('C', i);
1347
1348    SKIPBITS(13)
1349    /* week */
1350    GETBITS(i, 4)
1351    eph._URA = accuracyFromIndex(i, eph.type());
1352    GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
1353    GETBITS(eph._AODE, 5)
1354    GETBITS(i, 17)
1355    i <<= 3;
1356    eph._TOC.setBDS(i * 1000);
1357    GETFLOATSIGN(eph._clock_driftrate, 11,
1358        1.0 / (double )(1 << 30) / (double )(1 << 30) / (double )(1 << 6))
1359    GETFLOATSIGN(eph._clock_drift, 22,
1360        1.0 / (double )(1 << 30) / (double )(1 << 20))
1361    GETFLOATSIGN(eph._clock_bias, 24,
1362        1.0 / (double )(1 << 30) / (double )(1 << 3))
1363    GETBITS(eph._AODC, 5)
1364    GETFLOATSIGN(eph._Crs, 18, 1.0 / (double )(1 << 6))
1365    GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
1366    GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1367    GETFLOATSIGN(eph._Cuc, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1))
1368    GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))
1369    GETFLOATSIGN(eph._Cus, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1))
1370    GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))
1371    GETBITS(i, 17)
1372    i <<= 3;
1373    eph._TOEsec = i;
1374    eph._TOE.setBDS(i * 1000);
1375    GETFLOATSIGN(eph._Cic, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1))
1376    GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1377    GETFLOATSIGN(eph._Cis, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1))
1378    GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1379    GETFLOATSIGN(eph._Crc, 18, 1.0 / (double )(1 << 6))
1380    GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1381    GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
1382    GETFLOATSIGN(eph._TGD1, 10, 0.0000000001)
1383    GETFLOATSIGN(eph._TGD2, 10, 0.0000000001)
1384    GETBITS(eph._SatH1, 1)
1385
1386    eph._TOW = 0.9999E9;
1387    emit newBDSEph(eph);
1388    decoded = true;
1389  }
1390  return decoded;
1391}
1392
1393//
1394////////////////////////////////////////////////////////////////////////////
1395bool RTCM3Decoder::DecodeAntenna(unsigned char* data, int size) {
1396  char *antenna, type[256];
1397  int antnum = -1;
1398  uint64_t numbits = 0, bitfield = 0;
1399
1400  data += 4; /* header */
1401  size -= 6; /* header + crc */
1402
1403  SKIPBITS(12)
1404  GETSTRING(antnum, antenna)
1405  if (antnum > -1 && antnum < 265) {
1406    memcpy(type, antenna, antnum);
1407    type[antnum] = 0;
1408    if (!_antType.contains(type)) {
1409      _antType.push_back(type);
1410    }
1411  }
1412  return true;
1413}
1414
1415//
1416////////////////////////////////////////////////////////////////////////////
1417bool RTCM3Decoder::DecodeAntennaPosition(unsigned char* data, int size) {
1418  int type;
1419  uint64_t numbits = 0, bitfield = 0;
1420  double x, y, z;
1421
1422  data += 3; /* header */
1423  size -= 6; /* header + crc */
1424
1425  GETBITS(type, 12)
1426  _antList.push_back(t_antInfo());
1427  _antList.back().type = t_antInfo::ARP;
1428  SKIPBITS(22)
1429  GETBITSSIGN(x, 38)
1430  _antList.back().xx = x * 1e-4;
1431  SKIPBITS(2)
1432  GETBITSSIGN(y, 38)
1433  _antList.back().yy = y * 1e-4;
1434  SKIPBITS(2)
1435  GETBITSSIGN(z, 38)
1436  _antList.back().zz = z * 1e-4;
1437  if (type == 1006)
1438      {
1439    double h;
1440    GETBITS(h, 16)
1441    _antList.back().height = h * 1e-4;
1442    _antList.back().height_f = true;
1443  }
1444  _antList.back().message = type;
1445
1446  return true;
1447}
1448
1449//
1450////////////////////////////////////////////////////////////////////////////
1451t_irc RTCM3Decoder::Decode(char* buffer, int bufLen, vector<string>& errmsg) {
1452  bool decoded = false;
1453
1454  errmsg.clear();
1455
1456  while (bufLen && _MessageSize < sizeof(_Message)) {
1457    int l = sizeof(_Message) - _MessageSize;
1458    if (l > bufLen)
1459      l = bufLen;
1460    memcpy(_Message + _MessageSize, buffer, l);
1461    _MessageSize += l;
1462    bufLen -= l;
1463    buffer += l;
1464    int id;
1465    while ((id = GetMessage())) {
1466      /* reset station ID for file loading as it can change */
1467      if (_rawFile)
1468        _staID = _rawFile->staID();
1469      /* store the id into the list of loaded blocks */
1470      _typeList.push_back(id);
1471
1472      /* SSR I+II data handled in another function, already pass the
1473       * extracted data block. That does no harm, as it anyway skip everything
1474       * else. */
1475      if ((id >= 1057 && id <= 1068) || (id >= 1240 && id <= 1270)) {
1476        if (!_coDecoders.contains(_staID.toAscii()))
1477          _coDecoders[_staID.toAscii()] = new RTCM3coDecoder(_staID);
1478        RTCM3coDecoder* coDecoder = _coDecoders[_staID.toAscii()];
1479        if (coDecoder->Decode(reinterpret_cast<char *>(_Message), _BlockSize,
1480            errmsg) == success) {
1481          decoded = true;
1482        }
1483      }
1484      else if (id >= 1070 && id <= 1229) /* MSM */ {
1485        if (DecodeRTCM3MSM(_Message, _BlockSize))
1486          decoded = true;
1487      }
1488      else {
1489        switch (id) {
1490          case 1001:
1491          case 1003:
1492            emit(newMessage(
1493                QString("%1: Block %2 contain partial data! Ignored!")
1494                    .arg(_staID).arg(id).toAscii(), true));
1495            break; /* no use decoding partial data ATM, remove break when data can be used */
1496          case 1002:
1497          case 1004:
1498            if (DecodeRTCM3GPS(_Message, _BlockSize))
1499              decoded = true;
1500            break;
1501          case 1009:
1502          case 1011:
1503            emit(newMessage(
1504                QString("%1: Block %2 contain partial data! Ignored!")
1505                    .arg(_staID).arg(id).toAscii(), true));
1506            break; /* no use decoding partial data ATM, remove break when data can be used */
1507          case 1010:
1508          case 1012:
1509            if (DecodeRTCM3GLONASS(_Message, _BlockSize))
1510              decoded = true;
1511            break;
1512          case 1019:
1513            if (DecodeGPSEphemeris(_Message, _BlockSize))
1514              decoded = true;
1515            break;
1516          case 1020:
1517            if (DecodeGLONASSEphemeris(_Message, _BlockSize))
1518              decoded = true;
1519            break;
1520          case 1043:
1521            if (DecodeSBASEphemeris(_Message, _BlockSize))
1522              decoded = true;
1523            break;
1524          case 1044:
1525            if (DecodeQZSSEphemeris(_Message, _BlockSize))
1526              decoded = true;
1527            break;
1528          case 1045:
1529          case 1046:
1530            if (DecodeGalileoEphemeris(_Message, _BlockSize))
1531              decoded = true;
1532            break;
1533          case RTCM3ID_BDS:
1534            if (DecodeBDSEphemeris(_Message, _BlockSize))
1535              decoded = true;
1536            break;
1537          case 1007:
1538          case 1008:
1539          case 1033:
1540            DecodeAntenna(_Message, _BlockSize);
1541            break;
1542          case 1005:
1543          case 1006:
1544            DecodeAntennaPosition(_Message, _BlockSize);
1545            break;
1546        }
1547      }
1548    }
1549  }
1550  return decoded ? success : failure;
1551}
1552;
1553
1554//
1555////////////////////////////////////////////////////////////////////////////
1556uint32_t RTCM3Decoder::CRC24(long size, const unsigned char *buf) {
1557  uint32_t crc = 0;
1558  int i;
1559
1560  while (size--) {
1561    crc ^= (*buf++) << (16);
1562    for (i = 0; i < 8; i++)
1563        {
1564      crc <<= 1;
1565      if (crc & 0x1000000)
1566        crc ^= 0x01864cfb;
1567    }
1568  }
1569  return crc;
1570}
1571
1572//
1573////////////////////////////////////////////////////////////////////////////
1574int RTCM3Decoder::GetMessage(void) {
1575  unsigned char *m, *e;
1576  int i;
1577
1578  m = _Message + _SkipBytes;
1579  e = _Message + _MessageSize;
1580  _NeedBytes = _SkipBytes = 0;
1581  while (e - m >= 3) {
1582    if (m[0] == 0xD3) {
1583      _BlockSize = ((m[1] & 3) << 8) | m[2];
1584      if (e - m >= static_cast<int>(_BlockSize + 6)) {
1585        if (static_cast<uint32_t>((m[3 + _BlockSize] << 16)
1586            | (m[3 + _BlockSize + 1] << 8)
1587            | (m[3 + _BlockSize + 2])) == CRC24(_BlockSize + 3, m)) {
1588          _BlockSize += 6;
1589          _SkipBytes = _BlockSize;
1590          break;
1591        }
1592        else
1593          ++m;
1594      }
1595      else {
1596        _NeedBytes = _BlockSize;
1597        break;
1598      }
1599    }
1600    else
1601      ++m;
1602  }
1603  if (e - m < 3)
1604    _NeedBytes = 3;
1605
1606  /* copy buffer to front */
1607  i = m - _Message;
1608  if (i && m < e)
1609    memmove(_Message, m, static_cast<size_t>(_MessageSize - i));
1610  _MessageSize -= i;
1611
1612  return !_NeedBytes ? ((_Message[3] << 4) | (_Message[4] >> 4)) : 0;
1613}
1614
1615// Time of Corrections
1616//////////////////////////////////////////////////////////////////////////////
1617int RTCM3Decoder::corrGPSEpochTime() const {
1618  return
1619      _coDecoders.size() > 0 ?
1620          _coDecoders.begin().value()->corrGPSEpochTime() : -1;
1621}
Note: See TracBrowser for help on using the repository browser.