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

Last change on this file since 2669 was 2669, checked in by mervart, 13 years ago
File size: 13.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
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 // Initialize a new parser
148 // -----------------------
149 if (newParser) {
150 parser.rinex3 = 0;
151 memset(&parser, 0, sizeof(parser));
152 double secGPS;
153 currentGPSWeeks(parser.GPSWeek, secGPS);
154 parser.GPSTOW = int(secGPS);
155 }
156
157 // Remaining part decodes the Observations
158 // ---------------------------------------
159 if (_mode == unknown || _mode == observations || _checkMountPoint == _staID || _checkMountPoint == "ALL") {
160
161 for (int ii = 0; ii < bufLen; ii++) {
162 parser.Message[parser.MessageSize++] = buffer[ii];
163
164 if (parser.MessageSize >= parser.NeedBytes) {
165
166 while(int rr = RTCM3Parser(&parser)) {
167
168 // RTCMv3 message types
169 // --------------------
170 _typeList.push_back(parser.blocktype);
171
172 // RTCMv3 antenna descriptor
173 // -------------------------
174 if(rr == 1007 || rr == 1008 || rr == 1033)
175 {
176 _antType.push_back(parser.antenna); /* correct ? */
177 }
178
179 // RTCMv3 antenna XYZ
180 // ------------------
181 else if(rr == 1005)
182 {
183 _antList.push_back(t_antInfo());
184 _antList.back().type = t_antInfo::ARP;
185 _antList.back().xx = parser.antX * 1e-4;
186 _antList.back().yy = parser.antY * 1e-4;
187 _antList.back().zz = parser.antZ * 1e-4;
188 _antList.back().message = rr;
189
190 // Remember station position for 1003 message decoding
191 _antXYZ[0] = parser.antX * 1e-4;
192 _antXYZ[1] = parser.antY * 1e-4;
193 _antXYZ[2] = parser.antZ * 1e-4;
194 }
195
196 // RTCMv3 antenna XYZ-H
197 // --------------------
198 else if(rr == 1006)
199 {
200 _antList.push_back(t_antInfo());
201 _antList.back().type = t_antInfo::ARP;
202 _antList.back().xx = parser.antX * 1e-4;
203 _antList.back().yy = parser.antY * 1e-4;
204 _antList.back().zz = parser.antZ * 1e-4;
205 _antList.back().height = parser.antH * 1e-4;
206 _antList.back().height_f = true;
207 _antList.back().message = rr;
208
209 // Remember station position for 1003 message decoding
210 _antXYZ[0] = parser.antX * 1e-4;
211 _antXYZ[1] = parser.antY * 1e-4;
212 _antXYZ[2] = parser.antZ * 1e-4;
213 }
214
215 // GNSS Observations
216 // -----------------
217 else if (rr == 1 || rr == 2) {
218 decoded = true;
219
220 if (!parser.init) {
221 HandleHeader(&parser);
222 parser.init = 1;
223 }
224
225 // apply "GPS Integer L1 Pseudorange Modulus Ambiguity"
226 bool applyModulusAmb = false;
227 ///if (rr == 2) {
228 /// applyModulusAmb = true;
229 ///}
230
231 if (rr == 2) {
232 emit(newMessage( (_staID + ": No valid RINEX! All values are modulo 299792.458!").toAscii(), true));
233 }
234
235 for (int ii = 0; ii < parser.Data.numsats; ii++) {
236 p_obs obs = new t_obs();
237 if (parser.Data.satellites[ii] <= PRN_GPS_END) {
238 obs->_o.satSys = 'G';
239 obs->_o.satNum = parser.Data.satellites[ii];
240 }
241 else if (parser.Data.satellites[ii] <= PRN_GLONASS_END) {
242 obs->_o.satSys = 'R';
243 obs->_o.satNum = parser.Data.satellites[ii] - PRN_GLONASS_START + 1;
244 //// obs->_o.slotNum = parser.Data.channels[ii];
245 if (obs->_o.satNum <= PRN_GLONASS_NUM &&
246 parser.GLOFreq[obs->_o.satNum-1] != 0) {
247 obs->_o.slotNum = parser.GLOFreq[obs->_o.satNum-1] - 100;
248 }
249 else {
250 delete obs;
251 obs = 0;
252 }
253 }
254 else {
255 obs->_o.satSys = 'S';
256 obs->_o.satNum = parser.Data.satellites[ii] - PRN_WAAS_START + 20;
257 }
258
259 if (obs) {
260 _obsList.push_back(obs);
261 }
262 else {
263 continue;
264 }
265
266 obs->_o.GPSWeek = parser.Data.week;
267 obs->_o.GPSWeeks = parser.Data.timeofweek / 1000.0;
268
269 // Estimate "GPS Integer L1 Pseudorange Modulus Ambiguity"
270 // -------------------------------------------------------
271 double modulusAmb = 0;
272 if (applyModulusAmb) {
273 // Missing antenna coordinates: skip all data
274 if ( !_antXYZ[0] && !_antXYZ[1] && !_antXYZ[2] ) {
275 continue;
276 }
277
278 ostringstream prns;
279 prns << obs->_o.satSys << setfill('0') << setw(2) << obs->_o.satNum;
280
281 string prn = prns.str();
282
283 // Missing ephemerides, skip satellite
284 if (_ephList.find(prn) == _ephList.end()) {
285 continue;
286 }
287
288 const t_eph* eph = &(_ephList.find(prn)->second);
289
290 double rho, xSat, ySat, zSat, clkSat, GPSWeeks_tot;
291 int GPSWeek_tot;
292 cmpRho(eph, _antXYZ[0], _antXYZ[1], _antXYZ[2],
293 obs->_o.GPSWeek, obs->_o.GPSWeeks,
294 rho, GPSWeek_tot, GPSWeeks_tot,
295 xSat, ySat, zSat, clkSat);
296
297 const double CC = 299792458.0;
298
299 int nn = static_cast<int>(rho / (CC * 0.001));
300
301 modulusAmb = nn * CC * 0.001;
302 }
303
304 // Loop over all data types
305 // ------------------------
306 for (int jj = 0; jj < parser.numdatatypesGPS; jj++) {
307 int v = 0;
308 // sepearated declaration and initalization of df and pos. Perlt
309 int df;
310 int pos;
311 df = parser.dataflag[jj];
312 pos = parser.datapos[jj];
313 if ( (parser.Data.dataflags[ii] & df)
314 && !isnan(parser.Data.measdata[ii][pos])
315 && !isinf(parser.Data.measdata[ii][pos])) {
316 v = 1;
317 }
318 else {
319 df = parser.dataflagGPS[jj];
320 pos = parser.dataposGPS[jj];
321 if ( (parser.Data.dataflags[ii] & df)
322 && !isnan(parser.Data.measdata[ii][pos])
323 && !isinf(parser.Data.measdata[ii][pos])) {
324 v = 1;
325 }
326 }
327 if (!v) {
328 continue;
329 }
330 else
331 {
332 int isat = (parser.Data.satellites[ii] < 120
333 ? parser.Data.satellites[ii]
334 : parser.Data.satellites[ii] - 80);
335
336 // variables df and pos are used consequently. Perlt
337 if (df & GNSSDF_C1DATA) {
338 obs->_o.C1 = parser.Data.measdata[ii][pos] + modulusAmb;
339 }
340 else if (df & GNSSDF_C2DATA) {
341 obs->_o.C2 = parser.Data.measdata[ii][pos] + modulusAmb;
342 }
343 else if (df & GNSSDF_P1DATA) {
344 obs->_o.P1 = parser.Data.measdata[ii][pos] + modulusAmb;
345 }
346 else if (df & GNSSDF_P2DATA) {
347 obs->_o.P2 = parser.Data.measdata[ii][pos] + modulusAmb;
348 }
349 else if (df & (GNSSDF_L1CDATA|GNSSDF_L1PDATA)) {
350 obs->_o.L1 = parser.Data.measdata[ii][pos] + modulusAmb;
351 obs->_o.SNR1 = parser.Data.snrL1[ii];
352 obs->_o.lock_timei_L1 = parser.lastlockGPSl1[isat];
353 }
354 else if (df & (GNSSDF_L2CDATA|GNSSDF_L2PDATA)) {
355 obs->_o.L2 = parser.Data.measdata[ii][pos] + modulusAmb;
356 obs->_o.SNR2 = parser.Data.snrL2[ii];
357 obs->_o.lock_timei_L2 = parser.lastlockGPSl2[isat];
358 }
359 else if (df & (GNSSDF_S1CDATA|GNSSDF_S1PDATA)) {
360 obs->_o.S1 = parser.Data.measdata[ii][pos];
361 }
362 else if (df & (GNSSDF_S2CDATA|GNSSDF_S2PDATA)) {
363 obs->_o.S2 = parser.Data.measdata[ii][pos];
364 }
365 }
366 }
367 }
368 }
369
370 // GPS Ephemeris
371 // -------------
372 else if (rr == 1019) {
373 decoded = true;
374 gpsephemeris* ep = new gpsephemeris(parser.ephemerisGPS);
375 emit newGPSEph(ep);
376 }
377
378 // GLONASS Ephemeris
379 // -----------------
380 else if (rr == 1020) {
381 decoded = true;
382 glonassephemeris* ep = new glonassephemeris(parser.ephemerisGLONASS);
383 emit newGlonassEph(ep);
384 }
385 }
386 }
387 }
388 if (!_rawFile && _mode == unknown && decoded) {
389 _mode = observations;
390 }
391 }
392
393 if (decoded) {
394 return success;
395 }
396 else {
397 return failure;
398 }
399}
400
401// Store ephemerides
402////////////////////////////////////////////////////////////////////////////////////////
403bool RTCM3Decoder::storeEph(const gpsephemeris& gpseph) {
404 t_ephGPS eph; eph.set(&gpseph);
405
406 return storeEph(eph);
407}
408
409
410bool RTCM3Decoder::storeEph(const t_ephGPS& gpseph) {
411 const double secPerWeek = 7.0 * 24.0 * 3600.0;
412 double weekold = 0.0;
413 double weeknew = gpseph.GPSweek() + gpseph.GPSweeks() / secPerWeek;
414 if ( _ephList.find(gpseph.prn()) != _ephList.end() ) {
415 weekold = _ephList.find(gpseph.prn())->second.GPSweek()
416 + _ephList.find(gpseph.prn())->second.GPSweeks() / secPerWeek;
417 }
418
419 if ( weeknew - weekold > 1.0/secPerWeek ) {
420 _ephList[gpseph.prn()] = gpseph;
421
422 return true;
423 }
424
425 return false;
426}
Note: See TracBrowser for help on using the repository browser.