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

Last change on this file since 7477 was 7215, checked in by stuerze, 9 years ago

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