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

Last change on this file was 10236, checked in by stuerze, 6 months ago

minor changes

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