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

Last change on this file since 3002 was 3002, checked in by mervart, 13 years ago
File size: 15.2 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 <iomanip>
43#include <sstream>
44#include <math.h>
45#include <string.h>
46
47#include "RTCM3Decoder.h"
48#include "../RTCM/rtcm_utils.h"
49#include "bncconst.h"
50#include "bncapp.h"
51#include "bncutils.h"
52#include "bncsettings.h"
53
54using namespace std;
55
56#ifndef isinf
57# define isinf(x) 0
58#endif
59
60// Error Handling
61////////////////////////////////////////////////////////////////////////////
62void RTCM3Error(const char*, ...) {
63}
64
65// Constructor
66////////////////////////////////////////////////////////////////////////////
67RTCM3Decoder::RTCM3Decoder(const QString& staID, bncRawFile* rawFile) :
68 GPSDecoder() {
69
70 _staID = staID;
71 _rawFile = rawFile;
72
73 bncSettings settings;
74 _checkMountPoint = settings.value("miscMount").toString();
75
76 connect(this, SIGNAL(newGPSEph(gpsephemeris*)),
77 (bncApp*) qApp, SLOT(slotNewGPSEph(gpsephemeris*)));
78 connect(this, SIGNAL(newGlonassEph(glonassephemeris*)),
79 (bncApp*) qApp, SLOT(slotNewGlonassEph(glonassephemeris*)));
80 connect(this, SIGNAL(newGalileoEph(galileoephemeris*)),
81 (bncApp*) qApp, SLOT(slotNewGalileoEph(galileoephemeris*)));
82
83 // Mode can be either observations or corrections
84 // ----------------------------------------------
85 _mode = unknown;
86
87 // Antenna position (used for decoding of message 1003)
88 // ----------------------------------------------------
89 _antXYZ[0] = _antXYZ[1] = _antXYZ[2] = 0;
90
91}
92
93// Destructor
94////////////////////////////////////////////////////////////////////////////
95RTCM3Decoder::~RTCM3Decoder() {
96 QMapIterator<QByteArray, RTCM3coDecoder*> it(_coDecoders);
97 while (it.hasNext()) {
98 delete it.next();
99 }
100}
101
102//
103////////////////////////////////////////////////////////////////////////////
104t_irc RTCM3Decoder::Decode(char* buffer, int bufLen, vector<string>& errmsg) {
105
106 errmsg.clear();
107
108 bool decoded = false;
109
110 // If read from file, mode is always uknown
111 // ----------------------------------------
112 if (_rawFile) {
113 _mode = unknown;
114 _staID = _rawFile->staID();
115 }
116
117 // Try to decode Clock and Orbit Corrections
118 // -----------------------------------------
119 if (_mode == unknown || _mode == corrections) {
120
121 // Find the corresponding coDecoder
122 // --------------------------------
123 if (!_coDecoders.contains(_staID.toAscii())) {
124 _coDecoders[_staID.toAscii()] = new RTCM3coDecoder(_staID);
125 }
126 RTCM3coDecoder* coDecoder = _coDecoders[_staID.toAscii()];
127
128 if ( coDecoder->Decode(buffer, bufLen, errmsg) == success ) {
129 decoded = true;
130 if (!_rawFile && _mode == unknown) {
131 _mode = corrections;
132 }
133 }
134 }
135
136 // Find the corresponding parser, initialize a new parser if necessary
137 // -------------------------------------------------------------------
138 bool newParser = !_parsers.contains(_staID.toAscii());
139 RTCM3ParserData& parser = _parsers[_staID.toAscii()];
140 if (newParser) {
141 memset(&parser, 0, sizeof(parser));
142 parser.rinex3 = 0;
143 double secGPS;
144 currentGPSWeeks(parser.GPSWeek, secGPS);
145 parser.GPSTOW = int(secGPS);
146 }
147
148 // Get Glonass Slot Numbers from Global Array
149 // ------------------------------------------
150 bncApp* app = (bncApp*) qApp;
151 app->getGlonassSlotNums(parser.GLOFreq);
152
153 // Remaining part decodes the Observations
154 // ---------------------------------------
155 if (_mode == unknown || _mode == observations ||
156 _checkMountPoint == _staID || _checkMountPoint == "ALL") {
157
158 for (int iByte = 0; iByte < bufLen; iByte++) {
159
160 parser.Message[parser.MessageSize++] = buffer[iByte];
161
162 if (parser.MessageSize >= parser.NeedBytes) {
163
164 while (int rr = RTCM3Parser(&parser)) {
165
166 // RTCMv3 message types
167 // --------------------
168 _typeList.push_back(parser.blocktype);
169
170 // RTCMv3 antenna descriptor
171 // -------------------------
172 if (rr == 1007 || rr == 1008 || rr == 1033) {
173 _antType.push_back(parser.antenna);
174 }
175
176 // RTCMv3 antenna XYZ
177 // ------------------
178 else if (rr == 1005) {
179 _antList.push_back(t_antInfo());
180 _antList.back().type = t_antInfo::ARP;
181 _antList.back().xx = parser.antX * 1e-4;
182 _antList.back().yy = parser.antY * 1e-4;
183 _antList.back().zz = parser.antZ * 1e-4;
184 _antList.back().message = rr;
185
186 // Remember station position for 1003 message decoding
187 _antXYZ[0] = parser.antX * 1e-4;
188 _antXYZ[1] = parser.antY * 1e-4;
189 _antXYZ[2] = parser.antZ * 1e-4;
190 }
191
192 // RTCMv3 antenna XYZ-H
193 // --------------------
194 else if(rr == 1006) {
195 _antList.push_back(t_antInfo());
196 _antList.back().type = t_antInfo::ARP;
197 _antList.back().xx = parser.antX * 1e-4;
198 _antList.back().yy = parser.antY * 1e-4;
199 _antList.back().zz = parser.antZ * 1e-4;
200 _antList.back().height = parser.antH * 1e-4;
201 _antList.back().height_f = true;
202 _antList.back().message = rr;
203
204 // Remember station position for 1003 message decoding
205 _antXYZ[0] = parser.antX * 1e-4;
206 _antXYZ[1] = parser.antY * 1e-4;
207 _antXYZ[2] = parser.antZ * 1e-4;
208 }
209
210 // GNSS Observations
211 // -----------------
212 else if (rr == 1 || rr == 2) {
213 decoded = true;
214
215 if (!parser.init) {
216 HandleHeader(&parser);
217 parser.init = 1;
218 }
219
220 if (rr == 2) {
221 emit(newMessage( (_staID +
222 ": No valid RINEX! All values are modulo 299792.458!").toAscii(),
223 true));
224 }
225
226 gnssdata& gnssData = parser.Data;
227
228 for (int iSat = 0; iSat < gnssData.numsats; iSat++) {
229
230 t_obs obs;
231 int satID = gnssData.satellites[iSat];
232
233 // GPS
234 // ---
235 if (satID >= PRN_GPS_START && satID <= PRN_GPS_END) {
236 obs.satSys = 'G';
237 obs.satNum = satID;
238 }
239
240 // Glonass
241 // -------
242 else if (satID >= PRN_GLONASS_START && satID <= PRN_GLONASS_END) {
243 obs.satSys = 'R';
244 obs.satNum = satID - PRN_GLONASS_START + 1;
245 if (obs.satNum <= PRN_GLONASS_NUM &&
246 parser.GLOFreq[obs.satNum-1] != 0) {
247 obs.slotNum = parser.GLOFreq[obs.satNum-1] - 100;
248 }
249 else {
250 continue;
251 }
252 }
253
254 // Galileo
255 // -------
256 else if (satID >= PRN_GALILEO_START && satID <= PRN_GALILEO_END) {
257 obs.satSys = 'E';
258 obs.satNum = satID - PRN_GALILEO_START + 1;
259 }
260
261 // WAAS
262 // ----
263 else if (satID >= PRN_WAAS_START && satID <= PRN_WAAS_END) {
264 obs.satSys = 'S';
265 obs.satNum = satID - PRN_WAAS_START + 20;
266 }
267
268 // Giove A and B
269 // -------------
270 else if (satID >= PRN_GIOVE_START && satID <= PRN_GIOVE_END) {
271 obs.satSys = 'E';
272 obs.satNum = satID - PRN_GIOVE_START + PRN_GIOVE_OFFSET;
273 }
274
275 // Unknown System
276 // --------------
277 else {
278 continue;
279 }
280
281 obs.GPSWeek = gnssData.week;
282 obs.GPSWeeks = gnssData.timeofweek / 1000.0;
283
284 QString prn = QString("%1%2").arg(obs.satSys)
285 .arg(obs.satNum, 2, 10, QChar('0'));
286
287 // Handle loss-of-lock flags
288 // -------------------------
289 const int maxSlipCnt = 100;
290 if (!_slip_cnt_L1.contains(prn)) {
291 _slip_cnt_L1[prn] = 0;
292 _slip_cnt_L2[prn] = 0;
293 _slip_cnt_L5[prn] = 0;
294 }
295 if (GNSSDF2_LOCKLOSSL1 & gnssData.dataflags2[iSat]) {
296 if (_slip_cnt_L1[prn] < maxSlipCnt) {
297 ++_slip_cnt_L1[prn];
298 }
299 else {
300 _slip_cnt_L1[prn] = 1;
301 }
302 obs.slip_cnt_L1 = _slip_cnt_L1[prn];
303 }
304 if (GNSSDF2_LOCKLOSSL2 & gnssData.dataflags2[iSat]) {
305 if (_slip_cnt_L2[prn] < maxSlipCnt) {
306 ++_slip_cnt_L2[prn];
307 }
308 else {
309 _slip_cnt_L2[prn] = 1;
310 }
311 obs.slip_cnt_L2 = _slip_cnt_L2[prn];
312 }
313 if (GNSSDF2_LOCKLOSSL5 & gnssData.dataflags2[iSat]) {
314 if (_slip_cnt_L5[prn] < maxSlipCnt) {
315 ++_slip_cnt_L5[prn];
316 }
317 else {
318 _slip_cnt_L5[prn] = 1;
319 }
320 obs.slip_cnt_L5 = _slip_cnt_L5[prn];
321 }
322
323 // Loop over all data types
324 // ------------------------
325 for (int iEntry = 0; iEntry < GNSSENTRY_NUMBER; ++iEntry) {
326
327 unsigned df = (1 << iEntry);
328
329 if (df & gnssData.dataflags[iSat]) {
330
331 if (iEntry == GNSSENTRY_C1DATA) {
332 obs.C1 = gnssData.measdata[iSat][iEntry];
333 }
334 else if (iEntry == GNSSENTRY_C2DATA) {
335 obs.C2 = gnssData.measdata[iSat][iEntry];
336 }
337 else if (iEntry == GNSSENTRY_P1DATA) {
338 obs.P1 = gnssData.measdata[iSat][iEntry];
339 }
340 else if (iEntry == GNSSENTRY_P2DATA) {
341 obs.P2 = gnssData.measdata[iSat][iEntry];
342 }
343 else if (iEntry == GNSSENTRY_L1CDATA) {
344 obs.L1C = gnssData.measdata[iSat][iEntry];
345 }
346 else if (iEntry == GNSSENTRY_L1PDATA) {
347 obs.L1P = gnssData.measdata[iSat][iEntry];
348 }
349 else if (iEntry == GNSSENTRY_L2CDATA) {
350 obs.L2C = gnssData.measdata[iSat][iEntry];
351 }
352 else if (iEntry == GNSSENTRY_L2PDATA) {
353 obs.L2P = gnssData.measdata[iSat][iEntry];
354 }
355 else if (iEntry == GNSSENTRY_D1CDATA) {
356 obs.D1C = gnssData.measdata[iSat][iEntry];
357 }
358 else if (iEntry == GNSSENTRY_D1PDATA) {
359 obs.D1P = gnssData.measdata[iSat][iEntry];
360 }
361 else if (iEntry == GNSSENTRY_S1CDATA) {
362 obs.S1C = gnssData.measdata[iSat][iEntry];
363 }
364 else if (iEntry == GNSSENTRY_S1PDATA) {
365 obs.S1P = gnssData.measdata[iSat][iEntry];
366 }
367 else if (iEntry == GNSSENTRY_D2CDATA) {
368 obs.D2C = gnssData.measdata[iSat][iEntry];
369 }
370 else if (iEntry == GNSSENTRY_D2PDATA) {
371 obs.D2P = gnssData.measdata[iSat][iEntry];
372 }
373 else if (iEntry == GNSSENTRY_S2CDATA) {
374 obs.S2C = gnssData.measdata[iSat][iEntry];
375 }
376 else if (iEntry == GNSSENTRY_S2PDATA) {
377 obs.S2P = gnssData.measdata[iSat][iEntry];
378 }
379 else if (iEntry == GNSSENTRY_C5DATA) {
380 obs.C5 = gnssData.measdata[iSat][iEntry];
381 }
382 else if (iEntry == GNSSENTRY_L5DATA) {
383 obs.L5 = gnssData.measdata[iSat][iEntry];
384 }
385 else if (iEntry == GNSSENTRY_D5DATA) {
386 obs.D5 = gnssData.measdata[iSat][iEntry];
387 }
388 else if (iEntry == GNSSENTRY_S5DATA) {
389 obs.S5 = gnssData.measdata[iSat][iEntry];
390 }
391 }
392 }
393 _obsList.push_back(obs);
394 }
395 }
396
397 // GPS Ephemeris
398 // -------------
399 else if (rr == 1019) {
400 decoded = true;
401 emit newGPSEph(new gpsephemeris(parser.ephemerisGPS));
402 }
403
404 // GLONASS Ephemeris
405 // -----------------
406 else if (rr == 1020) {
407 decoded = true;
408 emit newGlonassEph(new glonassephemeris(parser.ephemerisGLONASS));
409 }
410
411 // Galileo Ephemeris
412 // -----------------
413 else if (rr == 1045) {
414 decoded = true;
415 emit newGalileoEph(new galileoephemeris(parser.ephemerisGALILEO));
416 }
417 }
418 }
419 }
420 if (!_rawFile && _mode == unknown && decoded) {
421 _mode = observations;
422 }
423 }
424
425 if (decoded) {
426 app->storeGlonassSlotNums(parser.GLOFreq);
427 return success;
428 }
429 else {
430 return failure;
431 }
432}
433
434// Store ephemerides
435//////////////////////////////////////////////////////////////////////////////
436bool RTCM3Decoder::storeEph(const gpsephemeris& gpseph) {
437 t_ephGPS eph; eph.set(&gpseph);
438
439 return storeEph(eph);
440}
441
442
443bool RTCM3Decoder::storeEph(const t_ephGPS& gpseph) {
444 const double secPerWeek = 7.0 * 24.0 * 3600.0;
445 double weekold = 0.0;
446 double weeknew = gpseph.GPSweek() + gpseph.GPSweeks() / secPerWeek;
447 if ( _ephList.find(gpseph.prn()) != _ephList.end() ) {
448 weekold = _ephList.find(gpseph.prn())->second.GPSweek()
449 + _ephList.find(gpseph.prn())->second.GPSweeks() / secPerWeek;
450 }
451
452 if ( weeknew - weekold > 1.0/secPerWeek ) {
453 _ephList[gpseph.prn()] = gpseph;
454
455 return true;
456 }
457
458 return false;
459}
460
461// Time of Corrections
462//////////////////////////////////////////////////////////////////////////////
463int RTCM3Decoder::corrGPSEpochTime() const {
464 if (_mode == corrections && _coDecoders.size() > 0) {
465 return _coDecoders.begin().value()->corrGPSEpochTime();
466 }
467 else {
468 return -1;
469 }
470}
Note: See TracBrowser for help on using the repository browser.