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

Last change on this file since 2686 was 2686, checked in by mervart, 14 years ago
File size: 13.4 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
81 // Sub-Decoder for Clock and Orbit Corrections
82 // -------------------------------------------
83 _coDecoder = new RTCM3coDecoder(staID);
84
85 // Mode can be either observations or corrections
86 // ----------------------------------------------
87 _mode = unknown;
88
89 // Antenna position (used for decoding of message 1003)
90 // ----------------------------------------------------
91 _antXYZ[0] = _antXYZ[1] = _antXYZ[2] = 0;
92
93}
94
95// Destructor
96////////////////////////////////////////////////////////////////////////////
97RTCM3Decoder::~RTCM3Decoder() {
98 delete _coDecoder;
99}
100
101//
102////////////////////////////////////////////////////////////////////////////
103t_irc RTCM3Decoder::Decode(char* buffer, int bufLen, vector<string>& errmsg) {
104
105 errmsg.clear();
106
107 bool decoded = false;
108
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
120 // Try to decode Clock and Orbit Corrections
121 // -----------------------------------------
122 if (_mode == unknown || _mode == corrections) {
123 if ( _coDecoder->Decode(buffer, bufLen, errmsg) == success ) {
124 decoded = true;
125 if (_mode == unknown) {
126 if (_rawFile) {
127 _staID_corrections = _rawFile->staID();
128 }
129 else {
130 _mode = corrections;
131 }
132 }
133 }
134 }
135
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
147 // Get Glonass Slot Numbers from Global Array
148 // ------------------------------------------
149 bncApp* app = (bncApp*) qApp;
150 app->getGlonassSlotNums(parser.GLOFreq);
151
152 // Initialize a new parser
153 // -----------------------
154 if (newParser) {
155 memset(&parser, 0, sizeof(parser));
156 parser.rinex3 = 0;
157 double secGPS;
158 currentGPSWeeks(parser.GPSWeek, secGPS);
159 parser.GPSTOW = int(secGPS);
160 }
161
162 // Remaining part decodes the Observations
163 // ---------------------------------------
164 if (_mode == unknown || _mode == observations ||
165 _checkMountPoint == _staID || _checkMountPoint == "ALL") {
166
167 for (int iByte = 0; iByte < bufLen; iByte++) {
168
169 parser.Message[parser.MessageSize++] = buffer[iByte];
170
171 if (parser.MessageSize >= parser.NeedBytes) {
172
173 while (int rr = RTCM3Parser(&parser)) {
174
175 // RTCMv3 message types
176 // --------------------
177 _typeList.push_back(parser.blocktype);
178
179 // RTCMv3 antenna descriptor
180 // -------------------------
181 if (rr == 1007 || rr == 1008 || rr == 1033) {
182 _antType.push_back(parser.antenna);
183 }
184
185 // RTCMv3 antenna XYZ
186 // ------------------
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;
194
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;
199 }
200
201 // RTCMv3 antenna XYZ-H
202 // --------------------
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;
212
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;
217 }
218
219 // GNSS Observations
220 // -----------------
221 else if (rr == 1 || rr == 2) {
222 decoded = true;
223
224 if (!parser.init) {
225 HandleHeader(&parser);
226 parser.init = 1;
227 }
228
229 if (rr == 2) {
230 emit(newMessage( (_staID +
231 ": No valid RINEX! All values are modulo 299792.458!").toAscii(),
232 true));
233 }
234
235 gnssdata& gnssData = parser.Data;
236
237 for (int iSat = 0; iSat < gnssData.numsats; iSat++) {
238
239 p_obs obs = new t_obs();
240 int satID = gnssData.satellites[iSat];
241
242 // GPS
243 // ---
244 if (satID >= PRN_GPS_START && satID <= PRN_GPS_END) {
245 obs->_o.satSys = 'G';
246 obs->_o.satNum = satID;
247 }
248
249 // Glonass
250 // -------
251 else if (satID >= PRN_GLONASS_START && satID <= PRN_GLONASS_END) {
252 obs->_o.satSys = 'R';
253 obs->_o.satNum = satID - PRN_GLONASS_START + 1;
254 if (obs->_o.satNum <= PRN_GLONASS_NUM &&
255 parser.GLOFreq[obs->_o.satNum-1] != 0) {
256 obs->_o.slotNum = parser.GLOFreq[obs->_o.satNum-1] - 100;
257 }
258 else {
259 delete obs;
260 obs = 0;
261 }
262 }
263
264 // Galileo
265 // -------
266 else if (satID >= PRN_GALILEO_START && satID <= PRN_GALILEO_END) {
267 obs->_o.satSys = 'E';
268 obs->_o.satNum = satID - PRN_GALILEO_START + 1;
269 }
270
271 // WAAS
272 // ----
273 else if (satID >= PRN_WAAS_START && satID <= PRN_WAAS_END) {
274 obs->_o.satSys = 'S';
275 obs->_o.satNum = satID - PRN_WAAS_START + 20;
276 }
277
278 // Giove A and B
279 // -------------
280 else if (satID >= PRN_GIOVE_START && satID <= PRN_GIOVE_END) {
281 obs->_o.satSys = 'E';
282 obs->_o.satNum = satID - PRN_GIOVE_START + PRN_GIOVE_OFFSET;
283 }
284
285 // Unknown System
286 // --------------
287 else {
288 delete obs;
289 obs = 0;
290 }
291
292 if (obs) {
293 _obsList.push_back(obs);
294 }
295 else {
296 continue;
297 }
298
299 obs->_o.GPSWeek = gnssData.week;
300 obs->_o.GPSWeeks = gnssData.timeofweek / 1000.0;
301
302 // Loop over all data types
303 // ------------------------
304 for (int iEntry = 0; iEntry < GNSSENTRY_NUMBER; ++iEntry) {
305
306 unsigned df = (1 << iEntry);
307
308 //// beg test
309 cout << obs->_o.satSys << obs->_o.satNum << " "
310 << iEntry << " " << df;
311 if (df & gnssData.dataflags[iSat]) {
312 cout << " present";
313 }
314 cout << endl;
315 //// end test
316
317 if (df & gnssData.dataflags[iSat]) {
318
319 if (iEntry == GNSSENTRY_C1DATA) {
320 obs->_o.C1 = gnssData.measdata[iSat][iEntry];
321 }
322 else if (iEntry == GNSSENTRY_C2DATA) {
323 obs->_o.C2 = gnssData.measdata[iSat][iEntry];
324 }
325 else if (iEntry == GNSSENTRY_P1DATA) {
326 obs->_o.P1 = gnssData.measdata[iSat][iEntry];
327 }
328 else if (iEntry == GNSSENTRY_P2DATA) {
329 obs->_o.P2 = gnssData.measdata[iSat][iEntry];
330 }
331 else if (iEntry == GNSSENTRY_L1CDATA ||
332 iEntry == GNSSENTRY_L1PDATA) {
333 obs->_o.L1 = gnssData.measdata[iSat][iEntry];
334 obs->_o.SNR1 = gnssData.snrL1[iSat];
335 if (GNSSDF2_LOCKLOSSL1 & gnssData.dataflags2[iSat]) {
336 ++obs->_o.slip_cnt_L1;
337 }
338 }
339 else if (iEntry == GNSSENTRY_L2CDATA ||
340 iEntry == GNSSENTRY_L2PDATA) {
341 obs->_o.L2 = gnssData.measdata[iSat][iEntry];
342 obs->_o.SNR2 = gnssData.snrL2[iSat];
343 if (GNSSDF2_LOCKLOSSL2 & gnssData.dataflags2[iSat]) {
344 ++obs->_o.slip_cnt_L2;
345 }
346 }
347 else if (iEntry == GNSSENTRY_S1CDATA ||
348 iEntry == GNSSENTRY_S1PDATA) {
349 obs->_o.S1 = gnssData.measdata[iSat][iEntry];
350 }
351 else if (iEntry == GNSSENTRY_S2CDATA ||
352 iEntry == GNSSENTRY_S2PDATA) {
353 obs->_o.S2 = gnssData.measdata[iSat][iEntry];
354 }
355
356 // New Carriers
357 // ------------
358 else if (iEntry == GNSSENTRY_C5DATA) {
359 obs->_o.C5 = gnssData.measdata[iSat][iEntry];
360 }
361 else if (iEntry == GNSSENTRY_L5DATA) {
362 obs->_o.L5 = gnssData.measdata[iSat][iEntry];
363 if (GNSSDF2_LOCKLOSSL5 & gnssData.dataflags2[iSat]) {
364 ++obs->_o.slip_cnt_L5;
365 }
366 }
367 else if (iEntry == GNSSENTRY_S5DATA) {
368 obs->_o.S5 = gnssData.measdata[iSat][iEntry];
369 }
370 }
371 }
372 }
373 }
374
375 // GPS Ephemeris
376 // -------------
377 else if (rr == 1019) {
378 decoded = true;
379 emit newGPSEph(new gpsephemeris(parser.ephemerisGPS));
380 }
381
382 // GLONASS Ephemeris
383 // -----------------
384 else if (rr == 1020) {
385 decoded = true;
386 emit newGlonassEph(new glonassephemeris(parser.ephemerisGLONASS));
387 }
388 }
389 }
390 }
391 if (!_rawFile && _mode == unknown && decoded) {
392 _mode = observations;
393 }
394 }
395
396 if (decoded) {
397 app->storeGlonassSlotNums(parser.GLOFreq);
398 return success;
399 }
400 else {
401 return failure;
402 }
403}
404
405// Store ephemerides
406//////////////////////////////////////////////////////////////////////////////
407bool RTCM3Decoder::storeEph(const gpsephemeris& gpseph) {
408 t_ephGPS eph; eph.set(&gpseph);
409
410 return storeEph(eph);
411}
412
413
414bool RTCM3Decoder::storeEph(const t_ephGPS& gpseph) {
415 const double secPerWeek = 7.0 * 24.0 * 3600.0;
416 double weekold = 0.0;
417 double weeknew = gpseph.GPSweek() + gpseph.GPSweeks() / secPerWeek;
418 if ( _ephList.find(gpseph.prn()) != _ephList.end() ) {
419 weekold = _ephList.find(gpseph.prn())->second.GPSweek()
420 + _ephList.find(gpseph.prn())->second.GPSweeks() / secPerWeek;
421 }
422
423 if ( weeknew - weekold > 1.0/secPerWeek ) {
424 _ephList[gpseph.prn()] = gpseph;
425
426 return true;
427 }
428
429 return false;
430}
Note: See TracBrowser for help on using the repository browser.