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

Last change on this file since 6471 was 6471, checked in by mervart, 11 years ago
File size: 12.9 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 qRegisterMetaType<bncTime>("bncTime");
74 qRegisterMetaType< QList<t_orbCorr> >("QList<t_orbCorr>");
75 qRegisterMetaType< QList<t_clkCorr> >("QList<t_clkCorr>");
76
77 connect(this, SIGNAL(newOrbCorrections(QList<t_orbCorr>)),
78 BNC_CORE, SLOT(slotNewOrbCorrections(QList<t_orbCorr>)));
79
80 connect(this, SIGNAL(newClkCorrections(QList<t_clkCorr>)),
81 BNC_CORE, SLOT(slotNewClkCorrections(QList<t_clkCorr>)));
82
83 connect(this, SIGNAL(providerIDChanged(QString)),
84 BNC_CORE, SIGNAL(providerIDChanged(QString)));
85
86 connect(this, SIGNAL(newMessage(QByteArray,bool)),
87 BNC_CORE, SLOT(slotMessage(const QByteArray,bool)));
88
89 reset();
90
91 _providerID[0] = -1;
92 _providerID[1] = -1;
93 _providerID[2] = -1;
94}
95
96// Destructor
97////////////////////////////////////////////////////////////////////////////
98RTCM3coDecoder::~RTCM3coDecoder() {
99 delete _out;
100}
101
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
111// Reopen Output File
112////////////////////////////////////////////////////////////////////////
113void RTCM3coDecoder::reopen() {
114
115 if (!_fileNameSkl.isEmpty()) {
116
117 bncSettings settings;
118
119 QDateTime datTim = currentDateAndTimeGPS();
120
121 QString hlpStr = bncRinex::nextEpochStr(datTim,
122 settings.value("corrIntr").toString());
123
124 QString fileNameHlp = _fileNameSkl
125 + QString("%1").arg(datTim.date().dayOfYear(), 3, 10, QChar('0'))
126 + hlpStr + datTim.toString(".yyC");
127
128 if (_fileName == fileNameHlp) {
129 return;
130 }
131 else {
132 _fileName = fileNameHlp;
133 }
134
135 delete _out;
136 if ( Qt::CheckState(settings.value("rnxAppend").toInt()) == Qt::Checked) {
137 _out = new ofstream( _fileName.toAscii().data(), ios_base::out | ios_base::app );
138 }
139 else {
140 _out = new ofstream( _fileName.toAscii().data() );
141 }
142 }
143}
144
145//
146////////////////////////////////////////////////////////////////////////////
147t_irc RTCM3coDecoder::Decode(char* buffer, int bufLen, vector<string>& errmsg) {
148
149 errmsg.clear();
150
151 _buffer.append(QByteArray(buffer,bufLen));
152
153 t_irc retCode = failure;
154
155 while(_buffer.size()) {
156
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
166 int bytesused = 0;
167 GCOB_RETURN irc = GetSSR(&_clkOrb, &_codeBias, &_vTEC, &_phaseBias,
168 _buffer.data(), _buffer.size(), &bytesused);
169
170 if (irc <= -30) { // not enough data - restore state and exit loop
171 memcpy(&_clkOrb, &clkOrbSav, sizeof(clkOrbSav));
172 memcpy(&_codeBias, &codeBiasSav, sizeof(codeBiasSav));
173 memcpy(&_phaseBias, &phaseBiasSav, sizeof(phaseBiasSav));
174 memcpy(&_vTEC, &vTECSav, sizeof(vTECSav));
175 break;
176 }
177
178 else if (irc < 0) { // error - skip 1 byte and retry
179 reset();
180 _buffer = _buffer.mid(bytesused ? bytesused : 1);
181 }
182
183 else { // OK or MESSAGEFOLLOWS
184 _buffer = _buffer.mid(bytesused);
185
186 if (irc == GCOBR_OK || irc == GCOBR_MESSAGEFOLLOWS ) {
187
188 setEpochTime(); // sets _lastTime
189
190 if (_lastTime.valid()) {
191 reopen();
192 checkProviderID();
193 sendResults();
194 retCode = success;
195 }
196 else {
197 retCode = failure;
198 }
199
200 reset();
201 }
202 }
203 }
204
205 return retCode;
206}
207
208//
209////////////////////////////////////////////////////////////////////////////
210void RTCM3coDecoder::sendResults() {
211
212 QList<t_orbCorr>& orbCorrections = _orbCorrections[_lastTime];
213 QList<t_clkCorr>& clkCorrections = _clkCorrections[_lastTime];
214
215 // Orbit and clock corrections of all satellites
216 // ---------------------------------------------
217 for (unsigned ii = 0; ii < CLOCKORBIT_NUMGPS + _clkOrb.NumberOfSat[CLOCKORBIT_SATGLONASS]; ii++) {
218 char sysCh = ' ';
219 if (ii < _clkOrb.NumberOfSat[CLOCKORBIT_SATGPS]) {
220 sysCh = 'G';
221 }
222 else if (ii >= CLOCKORBIT_NUMGPS) {
223 sysCh = 'R';
224 }
225 else {
226 continue;
227 }
228
229 // Orbit correction
230 // ----------------
231 if ( _clkOrb.messageType == COTYPE_GPSCOMBINED ||
232 _clkOrb.messageType == COTYPE_GLONASSCOMBINED ||
233 _clkOrb.messageType == COTYPE_GPSORBIT ||
234 _clkOrb.messageType == COTYPE_GLONASSORBIT ) {
235
236 t_orbCorr orbCorr;
237 orbCorr._prn.set(sysCh, _clkOrb.Sat[ii].ID);
238 orbCorr._staID = _staID.toAscii().data();
239 orbCorr._iod = _clkOrb.Sat[ii].IOD;
240 orbCorr._time = _lastTime;
241 orbCorr._system = 'R';
242 orbCorr._xr[0] = _clkOrb.Sat[ii].Orbit.DeltaRadial;
243 orbCorr._xr[1] = _clkOrb.Sat[ii].Orbit.DeltaAlongTrack;
244 orbCorr._xr[2] = _clkOrb.Sat[ii].Orbit.DeltaCrossTrack;
245 orbCorr._dotXr[0] = _clkOrb.Sat[ii].Orbit.DotDeltaRadial;
246 orbCorr._dotXr[1] = _clkOrb.Sat[ii].Orbit.DotDeltaAlongTrack;
247 orbCorr._dotXr[2] = _clkOrb.Sat[ii].Orbit.DotDeltaCrossTrack;
248
249 orbCorrections.push_back(orbCorr);
250
251 _IODs[orbCorr._prn] = _clkOrb.Sat[ii].IOD;
252 }
253
254 // Clock Corrections
255 // -----------------
256 if ( _clkOrb.messageType == COTYPE_GPSCOMBINED ||
257 _clkOrb.messageType == COTYPE_GLONASSCOMBINED ||
258 _clkOrb.messageType == COTYPE_GPSCLOCK ||
259 _clkOrb.messageType == COTYPE_GLONASSCLOCK ) {
260
261 t_clkCorr clkCorr;
262 clkCorr._prn.set(sysCh, _clkOrb.Sat[ii].ID);
263 clkCorr._staID = _staID.toAscii().data();
264 clkCorr._time = _lastTime;
265 clkCorr._dClk = _clkOrb.Sat[ii].Clock.DeltaA0 / t_CST::c;
266 clkCorr._dotDClk = _clkOrb.Sat[ii].Clock.DeltaA1 / t_CST::c;
267 clkCorr._dotDotDClk = _clkOrb.Sat[ii].Clock.DeltaA2 / t_CST::c;
268
269 _lastClkCorrections[clkCorr._prn] = clkCorr;
270
271 if (_IODs.contains(clkCorr._prn)) {
272 clkCorr._iod = _IODs[clkCorr._prn];
273 clkCorrections.push_back(clkCorr);
274 }
275 }
276
277 // High-Resolution Clocks
278 // ----------------------
279 if ( _clkOrb.messageType == COTYPE_GPSHR ||
280 _clkOrb.messageType == COTYPE_GLONASSHR ) {
281
282 t_prn prn(sysCh, _clkOrb.Sat[ii].ID);
283 if (_lastClkCorrections.contains(prn)) {
284 t_clkCorr clkCorr;
285 clkCorr = _lastClkCorrections[prn];
286 clkCorr._time = _lastTime;
287 clkCorr._dClk +=_clkOrb.Sat[ii].hrclock / t_CST::c;
288 if (_IODs.contains(clkCorr._prn)) {
289 clkCorr._iod = _IODs[clkCorr._prn];
290 clkCorrections.push_back(clkCorr);
291 }
292 }
293 }
294 }
295
296 // Code Biases
297 // -----------
298 QList<t_satCodeBias> satCodeBiases;
299 for (unsigned ii = 0; ii < CLOCKORBIT_NUMGPS + _codeBias.NumberOfSat[CLOCKORBIT_SATGLONASS]; ii++) {
300 char sysCh = ' ';
301 if (ii < _codeBias.NumberOfSat[CLOCKORBIT_SATGPS]) {
302 sysCh = 'G';
303 }
304 else if (ii >= CLOCKORBIT_NUMGPS) {
305 sysCh = 'R';
306 }
307 else {
308 continue;
309 }
310 t_satCodeBias satCodeBias;
311 satCodeBias._prn.set(sysCh, _codeBias.Sat[ii].ID);
312 satCodeBias._time = _lastTime;
313 for (unsigned jj = 0; jj < _codeBias.Sat[ii].NumberOfCodeBiases; jj++) {
314 }
315 }
316
317 // Dump all older epochs
318 // ---------------------
319 QMutableMapIterator<bncTime, QList<t_orbCorr> > itOrb(_orbCorrections);
320 while (itOrb.hasNext()) {
321 itOrb.next();
322 if (itOrb.key() < _lastTime) {
323 emit newOrbCorrections(itOrb.value());
324 t_orbCorr::writeEpoch(_out, itOrb.value());
325 itOrb.remove();
326 }
327 }
328 QMutableMapIterator<bncTime, QList<t_clkCorr> > itClk(_clkCorrections);
329 while (itClk.hasNext()) {
330 itClk.next();
331 if (itClk.key() < _lastTime) {
332 emit newClkCorrections(itClk.value());
333 t_clkCorr::writeEpoch(_out, itClk.value());
334 itClk.remove();
335 }
336 }
337}
338
339//
340////////////////////////////////////////////////////////////////////////////
341void RTCM3coDecoder::checkProviderID() {
342
343 if (_clkOrb.SSRProviderID == 0 && _clkOrb.SSRSolutionID == 0 && _clkOrb.SSRIOD == 0) {
344 return;
345 }
346
347 int newProviderID[3];
348 newProviderID[0] = _clkOrb.SSRProviderID;
349 newProviderID[1] = _clkOrb.SSRSolutionID;
350 newProviderID[2] = _clkOrb.SSRIOD;
351
352 bool alreadySet = false;
353 bool different = false;
354
355 for (unsigned ii = 0; ii < 3; ii++) {
356 if (_providerID[ii] != -1) {
357 alreadySet = true;
358 }
359 if (_providerID[ii] != newProviderID[ii]) {
360 different = true;
361 }
362 _providerID[ii] = newProviderID[ii];
363 }
364
365 if (alreadySet && different) {
366 emit newMessage("RTCM3coDecoder: Provider Changed " + _staID.toAscii() + "\n", true);
367 emit providerIDChanged(_staID);
368 }
369}
370
371
372//
373////////////////////////////////////////////////////////////////////////////
374void RTCM3coDecoder::setEpochTime() {
375
376 _lastTime.reset();
377
378 int epoSecGPS = -1;
379 int epoSecGlo = -1;
380 if (_clkOrb.NumberOfSat[CLOCKORBIT_SATGPS] > 0) {
381 epoSecGPS = _clkOrb.EpochTime[CLOCKORBIT_SATGPS]; // 0 .. 604799 s
382 }
383 else if (_codeBias.NumberOfSat[CLOCKORBIT_SATGPS] > 0) {
384 epoSecGPS = _codeBias.EpochTime[CLOCKORBIT_SATGPS]; // 0 .. 604799 s
385 }
386 else if (_phaseBias.NumberOfSat[CLOCKORBIT_SATGPS] > 0) {
387 epoSecGPS = _phaseBias.EpochTime[CLOCKORBIT_SATGPS]; // 0 .. 604799 s
388 }
389 else if (_vTEC.NumLayers > 0) {
390 epoSecGPS = _vTEC.EpochTime; // 0 .. 604799 s
391 }
392 else if (_clkOrb.NumberOfSat[CLOCKORBIT_SATGLONASS] > 0) {
393 epoSecGlo = _clkOrb.EpochTime[CLOCKORBIT_SATGLONASS]; // 0 .. 86399 s
394 }
395 else if (_codeBias.NumberOfSat[CLOCKORBIT_SATGLONASS] > 0) {
396 epoSecGlo = _codeBias.EpochTime[CLOCKORBIT_SATGLONASS]; // 0 .. 86399 s
397 }
398 else if (_phaseBias.NumberOfSat[CLOCKORBIT_SATGLONASS] > 0) {
399 epoSecGlo = _phaseBias.EpochTime[CLOCKORBIT_SATGLONASS]; // 0 .. 86399 s
400 }
401
402 // Retrieve current time
403 // ---------------------
404 int currentWeek = 0;
405 double currentSec = 0.0;
406 currentGPSWeeks(currentWeek, currentSec);
407 bncTime currentTime(currentWeek, currentSec);
408
409 // Set _lastTime close to currentTime
410 // ----------------------------------
411 if (epoSecGPS != -1) {
412 _lastTime.set(currentWeek, epoSecGPS);
413 }
414 else if (epoSecGlo != -1) {
415 QDate date = dateAndTimeFromGPSweek(currentTime.gpsw(), currentTime.gpssec()).date();
416 epoSecGlo = epoSecGlo - 3 * 3600 + gnumleap(date.year(), date.month(), date.day());
417 _lastTime.set(currentWeek, epoSecGlo);
418 }
419
420 if (_lastTime.valid()) {
421 double maxDiff = 12 * 3600.0;
422 while (_lastTime < currentTime - maxDiff) {
423 _lastTime = _lastTime + maxDiff;
424 }
425 while (_lastTime > currentTime + maxDiff) {
426 _lastTime = _lastTime - maxDiff;
427 }
428 }
429}
Note: See TracBrowser for help on using the repository browser.