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

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

Zdenek Lukes: added output of Glonass channel number in output binary format
File size: 12.2 KB
RevLine 
[297]1// Part of BNC, a utility for retrieving decoding and
[464]2// converting GNSS data streams from NTRIP broadcasters.
[297]3//
[464]4// Copyright (C) 2007
[297]5// German Federal Agency for Cartography and Geodesy (BKG)
6// http://www.bkg.bund.de
[464]7// Czech Technical University Prague, Department of Geodesy
[297]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.
[296]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>
[1807]42#include <iomanip>
43#include <sstream>
[296]44#include <math.h>
[585]45#include <string.h>
[296]46
47#include "RTCM3Decoder.h"
[1807]48#include "../RTCM/rtcm_utils.h"
[296]49#include "bncconst.h"
[511]50#include "bncapp.h"
[1535]51#include "bncutils.h"
52#include "bncsettings.h"
[296]53
54using namespace std;
55
56#ifndef isinf
57# define isinf(x) 0
58#endif
59
[320]60// Error Handling
61////////////////////////////////////////////////////////////////////////////
[504]62void RTCM3Error(const char*, ...) {
[505]63}
[320]64
[296]65// Constructor
66////////////////////////////////////////////////////////////////////////////
[970]67RTCM3Decoder::RTCM3Decoder(const QString& staID) : GPSDecoder() {
[505]68
[1535]69 bncSettings settings;
[1580]70 _checkMountPoint = settings.value("miscMount").toString();
[1022]71 _staID = staID;
72
73 // Ensure, that the Decoder uses the "old" convention for the data structure for Rinex2. Perlt
[689]74 _Parser.rinex3 = 0;
75
[505]76 memset(&_Parser, 0, sizeof(_Parser));
[880]77
[1153]78 double secGPS;
79 currentGPSWeeks(_Parser.GPSWeek, secGPS);
80 _Parser.GPSTOW = int(secGPS);
81
[939]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
[880]87 // Sub-Decoder for Clock and Orbit Corrections
88 // -------------------------------------------
[970]89 _coDecoder = new RTCM3coDecoder(staID);
[1021]90
91 // Mode can be either observations or corrections
92 // ----------------------------------------------
93 _mode = unknown;
[1807]94
95 // Antenna position (used for decoding of message 1003)
96 // ----------------------------------------------------
97 _antXYZ[0] = _antXYZ[1] = _antXYZ[2] = 0;
98
[296]99}
100
101// Destructor
102////////////////////////////////////////////////////////////////////////////
103RTCM3Decoder::~RTCM3Decoder() {
[880]104 delete _coDecoder;
[296]105}
106
107//
108////////////////////////////////////////////////////////////////////////////
[1218]109t_irc RTCM3Decoder::Decode(char* buffer, int bufLen, vector<string>& errmsg) {
[508]110
[1218]111 errmsg.clear();
112
[913]113 bool decoded = false;
114
[880]115 // Try to decode Clock and Orbit Corrections
116 // -----------------------------------------
[1021]117 if (_mode == unknown || _mode == corrections) {
[1218]118 if ( _coDecoder->Decode(buffer, bufLen, errmsg) == success ) {
[1021]119 decoded = true;
120 if (_mode == unknown) {
121 _mode = corrections;
122 }
123 }
[913]124 }
[880]125
126 // Remaining part decodes the Observations
127 // ---------------------------------------
[1033]128 if (_mode == unknown || _mode == observations || _checkMountPoint == _staID || _checkMountPoint == "ALL") {
[1127]129
[1021]130 for (int ii = 0; ii < bufLen; ii++) {
131 _Parser.Message[_Parser.MessageSize++] = buffer[ii];
[1127]132
[1021]133 if (_Parser.MessageSize >= _Parser.NeedBytes) {
[1185]134
[1239]135 while(int rr = RTCM3Parser(&_Parser)) {
[1130]136
[1239]137 // RTCMv3 message types
138 // --------------------
139 _typeList.push_back(_Parser.blocktype);
[1185]140
[1239]141 // RTCMv3 antenna descriptor
142 // -------------------------
143 if(rr == 1007 || rr == 1008 || rr == 1033)
144 {
145 _antType.push_back(_Parser.antenna); /* correct ? */
146 }
[1185]147
[1239]148 // RTCMv3 antenna XYZ
149 // ------------------
150 else if(rr == 1005)
151 {
[1268]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;
[1807]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;
[1239]163 }
[1033]164
[1239]165 // RTCMv3 antenna XYZ-H
166 // --------------------
167 else if(rr == 1006)
168 {
[1268]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;
[1807]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;
[1239]182 }
183
[1021]184 // GNSS Observations
185 // -----------------
[1239]186 else if (rr == 1 || rr == 2) {
[1127]187 decoded = true;
[1021]188
189 if (!_Parser.init) {
190 HandleHeader(&_Parser);
191 _Parser.init = 1;
192 }
193
[1807]194 // apply "GPS Integer L1 Pseudorange Modulus Ambiguity"
195 bool applyModulusAmb = false;
196 ///if (rr == 2) {
197 /// applyModulusAmb = true;
198 ///}
199
[1021]200 if (rr == 2) {
[1299]201 emit(newMessage( (_staID + ": No valid RINEX! All values are modulo 299792.458!").toAscii(), true));
[1021]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];
[366]210 }
[1021]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;
[1909]214 obs->_o.slot = _Parser.Data.channels[ii];
[1021]215 }
[511]216 else {
[1021]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;
[1807]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 }
[1021]257
[1807]258 // Loop over all data types
259 // ------------------------
[1021]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];
[511]267 if ( (_Parser.Data.dataflags[ii] & df)
268 && !isnan(_Parser.Data.measdata[ii][pos])
269 && !isinf(_Parser.Data.measdata[ii][pos])) {
[1021]270 v = 1;
[511]271 }
[1021]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])) {
[1126]278 v = 1;
[1021]279 }
[627]280 }
[1021]281 if (!v) {
282 continue;
[627]283 }
[1021]284 else
285 {
[1126]286 int isat = (_Parser.Data.satellites[ii] < 120
287 ? _Parser.Data.satellites[ii]
288 : _Parser.Data.satellites[ii] - 80);
289
[1021]290 // variables df and pos are used consequently. Perlt
291 if (df & GNSSDF_C1DATA) {
[1807]292 obs->_o.C1 = _Parser.Data.measdata[ii][pos] + modulusAmb;
[1021]293 }
294 else if (df & GNSSDF_C2DATA) {
[1807]295 obs->_o.C2 = _Parser.Data.measdata[ii][pos] + modulusAmb;
[1021]296 }
297 else if (df & GNSSDF_P1DATA) {
[1807]298 obs->_o.P1 = _Parser.Data.measdata[ii][pos] + modulusAmb;
[1021]299 }
300 else if (df & GNSSDF_P2DATA) {
[1807]301 obs->_o.P2 = _Parser.Data.measdata[ii][pos] + modulusAmb;
[1021]302 }
303 else if (df & (GNSSDF_L1CDATA|GNSSDF_L1PDATA)) {
[1807]304 obs->_o.L1 = _Parser.Data.measdata[ii][pos] + modulusAmb;
[1044]305 obs->_o.SNR1 = _Parser.Data.snrL1[ii];
[1126]306 obs->_o.lock_timei_L1 = _Parser.lastlockl1[isat];
[1021]307 }
308 else if (df & (GNSSDF_L2CDATA|GNSSDF_L2PDATA)) {
[1807]309 obs->_o.L2 = _Parser.Data.measdata[ii][pos] + modulusAmb;
[1044]310 obs->_o.SNR2 = _Parser.Data.snrL2[ii];
[1126]311 obs->_o.lock_timei_L2 = _Parser.lastlockl2[isat];
[1021]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 }
[627]319 }
[511]320 }
[366]321 }
[296]322 }
[1021]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 }
[296]339 }
340 }
341 }
[1021]342 if (_mode == unknown && decoded) {
343 _mode = observations;
344 }
[296]345 }
[1021]346
347 if (decoded) {
348 return success;
[658]349 }
350 else {
[1021]351 return failure;
[658]352 }
[296]353}
[1807]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.