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

Last change on this file since 2452 was 2387, checked in by mervart, 15 years ago

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