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

Last change on this file since 7141 was 7055, checked in by stuerze, 10 years ago

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