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

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