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

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