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

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