source: ntrip/branches/BNC_LM/RTCM3/RTCM3Decoder.cpp@ 9267

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