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

Last change on this file since 8023 was 8023, checked in by stuerze, 6 years ago

bug is fixed in RTCM2Decoder: a real check of observations validity is added

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