source: ntrip/trunk/BNC/src/RTCM/RTCM2Decoder.cpp@ 8081

Last change on this file since 8081 was 8025, checked in by stuerze, 8 years ago

last changes in RTCM2Decoder are reverted because the 'bug' is a feature

File size: 12.0 KB
RevLine 
[297]1// Part of BNC, a utility for retrieving decoding and
[464]2// converting GNSS data streams from NTRIP broadcasters.
[242]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.
[242]24
[464]25/* -------------------------------------------------------------------------
26 * BKG NTRIP Client
27 * -------------------------------------------------------------------------
28 *
29 * Class: RTCM2Decoder
30 *
31 * Purpose: RTCM2 Decoder
32 *
33 * Author: L. Mervart
34 *
35 * Created: 24-Aug-2006
36 *
[7528]37 * Changes:
[464]38 *
39 * -----------------------------------------------------------------------*/
40
[1044]41#include <math.h>
42#include <sstream>
43#include <iomanip>
[1218]44#include <set>
[1044]45
[242]46#include "../bncutils.h"
[1044]47#include "rtcm_utils.h"
[243]48#include "RTCM2Decoder.h"
[242]49
50using namespace std;
[1044]51using namespace rtcm2;
[242]52
[7528]53//
[242]54// Constructor
[7528]55//
[242]56
[6443]57RTCM2Decoder::RTCM2Decoder(const std::string& ID) : _ephUser(true) {
[1044]58 _ID = ID;
[242]59}
60
[7528]61//
[242]62// Destructor
[7528]63//
[242]64
65RTCM2Decoder::~RTCM2Decoder() {
66}
67
68//
[1044]69t_irc RTCM2Decoder::getStaCrd(double& xx, double& yy, double& zz) {
[6703]70 if (!_msg03.validMsg) {
[1044]71 return failure;
72 }
[6703]73
[1044]74 xx = _msg03.x + (_msg22.validMsg ? _msg22.dL1[0] : 0.0);
75 yy = _msg03.y + (_msg22.validMsg ? _msg22.dL1[1] : 0.0);
76 zz = _msg03.z + (_msg22.validMsg ? _msg22.dL1[2] : 0.0);
[242]77
[1044]78 return success;
79}
80
[1167]81//
[6703]82t_irc RTCM2Decoder::getStaCrd(double& xx, double& yy, double& zz, double& dx1,
83 double& dy1, double& dz1, double& dx2, double& dy2, double& dz2) {
[1167]84 xx = _msg03.x;
85 yy = _msg03.y;
86 zz = _msg03.z;
[1044]87
[1167]88 dx1 = (_msg22.validMsg ? _msg22.dL1[0] : 0.0);
89 dy1 = (_msg22.validMsg ? _msg22.dL1[1] : 0.0);
90 dz1 = (_msg22.validMsg ? _msg22.dL1[2] : 0.0);
91
92 dx2 = (_msg22.validMsg ? _msg22.dL2[0] : 0.0);
93 dy2 = (_msg22.validMsg ? _msg22.dL2[1] : 0.0);
94 dz2 = (_msg22.validMsg ? _msg22.dL2[2] : 0.0);
95
96 return success;
97}
98
[1044]99//
[1218]100t_irc RTCM2Decoder::Decode(char* buffer, int bufLen, vector<string>& errmsg) {
[242]101
[1218]102 errmsg.clear();
103
[242]104 _buffer.append(buffer, bufLen);
[6703]105 int refWeek;
[242]106 double refSecs;
107 currentGPSWeeks(refWeek, refSecs);
[702]108 bool decoded = false;
[242]109
[6703]110 while (true) {
[242]111 _PP.getPacket(_buffer);
112 if (!_PP.valid()) {
[702]113 if (decoded) {
114 return success;
115 } else {
116 return failure;
117 }
[242]118 }
[6703]119
[1268]120 // Store message number
121 _typeList.push_back(_PP.ID());
[242]122
[6703]123 if (_PP.ID() == 18 || _PP.ID() == 19) {
[242]124
125 _ObsBlock.extract(_PP);
126
127 if (_ObsBlock.valid()) {
[702]128 decoded = true;
[242]129
[6703]130 int epochWeek;
[242]131 double epochSecs;
132 _ObsBlock.resolveEpoch(refWeek, refSecs, epochWeek, epochSecs);
[6703]133
134 for (int iSat = 0; iSat < _ObsBlock.nSat; iSat++) {
[6137]135 t_satObs obs;
[341]136 if (_ObsBlock.PRN[iSat] > 100) {
[6137]137 obs._prn.set('R', _ObsBlock.PRN[iSat] % 100);
[6703]138 } else {
[6194]139 obs._prn.set('G', _ObsBlock.PRN[iSat]);
[6703]140 }
[6194]141 char sys = obs._prn.system();
[6137]142 obs._time.set(epochWeek, epochSecs);
[6703]143
[6137]144 t_frqObs* frqObs1C = new t_frqObs;
[6703]145 frqObs1C->_rnxType2ch = "1C";
[8025]146 frqObs1C->_codeValid = true;
[6703]147 frqObs1C->_code = _ObsBlock.rng_C1[iSat];
[6137]148 obs._obs.push_back(frqObs1C);
[2711]149
[6137]150 t_frqObs* frqObs1P = new t_frqObs;
[6703]151 frqObs1P->_rnxType2ch = (sys == 'G') ? "1W" : "1P";
[8025]152 frqObs1P->_codeValid = true;
[6703]153 frqObs1P->_code = _ObsBlock.rng_P1[iSat];
[8025]154 frqObs1P->_phaseValid = true;
[6703]155 frqObs1P->_phase = _ObsBlock.resolvedPhase_L1(iSat);
[7835]156 //frqObs1P->_slipCounter = _ObsBlock.slip_L1[iSat];
157 frqObs1P->_slipCounter = -1; // because RTCM2 definition is vice versa to RTCM3
[6137]158 obs._obs.push_back(frqObs1P);
159
160 t_frqObs* frqObs2P = new t_frqObs;
[6703]161 frqObs2P->_rnxType2ch = (sys == 'G') ? "2W" : "2P";
[8025]162 frqObs2P->_codeValid = true;
[6703]163 frqObs2P->_code = _ObsBlock.rng_P2[iSat];
[8025]164 frqObs2P->_phaseValid = true;
[6703]165 frqObs2P->_phase = _ObsBlock.resolvedPhase_L2(iSat);
[7835]166 //frqObs2P->_slipCounter = _ObsBlock.slip_L2[iSat];
167 frqObs2P->_slipCounter = -1; // because RTCM2 definition is vice versa to RTCM3
[6137]168 obs._obs.push_back(frqObs2P);
169
[2711]170 _obsList.push_back(obs);
[242]171 }
172 _ObsBlock.clear();
173 }
174 }
[8025]175
[6703]176 else if (_PP.ID() == 20 || _PP.ID() == 21) {
[1044]177 _msg2021.extract(_PP);
178
179 if (_msg2021.valid()) {
[1045]180 decoded = true;
[6703]181 translateCorr2Obs(errmsg);
182 }
[1044]183 }
184
[6703]185 else if (_PP.ID() == 3) {
[1044]186 _msg03.extract(_PP);
187 }
188
[6703]189 else if (_PP.ID() == 22) {
[1044]190 _msg22.extract(_PP);
191 }
[1268]192
[6703]193 else if (_PP.ID() == 23) {
[1269]194 _msg23.extract(_PP);
195 }
[1268]196
[6703]197 else if (_PP.ID() == 24) {
[1269]198 _msg24.extract(_PP);
199 }
[1268]200
[1269]201 // Output for RTCM scan
[6703]202 if (_PP.ID() == 3) {
203 if (_msg03.validMsg) {
204 _antList.push_back(t_antInfo());
205
206 this->getStaCrd(_antList.back().xx, _antList.back().yy,
207 _antList.back().zz);
208
209 _antList.back().type = t_antInfo::APC;
210 _antList.back().message = _PP.ID();
[1269]211 }
[6703]212 } else if (_PP.ID() == 23) {
[7874]213 if (_msg23.validMsg && !_antType.contains(_msg23.antType.c_str())) {
[6703]214 _antType.push_back(_msg23.antType.c_str());
[1269]215 }
[6703]216 } else if (_PP.ID() == 24) {
217 if (_msg24.validMsg) {
218 _antList.push_back(t_antInfo());
[3594]219
[6703]220 _antList.back().xx = _msg24.x;
221 _antList.back().yy = _msg24.y;
222 _antList.back().zz = _msg24.z;
223
224 _antList.back().height_f = true;
225 _antList.back().height = _msg24.h;
226
227 _antList.back().type = t_antInfo::ARP;
228 _antList.back().message = _PP.ID();
[1269]229 }
230 }
[242]231 }
[649]232 return success;
[242]233}
234
[1218]235void RTCM2Decoder::translateCorr2Obs(vector<string>& errmsg) {
[1044]236
[3580]237 QMutexLocker locker(&_mutex);
238
[6703]239 if (!_msg03.validMsg || !_msg2021.valid()) {
[1044]240 return;
241 }
242
243 double stax = _msg03.x + (_msg22.validMsg ? _msg22.dL1[0] : 0.0);
244 double stay = _msg03.y + (_msg22.validMsg ? _msg22.dL1[1] : 0.0);
245 double staz = _msg03.z + (_msg22.validMsg ? _msg22.dL1[2] : 0.0);
246
[6703]247 int refWeek;
[1044]248 double refSecs;
249 currentGPSWeeks(refWeek, refSecs);
250
251 // Resolve receiver time of measurement (see RTCM 2.3, page 4-42, Message 18, Note 1)
252 // ----------------------------------------------------------------------------------
[6703]253 double hoursec_est = _msg2021.hoursec(); // estimated time of measurement
254 double hoursec_rcv = rint(hoursec_est * 1e2) / 1e2; // receiver clock reading at hoursec_est
[1044]255 double rcv_clk_bias = (hoursec_est - hoursec_rcv) * c_light;
256
[6703]257 int GPSWeek;
[1044]258 double GPSWeeks;
[6703]259 resolveEpoch(hoursec_est, refWeek, refSecs, GPSWeek, GPSWeeks);
[1044]260
[6703]261 int GPSWeek_rcv;
[1044]262 double GPSWeeks_rcv;
[6703]263 resolveEpoch(hoursec_rcv, refWeek, refSecs, GPSWeek_rcv, GPSWeeks_rcv);
[1044]264
265 // Loop over all satellites
266 // ------------------------
267 for (RTCM2_2021::data_iterator icorr = _msg2021.data.begin();
[6703]268 icorr != _msg2021.data.end(); icorr++) {
[1044]269 const RTCM2_2021::HiResCorr* corr = icorr->second;
270
[1218]271 // beg test
[6703]272 if (corr->PRN >= 200) {
[1218]273 continue;
274 }
275 // end test
276
[3562]277 QString prn;
[6703]278 char sys;
[3562]279 if (corr->PRN < 200) {
[6194]280 sys = 'G';
[7043]281 prn = sys + QString("%1_0").arg(corr->PRN, 2, 10, QChar('0'));
[6703]282 } else {
[6194]283 sys = 'R';
[7043]284 prn = sys + QString("%1_0").arg(corr->PRN - 200, 2, 10, QChar('0'));
[3562]285 }
[1218]286
[1044]287 double L1 = 0;
288 double L2 = 0;
289 double P1 = 0;
290 double P2 = 0;
291 string obsT = "";
292
293 // new observation
[7528]294 t_satObs new_obs;
[1044]295
[6137]296 t_frqObs* frqObs1C = new t_frqObs;
[6703]297 frqObs1C->_rnxType2ch = "1C";
[7528]298 new_obs._obs.push_back(frqObs1C);
[6137]299
300 t_frqObs* frqObs1P = new t_frqObs;
[6703]301 frqObs1P->_rnxType2ch = (sys == 'G') ? "1W" : "1P";
[7528]302 new_obs._obs.push_back(frqObs1P);
[6137]303
304 t_frqObs* frqObs2P = new t_frqObs;
[6703]305 frqObs2P->_rnxType2ch = (sys == 'G') ? "2W" : "2P";
[7528]306 new_obs._obs.push_back(frqObs2P);
[6137]307
[1218]308 // missing IOD
309 vector<string> missingIOD;
[6703]310 vector<string> hasIOD;
[1044]311 for (unsigned ii = 0; ii < 4; ii++) {
[7169]312 unsigned int IODcorr = 0;
[6703]313 double corrVal = 0;
314 const t_eph* eph = 0;
315 double* obsVal = 0;
[1044]316
317 switch (ii) {
[6703]318 case 0: // --- L1 ---
319 IODcorr = corr->IODp1;
320 corrVal = corr->phase1 * LAMBDA_1;
321 obsVal = &L1;
322 obsT = "L1";
323 break;
324 case 1: // --- L2 ---
325 IODcorr = corr->IODp2;
326 corrVal = corr->phase2 * LAMBDA_2;
327 obsVal = &L2;
328 obsT = "L2";
329 break;
330 case 2: // --- P1 ---
331 IODcorr = corr->IODr1;
332 corrVal = corr->range1;
333 obsVal = &P1;
334 obsT = "P1";
335 break;
336 case 3: // --- P2 ---
337 IODcorr = corr->IODr2;
338 corrVal = corr->range2;
339 obsVal = &P2;
340 obsT = "P2";
341 break;
342 default:
343 continue;
[1044]344 }
345
[1299]346 // Select corresponding ephemerides
[6443]347 const t_eph* ephLast = _ephUser.ephLast(prn);
348 const t_eph* ephPrev = _ephUser.ephPrev(prn);
[6703]349 if (ephLast && ephLast->IOD() == IODcorr) {
[6443]350 eph = ephLast;
[6703]351 } else if (ephPrev && ephPrev->IOD() == IODcorr) {
[6443]352 eph = ephPrev;
[6442]353 }
[1218]354
[6703]355 if (eph) {
[1218]356 ostringstream msg;
357 msg << obsT << ':' << setw(3) << eph->IOD();
358 hasIOD.push_back(msg.str());
359
[6703]360 int GPSWeek_tot;
361 double GPSWeeks_tot;
362 double rho, xSat, ySat, zSat, clkSat;
363 cmpRho(eph, stax, stay, staz, GPSWeek, GPSWeeks, rho, GPSWeek_tot,
364 GPSWeeks_tot, xSat, ySat, zSat, clkSat);
[1218]365
[6703]366 *obsVal = rho - corrVal + rcv_clk_bias - clkSat;
[1044]367
[6703]368 if (*obsVal == 0)
369 *obsVal = ZEROVALUE;
[1044]370
[7528]371 if (corr->PRN < 200) {
372 new_obs._prn.set('G', corr->PRN);
373 } else {
374 new_obs._prn.set('R', corr->PRN - 200);
[6703]375 }
[7528]376 new_obs._time.set(GPSWeek_rcv, GPSWeeks_rcv);
[6703]377
378 // Store estimated measurements
379 // ----------------------------
380 switch (ii) {
381 case 0: // --- L1 ---
382 frqObs1P->_phaseValid = true;
383 frqObs1P->_phase = *obsVal / LAMBDA_1;
[7835]384 //frqObs1P->_slipCounter = corr->lock1;
385 frqObs1P->_slipCounter = -1; // because RTCM2 definition is vice versa to RTCM3
[6703]386 break;
387 case 1: // --- L2 ---
388 frqObs2P->_phaseValid = true;
389 frqObs2P->_phase = *obsVal / LAMBDA_2;
[7835]390 //frqObs2P->_slipCounter = corr->lock2;
391 frqObs2P->_slipCounter = -1; // because RTCM2 definition is vice versa to RTCM3
[6703]392 break;
393 case 2: // --- C1 / P1 ---
394 if (corr->Pind1) {
395 frqObs1P->_codeValid = true;
396 frqObs1P->_code = *obsVal;
397 } else {
398 frqObs1C->_codeValid = true;
399 frqObs1C->_code = *obsVal;
400 }
401 break;
402 case 3: // --- C2 / P2 ---
403 if (corr->Pind2) {
404 frqObs2P->_codeValid = true;
405 frqObs2P->_code = *obsVal;
406 }
407 break;
408 default:
409 continue;
410 }
411 } else if (IODcorr != 0) {
[1218]412 ostringstream msg;
413 msg << obsT << ':' << setw(3) << IODcorr;
414 missingIOD.push_back(msg.str());
415 }
[1044]416 } // loop over frequencies
[6703]417
[1218]418 // Error report
[6703]419 if (missingIOD.size()) {
[1218]420 ostringstream missingIODstr;
421
[6703]422 copy(missingIOD.begin(), missingIOD.end(),
423 ostream_iterator<string>(missingIODstr, " "));
[1218]424
[6703]425 errmsg.push_back(
426 "missing eph for " + string(prn.toAscii().data()) + " , IODs "
427 + missingIODstr.str());
[1218]428 }
429
430 // Store new observation
[7528]431 if (new_obs._time.mjd() > 0) {
432 _obsList.push_back(new_obs);
[1044]433 }
434 }
435}
Note: See TracBrowser for help on using the repository browser.