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

Last change on this file since 7755 was 7753, checked in by stuerze, 9 years ago

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