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

Last change on this file since 2386 was 2386, checked in by mervart, 14 years ago

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