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

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