source: ntrip/trunk/BNC/src/RTCM3/RTCM3Decoder.cpp@ 6595

Last change on this file since 6595 was 6595, checked in by stuerze, 9 years ago

some preparation to read RTCM3 Compass ephemeris messages
File size: 14.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 "bnccore.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(t_ephGPS)), BNC_CORE, SLOT(slotNewGPSEph(t_ephGPS)));
77 connect(this, SIGNAL(newGlonassEph(t_ephGlo)), BNC_CORE, SLOT(slotNewGlonassEph(t_ephGlo)));
78 connect(this, SIGNAL(newGalileoEph(t_ephGal)), BNC_CORE, SLOT(slotNewGalileoEph(t_ephGal)));
79 connect(this, SIGNAL(newSBASEph(t_ephSBAS)), BNC_CORE, SLOT(slotNewSBASEph(t_ephSBAS)));
80 connect(this, SIGNAL(newCompassEph(t_ephCompass)), BNC_CORE, SLOT(slotNewCompassEph(t_ephCompass)));
81
82 // Mode can be either observations or corrections
83 // ----------------------------------------------
84 _mode = unknown;
85
86 // Antenna position (used for decoding of message 1003)
87 // ----------------------------------------------------
88 _antXYZ[0] = _antXYZ[1] = _antXYZ[2] = 0;
89
90}
91
92// Destructor
93////////////////////////////////////////////////////////////////////////////
94RTCM3Decoder::~RTCM3Decoder() {
95 QMapIterator<QByteArray, RTCM3coDecoder*> it(_coDecoders);
96 while (it.hasNext()) {
97 it.next();
98 delete it.value();
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 // Remaining part decodes the Observations
149 // ---------------------------------------
150 if (_mode == unknown || _mode == observations ||
151 _checkMountPoint == _staID || _checkMountPoint == "ALL") {
152
153 for (int iByte = 0; iByte < bufLen; iByte++) {
154
155 parser.Message[parser.MessageSize++] = buffer[iByte];
156
157 if (parser.MessageSize >= parser.NeedBytes) {
158
159 while (int rr = RTCM3Parser(&parser)) {
160
161 // RTCMv3 message types
162 // --------------------
163 _typeList.push_back(parser.blocktype);
164
165 // RTCMv3 antenna descriptor
166 // -------------------------
167 if (rr == 1007 || rr == 1008 || rr == 1033) {
168 _antType.push_back(parser.antenna);
169 }
170
171 // RTCMv3 antenna XYZ
172 // ------------------
173 else if (rr == 1005) {
174 _antList.push_back(t_antInfo());
175 _antList.back().type = t_antInfo::ARP;
176 _antList.back().xx = parser.antX * 1e-4;
177 _antList.back().yy = parser.antY * 1e-4;
178 _antList.back().zz = parser.antZ * 1e-4;
179 _antList.back().message = rr;
180
181 // Remember station position for 1003 message decoding
182 _antXYZ[0] = parser.antX * 1e-4;
183 _antXYZ[1] = parser.antY * 1e-4;
184 _antXYZ[2] = parser.antZ * 1e-4;
185 }
186
187 // RTCMv3 antenna XYZ-H
188 // --------------------
189 else if(rr == 1006) {
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().height = parser.antH * 1e-4;
196 _antList.back().height_f = true;
197 _antList.back().message = rr;
198
199 // Remember station position for 1003 message decoding
200 _antXYZ[0] = parser.antX * 1e-4;
201 _antXYZ[1] = parser.antY * 1e-4;
202 _antXYZ[2] = parser.antZ * 1e-4;
203 }
204
205 // GNSS Observations
206 // -----------------
207 else if (rr == 1 || rr == 2) {
208 decoded = true;
209
210 if (!parser.init) {
211 HandleHeader(&parser);
212 parser.init = 1;
213 }
214
215 if (rr == 2) {
216 emit(newMessage( (_staID +
217 ": No valid RINEX! All values are modulo 299792.458!").toAscii(),
218 true));
219 }
220
221 gnssdata& gnssData = parser.Data;
222
223 for (int iSat = 0; iSat < gnssData.numsats; iSat++) {
224
225 t_satObs obs;
226 int satID = gnssData.satellites[iSat];
227
228 // GPS
229 // ---
230 if (satID >= PRN_GPS_START && satID <= PRN_GPS_END) {
231 obs._prn.set('G', satID);
232 }
233
234 // Glonass
235 // -------
236 else if (satID >= PRN_GLONASS_START && satID <= PRN_GLONASS_END) {
237 obs._prn.set('R', satID - PRN_GLONASS_START + 1);
238 }
239
240 // Galileo
241 // -------
242 else if (satID >= PRN_GALILEO_START && satID <= PRN_GALILEO_END) {
243 obs._prn.set('E', satID - PRN_GALILEO_START + 1);
244 }
245
246 // SBAS
247 // ----
248 else if (satID >= PRN_SBAS_START && satID <= PRN_SBAS_END) {
249 obs._prn.set('S', satID - PRN_SBAS_START + 20);
250 }
251
252 // Giove A and B
253 // -------------
254 else if (satID >= PRN_GIOVE_START && satID <= PRN_GIOVE_END) {
255 obs._prn.set('E', satID - PRN_GIOVE_START + PRN_GIOVE_OFFSET);
256 }
257
258 // QZSS
259 // -------------
260 else if (satID >= PRN_QZSS_START && satID <= PRN_QZSS_END) {
261 obs._prn.set('J', satID - PRN_QZSS_START + 1);
262 }
263
264 // COMPASS
265 // -------------
266 else if (satID >= PRN_COMPASS_START && satID <= PRN_COMPASS_END) {
267 obs._prn.set('C', satID - PRN_COMPASS_START + 1);
268 }
269
270 // Unknown System
271 // --------------
272 else {
273 continue;
274 }
275
276 obs._time.set(gnssData.week, gnssData.timeofweek / 1000.0);
277
278 QString prn(obs._prn.toString().c_str());
279
280 int obs_slip_cnt_L1 = 0;
281 int obs_slip_cnt_L2 = 0;
282 int obs_slip_cnt_L5 = 0;
283
284 // Handle loss-of-lock flags
285 // -------------------------
286 const int maxSlipCnt = 100;
287 if (!_slip_cnt_L1.contains(prn)) {
288 _slip_cnt_L1[prn] = 0;
289 _slip_cnt_L2[prn] = 0;
290 _slip_cnt_L5[prn] = 0;
291 }
292 if (GNSSDF2_LOCKLOSSL1 & gnssData.dataflags2[iSat]) {
293 if (_slip_cnt_L1[prn] < maxSlipCnt) {
294 ++_slip_cnt_L1[prn];
295 }
296 else {
297 _slip_cnt_L1[prn] = 1;
298 }
299 obs_slip_cnt_L1 = _slip_cnt_L1[prn];
300 }
301 if (GNSSDF2_LOCKLOSSL2 & gnssData.dataflags2[iSat]) {
302 if (_slip_cnt_L2[prn] < maxSlipCnt) {
303 ++_slip_cnt_L2[prn];
304 }
305 else {
306 _slip_cnt_L2[prn] = 1;
307 }
308 obs_slip_cnt_L2 = _slip_cnt_L2[prn];
309 }
310 if (GNSSDF2_LOCKLOSSL5 & gnssData.dataflags2[iSat]) {
311 if (_slip_cnt_L5[prn] < maxSlipCnt) {
312 ++_slip_cnt_L5[prn];
313 }
314 else {
315 _slip_cnt_L5[prn] = 1;
316 }
317 obs_slip_cnt_L5 = _slip_cnt_L5[prn];
318 }
319
320 // Loop over all data types
321 // ------------------------
322 for (int iEntry = 0; iEntry < GNSSENTRY_NUMBER; ++iEntry) {
323 if (gnssData.codetype[iSat][iEntry] == 0) {
324 continue;
325 }
326 string rnxType(gnssData.codetype[iSat][iEntry]);
327
328 t_frqObs* frqObs = 0;
329 for (unsigned iFrq = 0; iFrq < obs._obs.size(); iFrq++) {
330 if (obs._obs[iFrq]->_rnxType2ch == rnxType) {
331 frqObs = obs._obs[iFrq];
332 break;
333 }
334 }
335 if (frqObs == 0) {
336 frqObs = new t_frqObs;
337 frqObs->_rnxType2ch = rnxType;
338 obs._obs.push_back(frqObs);
339 }
340
341 switch(iEntry & 3) {
342 case GNSSENTRY_CODE:
343 frqObs->_codeValid = true;
344 frqObs->_code = gnssData.measdata[iSat][iEntry];
345 break;
346 case GNSSENTRY_PHASE:
347 frqObs->_phaseValid = true;
348 frqObs->_phase = gnssData.measdata[iSat][iEntry];
349 if (rnxType[0] == '1') {
350 frqObs->_slipCounter = obs_slip_cnt_L1;
351 }
352 else if (rnxType[0] == '2') {
353 frqObs->_slipCounter = obs_slip_cnt_L2;
354 }
355 else if (rnxType[0] == '5') {
356 frqObs->_slipCounter = obs_slip_cnt_L5;
357 }
358 break;
359 case GNSSENTRY_DOPPLER:
360 frqObs->_dopplerValid = true;
361 frqObs->_doppler = gnssData.measdata[iSat][iEntry];
362 break;
363 case GNSSENTRY_SNR:
364 frqObs->_snrValid = true;
365 frqObs->_snr = gnssData.measdata[iSat][iEntry];
366 break;
367 }
368 }
369 _obsList.push_back(obs);
370 }
371 }
372
373 // GPS Ephemeris
374 // -------------
375 else if (rr == 1019) {
376 decoded = true;
377 t_ephGPS eph; eph.set(&parser.ephemerisGPS);
378 emit newGPSEph(eph);
379 }
380
381 // GLONASS Ephemeris
382 // -----------------
383 else if (rr == 1020 && parser.ephemerisGLONASS.almanac_number >= 1 &&
384 parser.ephemerisGLONASS.almanac_number <= PRN_GLONASS_NUM) {
385 decoded = true;
386 t_ephGlo eph; eph.set(&parser.ephemerisGLONASS);
387 emit newGlonassEph(eph);
388 }
389
390 // Galileo Ephemeris
391 // -----------------
392 else if (rr == 1045 || rr == 1046) {
393 decoded = true;
394 t_ephGal eph; eph.set(&parser.ephemerisGALILEO);
395 emit newGalileoEph(eph);
396 }
397
398 // QZSS Ephemeris
399 // --------------
400 else if (rr == 1044) {
401 decoded = true;
402 t_ephGPS eph; eph.set(&parser.ephemerisGPS);
403 emit newGPSEph(eph);
404 }
405
406 // SBAS Ephemeris
407 // --------------
408 else if (rr == 1043) {
409 decoded = true;
410 t_ephSBAS eph; eph.set(&parser.ephemerisSBAS);
411 emit newSBASEph(eph);
412 }
413
414 // COMPASS Ephemeris
415 // -----------------
416 else if (rr == 63) {
417 decoded = true;
418// t_ephCompass eph; eph.set(&parser.ephemerisCompass);
419// emit newCompassEph(eph);
420 }
421 }
422 }
423 }
424 if (!_rawFile && _mode == unknown && decoded) {
425 _mode = observations;
426 }
427 }
428
429 if (decoded) {
430 return success;
431 }
432 else {
433 return failure;
434 }
435}
436
437// Time of Corrections
438//////////////////////////////////////////////////////////////////////////////
439int RTCM3Decoder::corrGPSEpochTime() const {
440 if (_mode == corrections && _coDecoders.size() > 0) {
441 return _coDecoders.begin().value()->corrGPSEpochTime();
442 }
443 else {
444 return -1;
445 }
446}
Note: See TracBrowser for help on using the repository browser.