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

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