source: ntrip/trunk/BNC/src/RTCM3/RTCM3coDecoder.cpp@ 6486

Last change on this file since 6486 was 6486, checked in by mervart, 9 years ago
File size: 17.1 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: RTCM3coDecoder
30 *
31 * Purpose: RTCM3 Clock Orbit Decoder
32 *
33 * Author: L. Mervart
34 *
35 * Created: 05-May-2008
36 *
37 * Changes:
38 *
39 * -----------------------------------------------------------------------*/
40
41#include <stdio.h>
42#include <math.h>
43
44#include "RTCM3coDecoder.h"
45#include "bncutils.h"
46#include "bncrinex.h"
47#include "bnccore.h"
48#include "bncsettings.h"
49#include "rtcm3torinex.h"
50#include "bnctime.h"
51
52using namespace std;
53
54// Constructor
55////////////////////////////////////////////////////////////////////////////
56RTCM3coDecoder::RTCM3coDecoder(const QString& staID) {
57
58 _staID = staID;
59
60 // File Output
61 // -----------
62 bncSettings settings;
63 QString path = settings.value("corrPath").toString();
64 if (!path.isEmpty()) {
65 expandEnvVar(path);
66 if ( path.length() > 0 && path[path.length()-1] != QDir::separator() ) {
67 path += QDir::separator();
68 }
69 _fileNameSkl = path + staID;
70 }
71 _out = 0;
72
73 connect(this, SIGNAL(newOrbCorrections(QList<t_orbCorr>)),
74 BNC_CORE, SLOT(slotNewOrbCorrections(QList<t_orbCorr>)));
75
76 connect(this, SIGNAL(newClkCorrections(QList<t_clkCorr>)),
77 BNC_CORE, SLOT(slotNewClkCorrections(QList<t_clkCorr>)));
78
79 connect(this, SIGNAL(newCodeBiases(QList<t_satCodeBias>)),
80 BNC_CORE, SLOT(slotNewCodeBiases(QList<t_satCodeBias>)));
81
82 connect(this, SIGNAL(newPhaseBiases(QList<t_satPhaseBias>)),
83 BNC_CORE, SLOT(slotNewPhaseBiases(QList<t_satPhaseBias>)));
84
85 connect(this, SIGNAL(newTec(t_vTec)),
86 BNC_CORE, SLOT(slotNewTec(t_vTec)));
87
88 connect(this, SIGNAL(providerIDChanged(QString)),
89 BNC_CORE, SIGNAL(providerIDChanged(QString)));
90
91 connect(this, SIGNAL(newMessage(QByteArray,bool)),
92 BNC_CORE, SLOT(slotMessage(const QByteArray,bool)));
93
94 reset();
95
96 _providerID[0] = -1;
97 _providerID[1] = -1;
98 _providerID[2] = -1;
99}
100
101// Destructor
102////////////////////////////////////////////////////////////////////////////
103RTCM3coDecoder::~RTCM3coDecoder() {
104 delete _out;
105}
106
107//
108////////////////////////////////////////////////////////////////////////////
109void RTCM3coDecoder::reset() {
110 memset(&_clkOrb, 0, sizeof(_clkOrb));
111 memset(&_codeBias, 0, sizeof(_codeBias));
112 memset(&_phaseBias, 0, sizeof(_phaseBias));
113 memset(&_vTEC, 0, sizeof(_vTEC));
114}
115
116// Reopen Output File
117////////////////////////////////////////////////////////////////////////
118void RTCM3coDecoder::reopen() {
119
120 if (!_fileNameSkl.isEmpty()) {
121
122 bncSettings settings;
123
124 QDateTime datTim = currentDateAndTimeGPS();
125
126 QString hlpStr = bncRinex::nextEpochStr(datTim,
127 settings.value("corrIntr").toString());
128
129 QString fileNameHlp = _fileNameSkl
130 + QString("%1").arg(datTim.date().dayOfYear(), 3, 10, QChar('0'))
131 + hlpStr + datTim.toString(".yyC");
132
133 if (_fileName == fileNameHlp) {
134 return;
135 }
136 else {
137 _fileName = fileNameHlp;
138 }
139
140 delete _out;
141 if ( Qt::CheckState(settings.value("rnxAppend").toInt()) == Qt::Checked) {
142 _out = new ofstream( _fileName.toAscii().data(), ios_base::out | ios_base::app );
143 }
144 else {
145 _out = new ofstream( _fileName.toAscii().data() );
146 }
147 }
148}
149
150//
151////////////////////////////////////////////////////////////////////////////
152t_irc RTCM3coDecoder::Decode(char* buffer, int bufLen, vector<string>& errmsg) {
153
154 errmsg.clear();
155
156 _buffer.append(QByteArray(buffer,bufLen));
157
158 t_irc retCode = failure;
159
160 while(_buffer.size()) {
161
162 struct ClockOrbit clkOrbSav;
163 struct CodeBias codeBiasSav;
164 struct PhaseBias phaseBiasSav;
165 struct VTEC vTECSav;
166 memcpy(&clkOrbSav, &_clkOrb, sizeof(clkOrbSav)); // save state
167 memcpy(&codeBiasSav, &_codeBias, sizeof(codeBiasSav));
168 memcpy(&phaseBiasSav, &_phaseBias, sizeof(phaseBiasSav));
169 memcpy(&vTECSav, &_vTEC, sizeof(vTECSav));
170
171 int bytesused = 0;
172 GCOB_RETURN irc = GetSSR(&_clkOrb, &_codeBias, &_vTEC, &_phaseBias,
173 _buffer.data(), _buffer.size(), &bytesused);
174
175 if (irc <= -30) { // not enough data - restore state and exit loop
176 memcpy(&_clkOrb, &clkOrbSav, sizeof(clkOrbSav));
177 memcpy(&_codeBias, &codeBiasSav, sizeof(codeBiasSav));
178 memcpy(&_phaseBias, &phaseBiasSav, sizeof(phaseBiasSav));
179 memcpy(&_vTEC, &vTECSav, sizeof(vTECSav));
180 break;
181 }
182
183 else if (irc < 0) { // error - skip 1 byte and retry
184 reset();
185 _buffer = _buffer.mid(bytesused ? bytesused : 1);
186 }
187
188 else { // OK or MESSAGEFOLLOWS
189 _buffer = _buffer.mid(bytesused);
190
191 if (irc == GCOBR_OK || irc == GCOBR_MESSAGEFOLLOWS ) {
192
193 setEpochTime(); // sets _lastTime
194
195 if (_lastTime.valid()) {
196 reopen();
197 checkProviderID();
198 sendResults();
199 retCode = success;
200 }
201 else {
202 retCode = failure;
203 }
204
205 reset();
206 }
207 }
208 }
209
210 return retCode;
211}
212
213//
214////////////////////////////////////////////////////////////////////////////
215void RTCM3coDecoder::sendResults() {
216
217 QList<t_orbCorr>& orbCorrections = _orbCorrections[_lastTime];
218 QList<t_clkCorr>& clkCorrections = _clkCorrections[_lastTime];
219 QList<t_satCodeBias>& codeBiases = _codeBiases[_lastTime];
220
221 // Orbit and clock corrections of all satellites
222 // ---------------------------------------------
223 for (unsigned ii = 0; ii < CLOCKORBIT_NUMGPS + _clkOrb.NumberOfSat[CLOCKORBIT_SATGLONASS]; ii++) {
224 char sysCh = ' ';
225 if (ii < _clkOrb.NumberOfSat[CLOCKORBIT_SATGPS]) {
226 sysCh = 'G';
227 }
228 else if (ii >= CLOCKORBIT_NUMGPS) {
229 sysCh = 'R';
230 }
231 else {
232 continue;
233 }
234
235 // Orbit correction
236 // ----------------
237 if ( _clkOrb.messageType == COTYPE_GPSCOMBINED ||
238 _clkOrb.messageType == COTYPE_GLONASSCOMBINED ||
239 _clkOrb.messageType == COTYPE_GPSORBIT ||
240 _clkOrb.messageType == COTYPE_GLONASSORBIT ) {
241
242 t_orbCorr orbCorr;
243 orbCorr._prn.set(sysCh, _clkOrb.Sat[ii].ID);
244 orbCorr._staID = _staID.toAscii().data();
245 orbCorr._iod = _clkOrb.Sat[ii].IOD;
246 orbCorr._time = _lastTime;
247 orbCorr._system = 'R';
248 orbCorr._xr[0] = _clkOrb.Sat[ii].Orbit.DeltaRadial;
249 orbCorr._xr[1] = _clkOrb.Sat[ii].Orbit.DeltaAlongTrack;
250 orbCorr._xr[2] = _clkOrb.Sat[ii].Orbit.DeltaCrossTrack;
251 orbCorr._dotXr[0] = _clkOrb.Sat[ii].Orbit.DotDeltaRadial;
252 orbCorr._dotXr[1] = _clkOrb.Sat[ii].Orbit.DotDeltaAlongTrack;
253 orbCorr._dotXr[2] = _clkOrb.Sat[ii].Orbit.DotDeltaCrossTrack;
254
255 orbCorrections.push_back(orbCorr);
256
257 _IODs[orbCorr._prn] = _clkOrb.Sat[ii].IOD;
258 }
259
260 // Clock Corrections
261 // -----------------
262 if ( _clkOrb.messageType == COTYPE_GPSCOMBINED ||
263 _clkOrb.messageType == COTYPE_GLONASSCOMBINED ||
264 _clkOrb.messageType == COTYPE_GPSCLOCK ||
265 _clkOrb.messageType == COTYPE_GLONASSCLOCK ) {
266
267 t_clkCorr clkCorr;
268 clkCorr._prn.set(sysCh, _clkOrb.Sat[ii].ID);
269 clkCorr._staID = _staID.toAscii().data();
270 clkCorr._time = _lastTime;
271 clkCorr._dClk = _clkOrb.Sat[ii].Clock.DeltaA0 / t_CST::c;
272 clkCorr._dotDClk = _clkOrb.Sat[ii].Clock.DeltaA1 / t_CST::c;
273 clkCorr._dotDotDClk = _clkOrb.Sat[ii].Clock.DeltaA2 / t_CST::c;
274
275 _lastClkCorrections[clkCorr._prn] = clkCorr;
276
277 if (_IODs.contains(clkCorr._prn)) {
278 clkCorr._iod = _IODs[clkCorr._prn];
279 clkCorrections.push_back(clkCorr);
280 }
281 }
282
283 // High-Resolution Clocks
284 // ----------------------
285 if ( _clkOrb.messageType == COTYPE_GPSHR ||
286 _clkOrb.messageType == COTYPE_GLONASSHR ) {
287
288 t_prn prn(sysCh, _clkOrb.Sat[ii].ID);
289 if (_lastClkCorrections.contains(prn)) {
290 t_clkCorr clkCorr;
291 clkCorr = _lastClkCorrections[prn];
292 clkCorr._time = _lastTime;
293 clkCorr._dClk +=_clkOrb.Sat[ii].hrclock / t_CST::c;
294 if (_IODs.contains(clkCorr._prn)) {
295 clkCorr._iod = _IODs[clkCorr._prn];
296 clkCorrections.push_back(clkCorr);
297 }
298 }
299 }
300 }
301
302 // Code Biases
303 // -----------
304 for (unsigned ii = 0; ii < CLOCKORBIT_NUMGPS + _codeBias.NumberOfSat[CLOCKORBIT_SATGLONASS]; ii++) {
305 char sysCh = ' ';
306 if (ii < _codeBias.NumberOfSat[CLOCKORBIT_SATGPS]) {
307 sysCh = 'G';
308 }
309 else if (ii >= CLOCKORBIT_NUMGPS) {
310 sysCh = 'R';
311 }
312 else {
313 continue;
314 }
315 t_satCodeBias satCodeBias;
316 satCodeBias._prn.set(sysCh, _codeBias.Sat[ii].ID);
317 satCodeBias._staID = _staID.toAscii().data();
318 satCodeBias._time = _lastTime;
319 for (unsigned jj = 0; jj < _codeBias.Sat[ii].NumberOfCodeBiases; jj++) {
320 const CodeBias::BiasSat::CodeBiasEntry& biasEntry = _codeBias.Sat[ii].Biases[jj];
321 t_frqCodeBias frqCodeBias;
322 frqCodeBias._rnxType2ch = codeTypeToRnxType(sysCh, biasEntry.Type);
323 frqCodeBias._value = biasEntry.Bias;
324 if (!frqCodeBias._rnxType2ch.empty()) {
325 satCodeBias._bias.push_back(frqCodeBias);
326 }
327 }
328 codeBiases.push_back(satCodeBias);
329 }
330
331 // Dump all older epochs
332 // ---------------------
333 QMutableMapIterator<bncTime, QList<t_orbCorr> > itOrb(_orbCorrections);
334 while (itOrb.hasNext()) {
335 itOrb.next();
336 if (itOrb.key() < _lastTime) {
337 emit newOrbCorrections(itOrb.value());
338 t_orbCorr::writeEpoch(_out, itOrb.value());
339 itOrb.remove();
340 }
341 }
342 QMutableMapIterator<bncTime, QList<t_clkCorr> > itClk(_clkCorrections);
343 while (itClk.hasNext()) {
344 itClk.next();
345 if (itClk.key() < _lastTime) {
346 emit newClkCorrections(itClk.value());
347 t_clkCorr::writeEpoch(_out, itClk.value());
348 itClk.remove();
349 }
350 }
351 QMutableMapIterator<bncTime, QList<t_satCodeBias> > itCB(_codeBiases);
352 while (itCB.hasNext()) {
353 itCB.next();
354 if (itCB.key() < _lastTime) {
355 emit newCodeBiases(itCB.value());
356 t_satCodeBias::writeEpoch(_out, itCB.value());
357 itCB.remove();
358 }
359 }
360}
361
362//
363////////////////////////////////////////////////////////////////////////////
364void RTCM3coDecoder::checkProviderID() {
365
366 if (_clkOrb.SSRProviderID == 0 && _clkOrb.SSRSolutionID == 0 && _clkOrb.SSRIOD == 0) {
367 return;
368 }
369
370 int newProviderID[3];
371 newProviderID[0] = _clkOrb.SSRProviderID;
372 newProviderID[1] = _clkOrb.SSRSolutionID;
373 newProviderID[2] = _clkOrb.SSRIOD;
374
375 bool alreadySet = false;
376 bool different = false;
377
378 for (unsigned ii = 0; ii < 3; ii++) {
379 if (_providerID[ii] != -1) {
380 alreadySet = true;
381 }
382 if (_providerID[ii] != newProviderID[ii]) {
383 different = true;
384 }
385 _providerID[ii] = newProviderID[ii];
386 }
387
388 if (alreadySet && different) {
389 emit newMessage("RTCM3coDecoder: Provider Changed " + _staID.toAscii() + "\n", true);
390 emit providerIDChanged(_staID);
391 }
392}
393
394//
395////////////////////////////////////////////////////////////////////////////
396void RTCM3coDecoder::setEpochTime() {
397
398 _lastTime.reset();
399
400 int epoSecGPS = -1;
401 int epoSecGlo = -1;
402 if (_clkOrb.NumberOfSat[CLOCKORBIT_SATGPS] > 0) {
403 epoSecGPS = _clkOrb.EpochTime[CLOCKORBIT_SATGPS]; // 0 .. 604799 s
404 }
405 else if (_codeBias.NumberOfSat[CLOCKORBIT_SATGPS] > 0) {
406 epoSecGPS = _codeBias.EpochTime[CLOCKORBIT_SATGPS]; // 0 .. 604799 s
407 }
408 else if (_phaseBias.NumberOfSat[CLOCKORBIT_SATGPS] > 0) {
409 epoSecGPS = _phaseBias.EpochTime[CLOCKORBIT_SATGPS]; // 0 .. 604799 s
410 }
411 else if (_vTEC.NumLayers > 0) {
412 epoSecGPS = _vTEC.EpochTime; // 0 .. 604799 s
413 }
414 else if (_clkOrb.NumberOfSat[CLOCKORBIT_SATGLONASS] > 0) {
415 epoSecGlo = _clkOrb.EpochTime[CLOCKORBIT_SATGLONASS]; // 0 .. 86399 s
416 }
417 else if (_codeBias.NumberOfSat[CLOCKORBIT_SATGLONASS] > 0) {
418 epoSecGlo = _codeBias.EpochTime[CLOCKORBIT_SATGLONASS]; // 0 .. 86399 s
419 }
420 else if (_phaseBias.NumberOfSat[CLOCKORBIT_SATGLONASS] > 0) {
421 epoSecGlo = _phaseBias.EpochTime[CLOCKORBIT_SATGLONASS]; // 0 .. 86399 s
422 }
423
424 // Retrieve current time
425 // ---------------------
426 int currentWeek = 0;
427 double currentSec = 0.0;
428 currentGPSWeeks(currentWeek, currentSec);
429 bncTime currentTime(currentWeek, currentSec);
430
431 // Set _lastTime close to currentTime
432 // ----------------------------------
433 if (epoSecGPS != -1) {
434 _lastTime.set(currentWeek, epoSecGPS);
435 }
436 else if (epoSecGlo != -1) {
437 QDate date = dateAndTimeFromGPSweek(currentTime.gpsw(), currentTime.gpssec()).date();
438 epoSecGlo = epoSecGlo - 3 * 3600 + gnumleap(date.year(), date.month(), date.day());
439 _lastTime.set(currentWeek, epoSecGlo);
440 }
441
442 if (_lastTime.valid()) {
443 double maxDiff = 12 * 3600.0;
444 while (_lastTime < currentTime - maxDiff) {
445 _lastTime = _lastTime + maxDiff;
446 }
447 while (_lastTime > currentTime + maxDiff) {
448 _lastTime = _lastTime - maxDiff;
449 }
450 }
451}
452
453//
454////////////////////////////////////////////////////////////////////////////
455string RTCM3coDecoder::codeTypeToRnxType(char system, CodeType type) const {
456 if (system == 'G') {
457 switch (type) {
458 case CODETYPEGPS_L1_CA: return "1C";
459 case CODETYPEGPS_L1_P: return "1P";
460 case CODETYPEGPS_L1_Z: return "1W";
461 case CODETYPEGPS_L2_CA: return "2C";
462 case CODETYPEGPS_SEMI_CODELESS: return "?N"; // which carrier ?
463 case CODETYPEGPS_L2_CM: return "2S";
464 case CODETYPEGPS_L2_CL: return "2L";
465 case CODETYPEGPS_L2_CML: return "2X";
466 case CODETYPEGPS_L2_P: return "2P";
467 case CODETYPEGPS_L2_Z: return "2W";
468 case CODETYPEGPS_L5_I: return "5I";
469 case CODETYPEGPS_L5_Q: return "5Q";
470 default: return "";
471 }
472 }
473 else if (system == 'R') {
474 switch (type) {
475 case CODETYPEGLONASS_L1_CA: return "1C";
476 case CODETYPEGLONASS_L1_P: return "1P";
477 case CODETYPEGLONASS_L2_CA: return "2C";
478 case CODETYPEGLONASS_L2_P: return "2P";
479 default: return "";
480 }
481 }
482 else if (system == 'E') {
483 switch (type) {
484 case CODETYPEGALILEO_E1_A: return "1A";
485 case CODETYPEGALILEO_E1_B: return "1B";
486 case CODETYPEGALILEO_E1_C: return "1C";
487 case CODETYPEGALILEO_E5A_I: return "5I";
488 case CODETYPEGALILEO_E5A_Q: return "5Q";
489 case CODETYPEGALILEO_E5B_I: return "7I";
490 case CODETYPEGALILEO_E5B_Q: return "7Q";
491 case CODETYPEGALILEO_E5_I: return "8I";
492 case CODETYPEGALILEO_E5_Q: return "8Q";
493 case CODETYPEGALILEO_E6_A: return "6A";
494 case CODETYPEGALILEO_E6_B: return "6B";
495 case CODETYPEGALILEO_E6_C: return "6C";
496 default: return "";
497 }
498 }
499 else if (system == 'J') {
500 switch (type) {
501 case CODETYPEQZSS_L1_CA: return "1C";
502 case CODETYPEQZSS_L1C_D: return "1S";
503 case CODETYPEQZSS_L1C_P: return "1L";
504 case CODETYPEQZSS_L1C_DP: return "1X";
505 case CODETYPEQZSS_L2_CM: return "2S";
506 case CODETYPEQZSS_L2_CL: return "2L";
507 case CODETYPEQZSS_L2_CML: return "2X";
508 case CODETYPEQZSS_L5_I: return "5I";
509 case CODETYPEQZSS_L5_Q: return "5Q";
510 case CODETYPEQZSS_L5_IQ: return "5X";
511 case CODETYPEQZSS_LEX_S: return "6S";
512 case CODETYPEQZSS_LEX_L: return "6L";
513 case CODETYPEQZSS_LEX_SL: return "6X";
514 default: return "";
515 }
516 }
517 else if (system == 'S') {
518 switch (type) {
519 case CODETYPE_SBAS_L1_CA: return "1C";
520 case CODETYPE_SBAS_L5_I: return "5I";
521 case CODETYPE_SBAS_L5_Q: return "5Q";
522 case CODETYPE_SBAS_L5_IQ: return "5X";
523 default: return "";
524 }
525 }
526 else if (system == 'C') {
527 switch (type) {
528 case CODETYPE_BDS_B1_I: return "1I";
529 case CODETYPE_BDS_B1_Q: return "1Q";
530 case CODETYPE_BDS_B1_IQ: return "1X";
531 case CODETYPE_BDS_B2_I: return "7I";
532 case CODETYPE_BDS_B2_Q: return "7Q";
533 case CODETYPE_BDS_B2_IQ: return "7X";
534 case CODETYPE_BDS_B3_I: return "6I";
535 case CODETYPE_BDS_B3_Q: return "6Q";
536 case CODETYPE_BDS_B3_IQ: return "6X";
537 default: return "";
538 }
539 }
540 return "";
541};
Note: See TracBrowser for help on using the repository browser.