source: ntrip/branches/BNC_LM/RTCM/RTCM2Decoder.cpp@ 6666

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