source: ntrip/branches/BNC_LM/RTCM3/RTCM3coDecoder.cpp@ 6158

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