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

Last change on this file since 9088 was 9088, checked in by stuerze, 22 months ago

adjusted allocation of slip and LTI according to the respective RTCM version

File size: 12.3 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->_codeValid = true;
147 frqObs1C->_code = _ObsBlock.rng_C1[iSat];
148 obs._obs.push_back(frqObs1C);
149
150 t_frqObs* frqObs1P = new t_frqObs;
151 frqObs1P->_rnxType2ch = (sys == 'G') ? "1W" : "1P";
152 frqObs1P->_codeValid = true;
153 frqObs1P->_code = _ObsBlock.rng_P1[iSat];
154 frqObs1P->_phaseValid = true;
155 frqObs1P->_phase = _ObsBlock.resolvedPhase_L1(iSat);
156 frqObs1P->_slipCounter = _ObsBlock.slip_L1[iSat];
157 obs._obs.push_back(frqObs1P);
158
159 t_frqObs* frqObs2P = new t_frqObs;
160 frqObs2P->_rnxType2ch = (sys == 'G') ? "2W" : "2P";
161 frqObs2P->_codeValid = true;
162 frqObs2P->_code = _ObsBlock.rng_P2[iSat];
163 frqObs2P->_phaseValid = true;
164 frqObs2P->_phase = _ObsBlock.resolvedPhase_L2(iSat);
165 frqObs2P->_slipCounter = _ObsBlock.slip_L2[iSat];
166 obs._obs.push_back(frqObs2P);
167
168 _obsList.push_back(obs);
169 }
170 _ObsBlock.clear();
171 }
172 }
173
174 else if (_PP.ID() == 20 || _PP.ID() == 21) {
175 _msg2021.extract(_PP);
176
177 if (_msg2021.valid()) {
178 decoded = true;
179 translateCorr2Obs(errmsg);
180 }
181 }
182
183 else if (_PP.ID() == 3) {
184 _msg03.extract(_PP);
185 }
186
187 else if (_PP.ID() == 22) {
188 _msg22.extract(_PP);
189 }
190
191 else if (_PP.ID() == 23) {
192 _msg23.extract(_PP);
193 }
194
195 else if (_PP.ID() == 24) {
196 _msg24.extract(_PP);
197 }
198
199 // Output for RTCM scan
200 if (_PP.ID() == 3) {
201 if (_msg03.validMsg) {
202 _antList.push_back(t_antRefPoint());
203
204 this->getStaCrd(_antList.back().xx, _antList.back().yy, _antList.back().zz);
205
206 _antList.back().type = t_antRefPoint::APC;
207 _antList.back().message = _PP.ID();
208 }
209 } else if (_PP.ID() == 23) {
210 if (_msg23.validMsg) {
211 int antlen = strlen(_msg23.antType.c_str());
212 int serlen = strlen(_msg23.antSN.c_str());
213 if ((antlen) &&
214 (_antType.empty() || strncmp(_antType.back().descriptor, _msg23.antType.c_str(), antlen) != 0)) {
215 _antType.push_back(t_antInfo());
216 memcpy(_antType.back().descriptor, _msg23.antType.c_str(), antlen);
217 _antType.back().descriptor[antlen] = 0;
218 if (serlen) {
219 memcpy(_antType.back().serialnumber, _msg23.antSN.c_str(), serlen);
220 _antType.back().serialnumber[serlen] = 0;
221 }
222 }
223 }
224 } else if (_PP.ID() == 24) {
225 if (_msg24.validMsg) {
226 _antList.push_back(t_antRefPoint());
227
228 _antList.back().xx = _msg24.x;
229 _antList.back().yy = _msg24.y;
230 _antList.back().zz = _msg24.z;
231
232 _antList.back().height_f = true;
233 _antList.back().height = _msg24.h;
234
235 _antList.back().type = t_antRefPoint::ARP;
236 _antList.back().message = _PP.ID();
237 }
238 }
239 }
240 return success;
241}
242
243void RTCM2Decoder::translateCorr2Obs(vector<string>& errmsg) {
244
245 QMutexLocker locker(&_mutex);
246
247 if (!_msg03.validMsg || !_msg2021.valid()) {
248 return;
249 }
250
251 double stax = _msg03.x + (_msg22.validMsg ? _msg22.dL1[0] : 0.0);
252 double stay = _msg03.y + (_msg22.validMsg ? _msg22.dL1[1] : 0.0);
253 double staz = _msg03.z + (_msg22.validMsg ? _msg22.dL1[2] : 0.0);
254
255 int refWeek;
256 double refSecs;
257 currentGPSWeeks(refWeek, refSecs);
258
259 // Resolve receiver time of measurement (see RTCM 2.3, page 4-42, Message 18, Note 1)
260 // ----------------------------------------------------------------------------------
261 double hoursec_est = _msg2021.hoursec(); // estimated time of measurement
262 double hoursec_rcv = rint(hoursec_est * 1e2) / 1e2; // receiver clock reading at hoursec_est
263 double rcv_clk_bias = (hoursec_est - hoursec_rcv) * c_light;
264
265 int GPSWeek;
266 double GPSWeeks;
267 resolveEpoch(hoursec_est, refWeek, refSecs, GPSWeek, GPSWeeks);
268
269 int GPSWeek_rcv;
270 double GPSWeeks_rcv;
271 resolveEpoch(hoursec_rcv, refWeek, refSecs, GPSWeek_rcv, GPSWeeks_rcv);
272
273 // Loop over all satellites
274 // ------------------------
275 for (RTCM2_2021::data_iterator icorr = _msg2021.data.begin();
276 icorr != _msg2021.data.end(); icorr++) {
277 const RTCM2_2021::HiResCorr* corr = icorr->second;
278
279 // beg test
280 if (corr->PRN >= 200) {
281 continue;
282 }
283 // end test
284
285 QString prn;
286 char sys;
287 if (corr->PRN < 200) {
288 sys = 'G';
289 prn = sys + QString("%1_0").arg(corr->PRN, 2, 10, QChar('0'));
290 } else {
291 sys = 'R';
292 prn = sys + QString("%1_0").arg(corr->PRN - 200, 2, 10, QChar('0'));
293 }
294
295 double L1 = 0;
296 double L2 = 0;
297 double P1 = 0;
298 double P2 = 0;
299 string obsT = "";
300
301 // new observation
302 t_satObs new_obs;
303
304 t_frqObs* frqObs1C = new t_frqObs;
305 frqObs1C->_rnxType2ch = "1C";
306 new_obs._obs.push_back(frqObs1C);
307
308 t_frqObs* frqObs1P = new t_frqObs;
309 frqObs1P->_rnxType2ch = (sys == 'G') ? "1W" : "1P";
310 new_obs._obs.push_back(frqObs1P);
311
312 t_frqObs* frqObs2P = new t_frqObs;
313 frqObs2P->_rnxType2ch = (sys == 'G') ? "2W" : "2P";
314 new_obs._obs.push_back(frqObs2P);
315
316 // missing IOD
317 vector<string> missingIOD;
318 vector<string> hasIOD;
319 for (unsigned ii = 0; ii < 4; ii++) {
320 unsigned int IODcorr = 0;
321 double corrVal = 0;
322 const t_eph* eph = 0;
323 double* obsVal = 0;
324
325 switch (ii) {
326 case 0: // --- L1 ---
327 IODcorr = corr->IODp1;
328 corrVal = corr->phase1 * LAMBDA_1;
329 obsVal = &L1;
330 obsT = "L1";
331 break;
332 case 1: // --- L2 ---
333 IODcorr = corr->IODp2;
334 corrVal = corr->phase2 * LAMBDA_2;
335 obsVal = &L2;
336 obsT = "L2";
337 break;
338 case 2: // --- P1 ---
339 IODcorr = corr->IODr1;
340 corrVal = corr->range1;
341 obsVal = &P1;
342 obsT = "P1";
343 break;
344 case 3: // --- P2 ---
345 IODcorr = corr->IODr2;
346 corrVal = corr->range2;
347 obsVal = &P2;
348 obsT = "P2";
349 break;
350 default:
351 continue;
352 }
353
354 // Select corresponding ephemerides
355 const t_eph* ephLast = _ephUser.ephLast(prn);
356 const t_eph* ephPrev = _ephUser.ephPrev(prn);
357 if (ephLast && ephLast->IOD() == IODcorr) {
358 eph = ephLast;
359 } else if (ephPrev && ephPrev->IOD() == IODcorr) {
360 eph = ephPrev;
361 }
362
363 if (eph) {
364 ostringstream msg;
365 msg << obsT << ':' << setw(3) << eph->IOD();
366 hasIOD.push_back(msg.str());
367
368 int GPSWeek_tot;
369 double GPSWeeks_tot;
370 double rho, xSat, ySat, zSat, clkSat;
371 cmpRho(eph, stax, stay, staz, GPSWeek, GPSWeeks, rho, GPSWeek_tot,
372 GPSWeeks_tot, xSat, ySat, zSat, clkSat);
373
374 *obsVal = rho - corrVal + rcv_clk_bias - clkSat;
375
376 if (*obsVal == 0)
377 *obsVal = ZEROVALUE;
378
379 if (corr->PRN < 200) {
380 new_obs._prn.set('G', corr->PRN);
381 } else {
382 new_obs._prn.set('R', corr->PRN - 200);
383 }
384 new_obs._time.set(GPSWeek_rcv, GPSWeeks_rcv);
385
386 // Store estimated measurements
387 // ----------------------------
388 switch (ii) {
389 case 0: // --- L1 ---
390 frqObs1P->_phaseValid = true;
391 frqObs1P->_phase = *obsVal / LAMBDA_1;
392 //frqObs1P->_slipCounter = corr->lock1;
393 frqObs1P->_slipCounter = -1; // because RTCM2 definition is vice versa to RTCM3
394 break;
395 case 1: // --- L2 ---
396 frqObs2P->_phaseValid = true;
397 frqObs2P->_phase = *obsVal / LAMBDA_2;
398 //frqObs2P->_slipCounter = corr->lock2;
399 frqObs2P->_slipCounter = -1; // because RTCM2 definition is vice versa to RTCM3
400 break;
401 case 2: // --- C1 / P1 ---
402 if (corr->Pind1) {
403 frqObs1P->_codeValid = true;
404 frqObs1P->_code = *obsVal;
405 } else {
406 frqObs1C->_codeValid = true;
407 frqObs1C->_code = *obsVal;
408 }
409 break;
410 case 3: // --- C2 / P2 ---
411 if (corr->Pind2) {
412 frqObs2P->_codeValid = true;
413 frqObs2P->_code = *obsVal;
414 }
415 break;
416 default:
417 continue;
418 }
419 } else if (IODcorr != 0) {
420 ostringstream msg;
421 msg << obsT << ':' << setw(3) << IODcorr;
422 missingIOD.push_back(msg.str());
423 }
424 } // loop over frequencies
425
426 // Error report
427 if (missingIOD.size()) {
428 ostringstream missingIODstr;
429
430 copy(missingIOD.begin(), missingIOD.end(),
431 ostream_iterator<string>(missingIODstr, " "));
432
433 errmsg.push_back(
434 "missing eph for " + string(prn.toLatin1().data()) + " , IODs "
435 + missingIODstr.str());
436 }
437
438 // Store new observation
439 if (new_obs._time.mjd() > 0) {
440 _obsList.push_back(new_obs);
441 }
442 }
443}
Note: See TracBrowser for help on using the repository browser.