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

Last change on this file since 9088 was 9088, checked in by stuerze, 22 months ago

adjusted allocation of slip and LTI according to the respective RTCM version

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