source: ntrip/trunk/BNC/RTCM3/RTCM3Decoder.cpp@ 1548

Last change on this file since 1548 was 1535, checked in by mervart, 16 years ago

* empty log message *
File size: 13.7 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: RTCM3Decoder
30 *
31 * Purpose: RTCM3 Decoder
32 *
33 * Author: L. Mervart
34 *
35 * Created: 24-Aug-2006
36 *
37 * Changes:
38 *
39 * -----------------------------------------------------------------------*/
40
41#include <iostream>
42#include <math.h>
43#include <string.h>
44
45#include "RTCM3Decoder.h"
46#include "RTCM3coDecoder.h"
47#include "bncconst.h"
48#include "bncapp.h"
49#include "bncutils.h"
50#include "bncsettings.h"
51
52using namespace std;
53
54#ifndef isinf
55# define isinf(x) 0
56#endif
57
58// Error Handling
59////////////////////////////////////////////////////////////////////////////
60void RTCM3Error(const char*, ...) {
61}
62
63// Constructor
64////////////////////////////////////////////////////////////////////////////
65RTCM3Decoder::RTCM3Decoder(const QString& staID) : GPSDecoder() {
66
67 bncSettings settings;
68 _checkMountPoint = settings.value("messTypes").toString();
69 _staID = staID;
70
71 // Latency
72 _numLat = 0;
73 _minLat = 1000.;
74 _maxLat = -1000.;
75 _sumLat = 0.;
76 _sumLatQ = 0.;
77 _followSec = false;
78 _meanDiff = 0.;
79 _diffSecGPS= 0.;
80 _numGaps = 0;
81 _oldSecGPS = 0.;
82 _newSecGPS = 0.;
83 _curLat = 0.;
84 _perfIntr = 86400;
85 if ( settings.value("perfIntr").toString().isEmpty() ) { _perfIntr = 0; }
86 if ( settings.value("perfIntr").toString().indexOf("1 min") != -1 ) { _perfIntr = 60; }
87 if ( settings.value("perfIntr").toString().indexOf("5 min") != -1 ) { _perfIntr = 300; }
88 if ( settings.value("perfIntr").toString().indexOf("15 min") != -1 ) { _perfIntr = 900; }
89 if ( settings.value("perfIntr").toString().indexOf("1 hour") != -1 ) { _perfIntr = 3600; }
90 if ( settings.value("perfIntr").toString().indexOf("6 hours") != -1 ) { _perfIntr = 21600; }
91 if ( settings.value("perfIntr").toString().indexOf("1 day") != -1 ) { _perfIntr = 86400; }
92
93 // Ensure, that the Decoder uses the "old" convention for the data structure for Rinex2. Perlt
94 _Parser.rinex3 = 0;
95
96 memset(&_Parser, 0, sizeof(_Parser));
97
98 double secGPS;
99 currentGPSWeeks(_Parser.GPSWeek, secGPS);
100 _Parser.GPSTOW = int(secGPS);
101
102 connect(this, SIGNAL(newGPSEph(gpsephemeris*)),
103 (bncApp*) qApp, SLOT(slotNewGPSEph(gpsephemeris*)));
104 connect(this, SIGNAL(newGlonassEph(glonassephemeris*)),
105 (bncApp*) qApp, SLOT(slotNewGlonassEph(glonassephemeris*)));
106
107 // Sub-Decoder for Clock and Orbit Corrections
108 // -------------------------------------------
109 _coDecoder = new RTCM3coDecoder(staID);
110
111 // Mode can be either observations or corrections
112 // ----------------------------------------------
113 _mode = unknown;
114}
115
116// Destructor
117////////////////////////////////////////////////////////////////////////////
118RTCM3Decoder::~RTCM3Decoder() {
119 delete _coDecoder;
120}
121
122//
123////////////////////////////////////////////////////////////////////////////
124t_irc RTCM3Decoder::Decode(char* buffer, int bufLen, vector<string>& errmsg) {
125
126 errmsg.clear();
127
128 bool decoded = false;
129
130 // Try to decode Clock and Orbit Corrections
131 // -----------------------------------------
132 if (_mode == unknown || _mode == corrections) {
133 if ( _coDecoder->Decode(buffer, bufLen, errmsg) == success ) {
134 decoded = true;
135
136 // Latency
137 // -------
138 if (_perfIntr>0) {
139 if (0<_coDecoder->_epochList.size()) {
140 for (int ii=0;ii<_coDecoder->_epochList.size();ii++) {
141 int week;
142 double sec;
143 _newSecGPS = _coDecoder->_epochList[ii];
144 currentGPSWeeks(week, sec);
145 double dt = fabs(sec - _newSecGPS);
146 const double secPerWeek = 7.0 * 24.0 * 3600.0;
147 if (dt > 0.5 * secPerWeek) {
148 if (sec > _newSecGPS) {
149 sec -= secPerWeek;
150 } else {
151 sec += secPerWeek;
152 }
153 }
154 if (_newSecGPS != _oldSecGPS) {
155 if (int(_newSecGPS) % _perfIntr < int(_oldSecGPS) % _perfIntr) {
156 if (_numLat>0) {
157 QString late;
158 if (_meanDiff>0.) {
159 late = QString(": Mean latency %1 sec, min %2, max %3, rms %4, %5 epochs, %6 gaps")
160 .arg(int(_sumLat/_numLat*100)/100.)
161 .arg(int(_minLat*100)/100.)
162 .arg(int(_maxLat*100)/100.)
163 .arg(int((sqrt((_sumLatQ - _sumLat * _sumLat / _numLat)/_numLat))*100)/100.)
164 .arg(_numLat)
165 .arg(_numGaps);
166 emit(newMessage(QString(_staID + late ).toAscii(), true) );
167 } else {
168 late = QString(": Mean latency %1 sec, min %2, max %3, rms %4, %5 epochs")
169 .arg(int(_sumLat/_numLat*100)/100.)
170 .arg(int(_minLat*100)/100.)
171 .arg(int(_maxLat*100)/100.)
172 .arg(int((sqrt((_sumLatQ - _sumLat * _sumLat / _numLat)/_numLat))*100)/100.)
173 .arg(_numLat);
174 emit(newMessage(QString(_staID + late ).toAscii(), true) );
175 }
176 }
177 _meanDiff = int(_diffSecGPS)/_numLat;
178 _diffSecGPS = 0.;
179 _numGaps = 0;
180 _sumLat = 0.;
181 _sumLatQ = 0.;
182 _numLat = 0;
183 _minLat = 1000.;
184 _maxLat = -1000.;
185 }
186 if (_followSec) {
187 _diffSecGPS += _newSecGPS - _oldSecGPS;
188 if (_meanDiff>0.) {
189 if (_newSecGPS - _oldSecGPS > 1.5 * _meanDiff) {
190 _numGaps += 1;
191 }
192 }
193 }
194 _curLat = sec - _newSecGPS;
195 _sumLat += _curLat;
196 _sumLatQ += _curLat * _curLat;
197 if (_curLat < _minLat) {_minLat = _curLat;}
198 if (_curLat >= _maxLat) {_maxLat = _curLat;}
199 _numLat += 1;
200 _oldSecGPS = _newSecGPS;
201 _followSec = true;
202 }
203 }
204 }
205 }
206 if (_mode == unknown) {
207 _mode = corrections;
208 }
209 }
210 _coDecoder->_epochList.clear();
211 }
212
213 // Remaining part decodes the Observations
214 // ---------------------------------------
215 if (_mode == unknown || _mode == observations || _checkMountPoint == _staID || _checkMountPoint == "ALL") {
216
217 for (int ii = 0; ii < bufLen; ii++) {
218 _Parser.Message[_Parser.MessageSize++] = buffer[ii];
219
220 if (_Parser.MessageSize >= _Parser.NeedBytes) {
221
222 while(int rr = RTCM3Parser(&_Parser)) {
223
224 // RTCMv3 message types
225 // --------------------
226 _typeList.push_back(_Parser.blocktype);
227
228 // RTCMv3 antenna descriptor
229 // -------------------------
230 if(rr == 1007 || rr == 1008 || rr == 1033)
231 {
232 _antType.push_back(_Parser.antenna); /* correct ? */
233 }
234
235 // RTCMv3 antenna XYZ
236 // ------------------
237 else if(rr == 1005)
238 {
239 _antList.push_back(t_antInfo());
240 _antList.back().type = t_antInfo::ARP;
241 _antList.back().xx = _Parser.antX * 1e-4;
242 _antList.back().yy = _Parser.antY * 1e-4;
243 _antList.back().zz = _Parser.antZ * 1e-4;
244 _antList.back().message = rr;
245 }
246
247 // RTCMv3 antenna XYZ-H
248 // --------------------
249 else if(rr == 1006)
250 {
251 _antList.push_back(t_antInfo());
252 _antList.back().type = t_antInfo::ARP;
253 _antList.back().xx = _Parser.antX * 1e-4;
254 _antList.back().yy = _Parser.antY * 1e-4;
255 _antList.back().zz = _Parser.antZ * 1e-4;
256 _antList.back().height = _Parser.antH * 1e-4;
257 _antList.back().height_f = true;
258 _antList.back().message = rr;
259 }
260
261 // GNSS Observations
262 // -----------------
263 else if (rr == 1 || rr == 2) {
264 decoded = true;
265
266 if (!_Parser.init) {
267 HandleHeader(&_Parser);
268 _Parser.init = 1;
269 }
270
271 if (rr == 2) {
272 emit(newMessage( (_staID + ": No valid RINEX! All values are modulo 299792.458!").toAscii(), true));
273 }
274
275 for (int ii = 0; ii < _Parser.Data.numsats; ii++) {
276 p_obs obs = new t_obs();
277 _obsList.push_back(obs);
278 if (_Parser.Data.satellites[ii] <= PRN_GPS_END) {
279 obs->_o.satSys = 'G';
280 obs->_o.satNum = _Parser.Data.satellites[ii];
281 }
282 else if (_Parser.Data.satellites[ii] <= PRN_GLONASS_END) {
283 obs->_o.satSys = 'R';
284 obs->_o.satNum = _Parser.Data.satellites[ii] - PRN_GLONASS_START + 1;
285 }
286 else {
287 obs->_o.satSys = 'S';
288 obs->_o.satNum = _Parser.Data.satellites[ii] - PRN_WAAS_START + 20;
289 }
290 obs->_o.GPSWeek = _Parser.Data.week;
291 obs->_o.GPSWeeks = _Parser.Data.timeofweek / 1000.0;
292
293 for (int jj = 0; jj < _Parser.numdatatypesGPS; jj++) {
294 int v = 0;
295 // sepearated declaration and initalization of df and pos. Perlt
296 int df;
297 int pos;
298 df = _Parser.dataflag[jj];
299 pos = _Parser.datapos[jj];
300 if ( (_Parser.Data.dataflags[ii] & df)
301 && !isnan(_Parser.Data.measdata[ii][pos])
302 && !isinf(_Parser.Data.measdata[ii][pos])) {
303 v = 1;
304 }
305 else {
306 df = _Parser.dataflagGPS[jj];
307 pos = _Parser.dataposGPS[jj];
308 if ( (_Parser.Data.dataflags[ii] & df)
309 && !isnan(_Parser.Data.measdata[ii][pos])
310 && !isinf(_Parser.Data.measdata[ii][pos])) {
311 v = 1;
312 }
313 }
314 if (!v) {
315 continue;
316 }
317 else
318 {
319 int isat = (_Parser.Data.satellites[ii] < 120
320 ? _Parser.Data.satellites[ii]
321 : _Parser.Data.satellites[ii] - 80);
322
323 // variables df and pos are used consequently. Perlt
324 if (df & GNSSDF_C1DATA) {
325 obs->_o.C1 = _Parser.Data.measdata[ii][pos];
326 }
327 else if (df & GNSSDF_C2DATA) {
328 obs->_o.C2 = _Parser.Data.measdata[ii][pos];
329 }
330 else if (df & GNSSDF_P1DATA) {
331 obs->_o.P1 = _Parser.Data.measdata[ii][pos];
332 }
333 else if (df & GNSSDF_P2DATA) {
334 obs->_o.P2 = _Parser.Data.measdata[ii][pos];
335 }
336 else if (df & (GNSSDF_L1CDATA|GNSSDF_L1PDATA)) {
337 obs->_o.L1 = _Parser.Data.measdata[ii][pos];
338 obs->_o.SNR1 = _Parser.Data.snrL1[ii];
339 obs->_o.lock_timei_L1 = _Parser.lastlockl1[isat];
340 }
341 else if (df & (GNSSDF_L2CDATA|GNSSDF_L2PDATA)) {
342 obs->_o.L2 = _Parser.Data.measdata[ii][pos];
343 obs->_o.SNR2 = _Parser.Data.snrL2[ii];
344 obs->_o.lock_timei_L2 = _Parser.lastlockl2[isat];
345 }
346 else if (df & (GNSSDF_S1CDATA|GNSSDF_S1PDATA)) {
347 obs->_o.S1 = _Parser.Data.measdata[ii][pos];
348 }
349 else if (df & (GNSSDF_S2CDATA|GNSSDF_S2PDATA)) {
350 obs->_o.S2 = _Parser.Data.measdata[ii][pos];
351 }
352 }
353 }
354 }
355 }
356
357 // GPS Ephemeris
358 // -------------
359 else if (rr == 1019) {
360 decoded = true;
361 gpsephemeris* ep = new gpsephemeris(_Parser.ephemerisGPS);
362
363#ifdef DEBUG_RTCM2_2021
364 QString msg = QString("%1: got eph %2 IODC %3 GPSweek %4 TOC %5 TOE %6")
365 .arg(_staID)
366 .arg(ep->satellite, 2)
367 .arg(ep->IODC, 4)
368 .arg(ep->GPSweek, 4)
369 .arg(ep->TOC, 6)
370 .arg(ep->TOE, 6);
371 emit newMessage(msg.toAscii(), false);
372#endif
373
374 emit newGPSEph(ep);
375 }
376
377 // GLONASS Ephemeris
378 // -----------------
379 else if (rr == 1020) {
380 decoded = true;
381 glonassephemeris* ep = new glonassephemeris(_Parser.ephemerisGLONASS);
382 emit newGlonassEph(ep);
383 }
384 }
385 }
386 }
387 if (_mode == unknown && decoded) {
388 _mode = observations;
389 }
390 }
391
392 if (decoded) {
393 return success;
394 }
395 else {
396 return failure;
397 }
398}
Note: See TracBrowser for help on using the repository browser.