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

Last change on this file since 1807 was 1807, checked in by zdenek, 15 years ago

ZL: FEATURE: changes for decoding of message 1003 from RTCM 3.0 streams
File size: 12.1 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 }
215 else {
216 obs->_o.satSys = 'S';
217 obs->_o.satNum = _Parser.Data.satellites[ii] - PRN_WAAS_START + 20;
218 }
219 obs->_o.GPSWeek = _Parser.Data.week;
220 obs->_o.GPSWeeks = _Parser.Data.timeofweek / 1000.0;
221
222 // Estimate "GPS Integer L1 Pseudorange Modulus Ambiguity"
223 // -------------------------------------------------------
224 double modulusAmb = 0;
225 if (applyModulusAmb) {
226 // Missing antenna coordinates: skip all data
227 if ( !_antXYZ[0] && !_antXYZ[1] && !_antXYZ[2] ) {
228 continue;
229 }
230
231 ostringstream prns;
232 prns << obs->_o.satSys << setfill('0') << setw(2) << obs->_o.satNum;
233
234 string prn = prns.str();
235
236 // Missing ephemerides, skip satellite
237 if (_ephList.find(prn) == _ephList.end()) {
238 continue;
239 }
240
241 const t_eph* eph = &(_ephList.find(prn)->second);
242
243 double rho, xSat, ySat, zSat, clkSat, GPSWeeks_tot;
244 int GPSWeek_tot;
245 cmpRho(eph, _antXYZ[0], _antXYZ[1], _antXYZ[2],
246 obs->_o.GPSWeek, obs->_o.GPSWeeks,
247 rho, GPSWeek_tot, GPSWeeks_tot,
248 xSat, ySat, zSat, clkSat);
249
250 const double CC = 299792458.0;
251
252 int nn = static_cast<int>(rho / (CC * 0.001));
253
254 modulusAmb = nn * CC * 0.001;
255 }
256
257 // Loop over all data types
258 // ------------------------
259 for (int jj = 0; jj < _Parser.numdatatypesGPS; jj++) {
260 int v = 0;
261 // sepearated declaration and initalization of df and pos. Perlt
262 int df;
263 int pos;
264 df = _Parser.dataflag[jj];
265 pos = _Parser.datapos[jj];
266 if ( (_Parser.Data.dataflags[ii] & df)
267 && !isnan(_Parser.Data.measdata[ii][pos])
268 && !isinf(_Parser.Data.measdata[ii][pos])) {
269 v = 1;
270 }
271 else {
272 df = _Parser.dataflagGPS[jj];
273 pos = _Parser.dataposGPS[jj];
274 if ( (_Parser.Data.dataflags[ii] & df)
275 && !isnan(_Parser.Data.measdata[ii][pos])
276 && !isinf(_Parser.Data.measdata[ii][pos])) {
277 v = 1;
278 }
279 }
280 if (!v) {
281 continue;
282 }
283 else
284 {
285 int isat = (_Parser.Data.satellites[ii] < 120
286 ? _Parser.Data.satellites[ii]
287 : _Parser.Data.satellites[ii] - 80);
288
289 // variables df and pos are used consequently. Perlt
290 if (df & GNSSDF_C1DATA) {
291 obs->_o.C1 = _Parser.Data.measdata[ii][pos] + modulusAmb;
292 }
293 else if (df & GNSSDF_C2DATA) {
294 obs->_o.C2 = _Parser.Data.measdata[ii][pos] + modulusAmb;
295 }
296 else if (df & GNSSDF_P1DATA) {
297 obs->_o.P1 = _Parser.Data.measdata[ii][pos] + modulusAmb;
298 }
299 else if (df & GNSSDF_P2DATA) {
300 obs->_o.P2 = _Parser.Data.measdata[ii][pos] + modulusAmb;
301 }
302 else if (df & (GNSSDF_L1CDATA|GNSSDF_L1PDATA)) {
303 obs->_o.L1 = _Parser.Data.measdata[ii][pos] + modulusAmb;
304 obs->_o.SNR1 = _Parser.Data.snrL1[ii];
305 obs->_o.lock_timei_L1 = _Parser.lastlockl1[isat];
306 }
307 else if (df & (GNSSDF_L2CDATA|GNSSDF_L2PDATA)) {
308 obs->_o.L2 = _Parser.Data.measdata[ii][pos] + modulusAmb;
309 obs->_o.SNR2 = _Parser.Data.snrL2[ii];
310 obs->_o.lock_timei_L2 = _Parser.lastlockl2[isat];
311 }
312 else if (df & (GNSSDF_S1CDATA|GNSSDF_S1PDATA)) {
313 obs->_o.S1 = _Parser.Data.measdata[ii][pos];
314 }
315 else if (df & (GNSSDF_S2CDATA|GNSSDF_S2PDATA)) {
316 obs->_o.S2 = _Parser.Data.measdata[ii][pos];
317 }
318 }
319 }
320 }
321 }
322
323 // GPS Ephemeris
324 // -------------
325 else if (rr == 1019) {
326 decoded = true;
327 gpsephemeris* ep = new gpsephemeris(_Parser.ephemerisGPS);
328 emit newGPSEph(ep);
329 }
330
331 // GLONASS Ephemeris
332 // -----------------
333 else if (rr == 1020) {
334 decoded = true;
335 glonassephemeris* ep = new glonassephemeris(_Parser.ephemerisGLONASS);
336 emit newGlonassEph(ep);
337 }
338 }
339 }
340 }
341 if (_mode == unknown && decoded) {
342 _mode = observations;
343 }
344 }
345
346 if (decoded) {
347 return success;
348 }
349 else {
350 return failure;
351 }
352}
353
354// Store ephemerides
355////////////////////////////////////////////////////////////////////////////////////////
356bool RTCM3Decoder::storeEph(const gpsephemeris& gpseph) {
357 t_ephGPS eph; eph.set(&gpseph);
358
359 return storeEph(eph);
360}
361
362
363bool RTCM3Decoder::storeEph(const t_ephGPS& gpseph) {
364 double weekold = 0.0;
365 double weeknew = gpseph.GPSweek() + gpseph.GPSweeks() / 86400.0;
366 if ( _ephList.find(gpseph.prn()) != _ephList.end() ) {
367 weekold = _ephList.find(gpseph.prn())->second.GPSweek()
368 + _ephList.find(gpseph.prn())->second.GPSweeks() / 86400.0;
369 }
370
371 if ( weeknew - weekold > 1/86400.0 ) {
372 _ephList[gpseph.prn()] = gpseph;
373
374 return true;
375 }
376
377 return false;
378}
Note: See TracBrowser for help on using the repository browser.