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

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