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

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