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

Last change on this file since 5681 was 5665, checked in by stoecker, 10 years ago

update due to RTCM-SSR fixes
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
206 int weekDay = int(GPSweeksHlp/86400.0);
207 int GPSDaySecHlp = int(GPSweeksHlp) - weekDay * 86400;
208
209 // Handle the difference between system clock and correction epoch
210 // ---------------------------------------------------------------
211 if (GPSDaySec < GPSDaySecHlp - 3600) {
212 weekDay += 1;
213 if (weekDay > 6) {
214 weekDay = 0;
215 GPSweek += 1;
216 }
217 }
218 else if (GPSDaySec > GPSDaySecHlp + 3600) {
219 weekDay -= 1;
220 if (weekDay < 0) {
221 weekDay = 6;
222 GPSweek -= 1;
223 }
224 }
225
226 _GPSweeks = weekDay * 86400.0 + GPSDaySec;
227 }
228
229 checkProviderID();
230
231 QStringList asciiLines = corrsToASCIIlines(GPSweek, _GPSweeks,
232 _co, &_bias);
233
234 QStringListIterator it(asciiLines);
235 while (it.hasNext()) {
236 QString line = it.next();
237 printLine(line, GPSweek, _GPSweeks);
238 }
239
240 retCode = success;
241 memset(&_co, 0, sizeof(_co));
242 memset(&_bias, 0, sizeof(_bias));
243 }
244 }
245 }
246
247 if (retCode != success) {
248 _GPSweeks = -1.0;
249 }
250 return retCode;
251}
252
253//
254////////////////////////////////////////////////////////////////////////////
255void RTCM3coDecoder::printLine(const QString& line, int GPSweek,
256 double GPSweeks) {
257 if (_out) {
258 *_out << line.toAscii().data() << endl;
259 _out->flush();
260 }
261
262 int currWeek;
263 double currSec;
264 currentGPSWeeks(currWeek, currSec);
265 bncTime currTime(currWeek, currSec);
266
267 bncTime coTime(GPSweek, GPSweeks);
268
269 double dt = currTime - coTime;
270 const double MAXDT = 10 * 60.0;
271 if (fabs(dt) > MAXDT) {
272 emit newMessage("suspicious correction: " + _staID.toAscii() + " "
273 + line.toAscii(), false);
274 }
275 else {
276 emit newCorrLine(line, _staID, coTime);
277 }
278}
279
280//
281////////////////////////////////////////////////////////////////////////////
282QStringList RTCM3coDecoder::corrsToASCIIlines(int GPSweek, double GPSweeks,
283 const ClockOrbit& co,
284 const CodeBias* bias) {
285
286 QStringList retLines;
287
288 // Loop over all satellites (GPS and Glonass)
289 // ------------------------------------------
290 if (co.NumberOfSat[CLOCKORBIT_SATGPS] > 0 || co.NumberOfSat[CLOCKORBIT_SATGLONASS] > 0) {
291 QString line1;
292 line1.sprintf("! Orbits/Clocks: %d GPS %d Glonass",
293 co.NumberOfSat[CLOCKORBIT_SATGPS], co.NumberOfSat[CLOCKORBIT_SATGLONASS]);
294 retLines << line1;
295 }
296 for (int ii = 0; ii < CLOCKORBIT_NUMGPS+co.NumberOfSat[CLOCKORBIT_SATGLONASS]; ii++) {
297 char sysCh = ' ';
298 if (ii < co.NumberOfSat[CLOCKORBIT_SATGPS]) {
299 sysCh = 'G';
300 }
301 else if (ii >= CLOCKORBIT_NUMGPS) {
302 sysCh = 'R';
303 }
304
305 if (sysCh != ' ') {
306
307 QString linePart;
308 linePart.sprintf("%d %d %d %.1f %c%2.2d",
309 co.messageType, co.UpdateInterval, GPSweek, GPSweeks,
310 sysCh, co.Sat[ii].ID);
311
312 // Combined message (orbit and clock)
313 // ----------------------------------
314 if ( co.messageType == COTYPE_GPSCOMBINED ||
315 co.messageType == COTYPE_GLONASSCOMBINED ) {
316 QString line;
317 line.sprintf(" %3d"
318 " %8.3f %8.3f %8.3f %8.3f"
319 " %10.5f %10.5f %10.5f %10.5f"
320 " %10.5f",
321 co.Sat[ii].IOD,
322 co.Sat[ii].Clock.DeltaA0,
323 co.Sat[ii].Orbit.DeltaRadial,
324 co.Sat[ii].Orbit.DeltaAlongTrack,
325 co.Sat[ii].Orbit.DeltaCrossTrack,
326 co.Sat[ii].Clock.DeltaA1,
327 co.Sat[ii].Orbit.DotDeltaRadial,
328 co.Sat[ii].Orbit.DotDeltaAlongTrack,
329 co.Sat[ii].Orbit.DotDeltaCrossTrack,
330 co.Sat[ii].Clock.DeltaA2);
331 retLines << linePart+line;
332 }
333
334 // Orbits only
335 // -----------
336 else if ( co.messageType == COTYPE_GPSORBIT ||
337 co.messageType == COTYPE_GLONASSORBIT ) {
338 QString line;
339 line.sprintf(" %3d"
340 " %8.3f %8.3f %8.3f"
341 " %10.5f %10.5f %10.5f",
342 co.Sat[ii].IOD,
343 co.Sat[ii].Orbit.DeltaRadial,
344 co.Sat[ii].Orbit.DeltaAlongTrack,
345 co.Sat[ii].Orbit.DeltaCrossTrack,
346 co.Sat[ii].Orbit.DotDeltaRadial,
347 co.Sat[ii].Orbit.DotDeltaAlongTrack,
348 co.Sat[ii].Orbit.DotDeltaCrossTrack);
349 retLines << linePart+line;
350 }
351
352 // Clocks only
353 // -----------
354 else if ( co.messageType == COTYPE_GPSCLOCK ||
355 co.messageType == COTYPE_GLONASSCLOCK ) {
356 QString line;
357 line.sprintf(" %8.3f %10.5f %10.5f",
358 co.Sat[ii].Clock.DeltaA0,
359 co.Sat[ii].Clock.DeltaA1,
360 co.Sat[ii].Clock.DeltaA2);
361 retLines << linePart+line;
362 }
363
364 // User Range Accuracy
365 // -------------------
366 else if ( co.messageType == COTYPE_GPSURA ||
367 co.messageType == COTYPE_GLONASSURA ) {
368 QString line;
369 line.sprintf(" %f", co.Sat[ii].UserRangeAccuracy);
370 retLines << linePart+line;
371 }
372
373 // High-Resolution Clocks
374 // ----------------------
375 else if ( co.messageType == COTYPE_GPSHR ||
376 co.messageType == COTYPE_GLONASSHR ) {
377 QString line;
378 line.sprintf(" %8.3f", co.Sat[ii].hrclock);
379 retLines << linePart+line;
380 }
381 }
382 }
383
384 // Loop over all satellites (GPS and Glonass)
385 // ------------------------------------------
386 if (bias) {
387 if (bias->NumberOfSat[CLOCKORBIT_SATGPS] > 0 || bias->NumberOfSat[CLOCKORBIT_SATGLONASS] > 0) {
388 QString line1;
389 line1.sprintf("! Biases: %d GPS %d Glonass",
390 bias->NumberOfSat[CLOCKORBIT_SATGPS], bias->NumberOfSat[CLOCKORBIT_SATGLONASS]);
391 retLines << line1;
392 }
393 for (int ii = 0; ii < CLOCKORBIT_NUMGPS + bias->NumberOfSat[CLOCKORBIT_SATGLONASS]; ii++) {
394 char sysCh = ' ';
395 int messageType;
396 if (ii < bias->NumberOfSat[CLOCKORBIT_SATGPS]) {
397 sysCh = 'G';
398 messageType = BTYPE_GPS;
399 }
400 else if (ii >= CLOCKORBIT_NUMGPS) {
401 sysCh = 'R';
402 messageType = BTYPE_GLONASS;
403 }
404 if (sysCh != ' ') {
405 QString line;
406 line.sprintf("%d %d %d %.1f %c%2.2d %d",
407 messageType, bias->UpdateInterval, GPSweek, GPSweeks,
408 sysCh, bias->Sat[ii].ID,
409 bias->Sat[ii].NumberOfCodeBiases);
410 for (int jj = 0; jj < bias->Sat[ii].NumberOfCodeBiases; jj++) {
411 QString hlp;
412 hlp.sprintf(" %d %8.3f", bias->Sat[ii].Biases[jj].Type,
413 bias->Sat[ii].Biases[jj].Bias);
414 line += hlp;
415 }
416 retLines << line;
417 }
418 }
419 }
420
421 return retLines;
422}
423
424//
425////////////////////////////////////////////////////////////////////////////
426void RTCM3coDecoder::checkProviderID() {
427
428 if (_co.SSRProviderID == 0 && _co.SSRSolutionID == 0 && _co.SSRIOD == 0) {
429 return;
430 }
431
432 int newProviderID[3];
433 newProviderID[0] = _co.SSRProviderID;
434 newProviderID[1] = _co.SSRSolutionID;
435 newProviderID[2] = _co.SSRIOD;
436
437 bool alreadySet = false;
438 bool different = false;
439
440 for (unsigned ii = 0; ii < 3; ii++) {
441 if (_providerID[ii] != -1) {
442 alreadySet = true;
443 }
444 if (_providerID[ii] != newProviderID[ii]) {
445 different = true;
446 }
447 _providerID[ii] = newProviderID[ii];
448 }
449
450 if (alreadySet && different) {
451 emit newMessage("RTCM3coDecoder: Provider Changed " + _staID.toAscii() + "\n", true);
452 emit providerIDChanged(_staID);
453 }
454}
Note: See TracBrowser for help on using the repository browser.