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

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