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

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