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

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