source: ntrip/branches/BNC_2.11.0/src/RTCM3/RTCM3coDecoder.cpp@ 6611

Last change on this file since 6611 was 6611, checked in by stuerze, 9 years ago

minor changes to get a correct time stamp for GLONASS corrections at the end of a week
File size: 14.4 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 _GPSweeks = -1.0;
73
74 qRegisterMetaType<bncTime>("bncTime");
75
76 connect(this, SIGNAL(newCorrLine(QString, QString, bncTime)),
77 BNC_CORE, SLOT(slotNewCorrLine(QString, QString, bncTime)));
78
79 connect(this, SIGNAL(providerIDChanged(QString)),
80 BNC_CORE, SIGNAL(providerIDChanged(QString)));
81
82 connect(this, SIGNAL(newMessage(QByteArray,bool)),
83 BNC_CORE, SLOT(slotMessage(const QByteArray,bool)));
84
85 memset(&_co, 0, sizeof(_co));
86 memset(&_bias, 0, sizeof(_bias));
87
88 _providerID[0] = -1;
89 _providerID[1] = -1;
90 _providerID[2] = -1;
91}
92
93// Destructor
94////////////////////////////////////////////////////////////////////////////
95RTCM3coDecoder::~RTCM3coDecoder() {
96 delete _out;
97}
98
99// Reopen Output File
100////////////////////////////////////////////////////////////////////////
101void RTCM3coDecoder::reopen(const QString& fileNameSkl, QString& fileName,
102 ofstream*& out) {
103
104 if (!fileNameSkl.isEmpty()) {
105
106 bncSettings settings;
107
108 QDateTime datTim = currentDateAndTimeGPS();
109
110 QString hlpStr = bncRinex::nextEpochStr(datTim,
111 settings.value("corrIntr").toString());
112
113 QString fileNameHlp = fileNameSkl
114 + QString("%1").arg(datTim.date().dayOfYear(), 3, 10, QChar('0'))
115 + hlpStr + datTim.toString(".yyC");
116
117 if (fileName == fileNameHlp) {
118 return;
119 }
120 else {
121 fileName = fileNameHlp;
122 }
123
124 delete out;
125 if ( Qt::CheckState(settings.value("rnxAppend").toInt()) == Qt::Checked) {
126 out = new ofstream( fileName.toAscii().data(),
127 ios_base::out | ios_base::app );
128 }
129 else {
130 out = new ofstream( fileName.toAscii().data() );
131 }
132 }
133}
134
135//
136////////////////////////////////////////////////////////////////////////////
137t_irc RTCM3coDecoder::Decode(char* buffer, int bufLen, vector<string>& errmsg) {
138
139 errmsg.clear();
140
141 _buffer.append(QByteArray(buffer,bufLen));
142
143 t_irc retCode = failure;
144
145 while(_buffer.size()) {
146
147 int bytesused = 0;
148 struct ClockOrbit co_sav;
149 memcpy(&co_sav, &_co, sizeof(co_sav)); // save state
150
151 GCOB_RETURN irc = GetSSR(&_co, &_bias, 0, 0, _buffer.data(),
152 _buffer.size(), &bytesused);
153
154 if (irc <= -30) { // not enough data - restore state and exit loop
155 memcpy(&_co, &co_sav, sizeof(co_sav));
156 break;
157 }
158
159 else if (irc < 0) { // error - skip 1 byte and retry
160 memset(&_co, 0, sizeof(_co));
161 memset(&_bias, 0, sizeof(_bias));
162 _buffer = _buffer.mid(bytesused ? bytesused : 1);
163 }
164
165 else { // OK or MESSAGEFOLLOWS
166 _buffer = _buffer.mid(bytesused);
167
168 if ( (irc == GCOBR_OK || irc == GCOBR_MESSAGEFOLLOWS ) &&
169 (_co.NumberOfSat[CLOCKORBIT_SATGPS] > 0 || _co.NumberOfSat[CLOCKORBIT_SATGLONASS] > 0 ||
170 _bias.NumberOfSat[CLOCKORBIT_SATGPS] > 0 || _bias.NumberOfSat[CLOCKORBIT_SATGLONASS] > 0) ) {
171
172 reopen(_fileNameSkl, _fileName, _out);
173
174 // Guess GPS week and sec using system time
175 // ----------------------------------------
176 int GPSweek;
177 double GPSweeksHlp;
178 currentGPSWeeks(GPSweek, GPSweeksHlp);
179
180 // Correction Epoch from GPSEpochTime
181 // ----------------------------------
182 if (_co.NumberOfSat[CLOCKORBIT_SATGPS] > 0 || _bias.NumberOfSat[CLOCKORBIT_SATGPS] > 0) {
183 int GPSEpochTime = (_co.NumberOfSat[CLOCKORBIT_SATGPS] > 0) ?
184 _co.EpochTime[CLOCKORBIT_SATGPS] : _bias.EpochTime[CLOCKORBIT_SATGPS];
185 if (GPSweeksHlp > GPSEpochTime + 86400.0) {
186 GPSweek += 1;
187 }
188 else if (GPSweeksHlp < GPSEpochTime - 86400.0) {
189 GPSweek -= 1;
190 }
191 _GPSweeks = GPSEpochTime;
192 }
193
194 // Correction Epoch from Glonass Epoch
195 // -----------------------------------
196 else if (_co.NumberOfSat[CLOCKORBIT_SATGLONASS] > 0 || _bias.NumberOfSat[CLOCKORBIT_SATGLONASS] > 0){
197 int GLONASSEpochTime = (_co.NumberOfSat[CLOCKORBIT_SATGLONASS] > 0) ?
198 _co.EpochTime[CLOCKORBIT_SATGLONASS] : _bias.EpochTime[CLOCKORBIT_SATGLONASS];
199
200 // Second of day (GPS time) from Glonass Epoch
201 // -------------------------------------------
202 QDate date = dateAndTimeFromGPSweek(GPSweek, GPSweeksHlp).date();
203 int leapSecond = gnumleap(date.year(), date.month(), date.day());
204 int GPSDaySec = GLONASSEpochTime - 3 * 3600 + leapSecond;
205 if (GPSDaySec < 0) {
206 GPSDaySec += 86400;
207 }
208 int weekDay = int(GPSweeksHlp/86400.0);
209 int GPSDaySecHlp = int(GPSweeksHlp) - weekDay * 86400;
210
211 // Handle the difference between system clock and correction epoch
212 // ---------------------------------------------------------------
213 if (GPSDaySec < GPSDaySecHlp - 3600) {
214 weekDay += 1;
215 if (weekDay > 6) {
216 weekDay = 0;
217 GPSweek += 1;
218 }
219 }
220 else if (GPSDaySec > GPSDaySecHlp + 3600) {
221 weekDay -= 1;
222 if (weekDay < 0) {
223 weekDay = 6;
224 GPSweek -= 1;
225 }
226 }
227
228 _GPSweeks = weekDay * 86400.0 + GPSDaySec;
229 }
230
231 checkProviderID();
232
233 QStringList asciiLines = corrsToASCIIlines(GPSweek, _GPSweeks,
234 _co, &_bias);
235
236 QStringListIterator it(asciiLines);
237 while (it.hasNext()) {
238 QString line = it.next();
239 printLine(line, GPSweek, _GPSweeks);
240 }
241
242 retCode = success;
243 memset(&_co, 0, sizeof(_co));
244 memset(&_bias, 0, sizeof(_bias));
245 }
246 }
247 }
248
249 if (retCode != success) {
250 _GPSweeks = -1.0;
251 }
252 return retCode;
253}
254
255//
256////////////////////////////////////////////////////////////////////////////
257void RTCM3coDecoder::printLine(const QString& line, int GPSweek,
258 double GPSweeks) {
259 if (_out) {
260 *_out << line.toAscii().data() << endl;
261 _out->flush();
262 }
263
264 int currWeek;
265 double currSec;
266 currentGPSWeeks(currWeek, currSec);
267 bncTime currTime(currWeek, currSec);
268
269 bncTime coTime(GPSweek, GPSweeks);
270
271 double dt = currTime - coTime;
272 const double MAXDT = 10 * 60.0;
273 if (fabs(dt) > MAXDT) {
274 emit newMessage("suspicious correction: " + _staID.toAscii() + " "
275 + line.toAscii(), false);
276 }
277 else {
278 emit newCorrLine(line, _staID, coTime);
279 }
280}
281
282//
283////////////////////////////////////////////////////////////////////////////
284QStringList RTCM3coDecoder::corrsToASCIIlines(int GPSweek, double GPSweeks,
285 const ClockOrbit& co,
286 const CodeBias* bias) {
287
288 QStringList retLines;
289
290 // Loop over all satellites (GPS and Glonass)
291 // ------------------------------------------
292 if (co.NumberOfSat[CLOCKORBIT_SATGPS] > 0 || co.NumberOfSat[CLOCKORBIT_SATGLONASS] > 0) {
293 QString line1;
294 line1.sprintf("! Orbits/Clocks: %d GPS %d Glonass",
295 co.NumberOfSat[CLOCKORBIT_SATGPS], co.NumberOfSat[CLOCKORBIT_SATGLONASS]);
296 retLines << line1;
297 }
298 for (int ii = 0; ii < CLOCKORBIT_NUMGPS+co.NumberOfSat[CLOCKORBIT_SATGLONASS]; ii++) {
299 char sysCh = ' ';
300 if (ii < co.NumberOfSat[CLOCKORBIT_SATGPS]) {
301 sysCh = 'G';
302 }
303 else if (ii >= CLOCKORBIT_NUMGPS) {
304 sysCh = 'R';
305 }
306
307 if (sysCh != ' ') {
308
309 QString linePart;
310 linePart.sprintf("%d %d %d %.1f %c%2.2d",
311 co.messageType, co.UpdateInterval, GPSweek, GPSweeks,
312 sysCh, co.Sat[ii].ID);
313
314 // Combined message (orbit and clock)
315 // ----------------------------------
316 if ( co.messageType == COTYPE_GPSCOMBINED ||
317 co.messageType == COTYPE_GLONASSCOMBINED ) {
318 QString line;
319 line.sprintf(" %3d"
320 " %8.3f %8.3f %8.3f %8.3f"
321 " %10.5f %10.5f %10.5f %10.5f"
322 " %10.5f",
323 co.Sat[ii].IOD,
324 co.Sat[ii].Clock.DeltaA0,
325 co.Sat[ii].Orbit.DeltaRadial,
326 co.Sat[ii].Orbit.DeltaAlongTrack,
327 co.Sat[ii].Orbit.DeltaCrossTrack,
328 co.Sat[ii].Clock.DeltaA1,
329 co.Sat[ii].Orbit.DotDeltaRadial,
330 co.Sat[ii].Orbit.DotDeltaAlongTrack,
331 co.Sat[ii].Orbit.DotDeltaCrossTrack,
332 co.Sat[ii].Clock.DeltaA2);
333 retLines << linePart+line;
334 }
335
336 // Orbits only
337 // -----------
338 else if ( co.messageType == COTYPE_GPSORBIT ||
339 co.messageType == COTYPE_GLONASSORBIT ) {
340 QString line;
341 line.sprintf(" %3d"
342 " %8.3f %8.3f %8.3f"
343 " %10.5f %10.5f %10.5f",
344 co.Sat[ii].IOD,
345 co.Sat[ii].Orbit.DeltaRadial,
346 co.Sat[ii].Orbit.DeltaAlongTrack,
347 co.Sat[ii].Orbit.DeltaCrossTrack,
348 co.Sat[ii].Orbit.DotDeltaRadial,
349 co.Sat[ii].Orbit.DotDeltaAlongTrack,
350 co.Sat[ii].Orbit.DotDeltaCrossTrack);
351 retLines << linePart+line;
352 }
353
354 // Clocks only
355 // -----------
356 else if ( co.messageType == COTYPE_GPSCLOCK ||
357 co.messageType == COTYPE_GLONASSCLOCK ) {
358 QString line;
359 line.sprintf(" %8.3f %10.5f %10.5f",
360 co.Sat[ii].Clock.DeltaA0,
361 co.Sat[ii].Clock.DeltaA1,
362 co.Sat[ii].Clock.DeltaA2);
363 retLines << linePart+line;
364 }
365
366 // User Range Accuracy
367 // -------------------
368 else if ( co.messageType == COTYPE_GPSURA ||
369 co.messageType == COTYPE_GLONASSURA ) {
370 QString line;
371 line.sprintf(" %f", co.Sat[ii].UserRangeAccuracy);
372 retLines << linePart+line;
373 }
374
375 // High-Resolution Clocks
376 // ----------------------
377 else if ( co.messageType == COTYPE_GPSHR ||
378 co.messageType == COTYPE_GLONASSHR ) {
379 QString line;
380 line.sprintf(" %8.3f", co.Sat[ii].hrclock);
381 retLines << linePart+line;
382 }
383 }
384 }
385
386 // Loop over all satellites (GPS and Glonass)
387 // ------------------------------------------
388 if (bias) {
389 if (bias->NumberOfSat[CLOCKORBIT_SATGPS] > 0 || bias->NumberOfSat[CLOCKORBIT_SATGLONASS] > 0) {
390 QString line1;
391 line1.sprintf("! Biases: %d GPS %d Glonass",
392 bias->NumberOfSat[CLOCKORBIT_SATGPS], bias->NumberOfSat[CLOCKORBIT_SATGLONASS]);
393 retLines << line1;
394 }
395 for (int ii = 0; ii < CLOCKORBIT_NUMGPS + bias->NumberOfSat[CLOCKORBIT_SATGLONASS]; ii++) {
396 char sysCh = ' ';
397 int messageType;
398 if (ii < bias->NumberOfSat[CLOCKORBIT_SATGPS]) {
399 sysCh = 'G';
400 messageType = BTYPE_GPS;
401 }
402 else if (ii >= CLOCKORBIT_NUMGPS) {
403 sysCh = 'R';
404 messageType = BTYPE_GLONASS;
405 }
406 if (sysCh != ' ') {
407 QString line;
408 line.sprintf("%d %d %d %.1f %c%2.2d %d",
409 messageType, bias->UpdateInterval, GPSweek, GPSweeks,
410 sysCh, bias->Sat[ii].ID,
411 bias->Sat[ii].NumberOfCodeBiases);
412 for (int jj = 0; jj < bias->Sat[ii].NumberOfCodeBiases; jj++) {
413 QString hlp;
414 hlp.sprintf(" %d %8.3f", bias->Sat[ii].Biases[jj].Type,
415 bias->Sat[ii].Biases[jj].Bias);
416 line += hlp;
417 }
418 retLines << line;
419 }
420 }
421 }
422
423 return retLines;
424}
425
426//
427////////////////////////////////////////////////////////////////////////////
428void RTCM3coDecoder::checkProviderID() {
429
430 if (_co.SSRProviderID == 0 && _co.SSRSolutionID == 0 && _co.SSRIOD == 0) {
431 return;
432 }
433
434 int newProviderID[3];
435 newProviderID[0] = _co.SSRProviderID;
436 newProviderID[1] = _co.SSRSolutionID;
437 newProviderID[2] = _co.SSRIOD;
438
439 bool alreadySet = false;
440 bool different = false;
441
442 for (unsigned ii = 0; ii < 3; ii++) {
443 if (_providerID[ii] != -1) {
444 alreadySet = true;
445 }
446 if (_providerID[ii] != newProviderID[ii]) {
447 different = true;
448 }
449 _providerID[ii] = newProviderID[ii];
450 }
451
452 if (alreadySet && different) {
453 emit newMessage("RTCM3coDecoder: Provider Changed " + _staID.toAscii() + "\n", true);
454 emit providerIDChanged(_staID);
455 }
456}
Note: See TracBrowser for help on using the repository browser.