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

Last change on this file since 1370 was 1299, checked in by zdenek, 16 years ago

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