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

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