source: ntrip/trunk/BNC/RTCM/RTCM2Decoder.cpp@ 1167

Last change on this file since 1167 was 1167, checked in by mervart, 16 years ago

* empty log message *
File size: 9.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
45#include "../bncutils.h"
46#include "rtcm_utils.h"
47#include "GPSDecoder.h"
48#include "RTCM2Decoder.h"
49
50using namespace std;
51using namespace rtcm2;
52
53//
54// Constructor
55//
56
57RTCM2Decoder::RTCM2Decoder(const std::string& ID) {
58 _ID = ID;
59}
60
61//
62// Destructor
63//
64
65RTCM2Decoder::~RTCM2Decoder() {
66 for (t_pairMap::iterator ii = _ephPair.begin(); ii != _ephPair.end(); ii++) {
67 delete ii->second;
68 }
69}
70
71
72//
73t_irc RTCM2Decoder::getStaCrd(double& xx, double& yy, double& zz) {
74 if ( !_msg03.validMsg ) {
75 return failure;
76 }
77
78 xx = _msg03.x + (_msg22.validMsg ? _msg22.dL1[0] : 0.0);
79 yy = _msg03.y + (_msg22.validMsg ? _msg22.dL1[1] : 0.0);
80 zz = _msg03.z + (_msg22.validMsg ? _msg22.dL1[2] : 0.0);
81
82 return success;
83}
84
85//
86t_irc RTCM2Decoder::getStaCrd(double& xx, double& yy, double& zz,
87 double& dx1, double& dy1, double& dz1,
88 double& dx2, double& dy2, double& dz2) {
89 xx = _msg03.x;
90 yy = _msg03.y;
91 zz = _msg03.z;
92
93 dx1 = (_msg22.validMsg ? _msg22.dL1[0] : 0.0);
94 dy1 = (_msg22.validMsg ? _msg22.dL1[1] : 0.0);
95 dz1 = (_msg22.validMsg ? _msg22.dL1[2] : 0.0);
96
97 dx2 = (_msg22.validMsg ? _msg22.dL2[0] : 0.0);
98 dy2 = (_msg22.validMsg ? _msg22.dL2[1] : 0.0);
99 dz2 = (_msg22.validMsg ? _msg22.dL2[2] : 0.0);
100
101 return success;
102}
103
104
105//
106t_irc RTCM2Decoder::Decode(char* buffer, int bufLen) {
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 if ( _PP.ID()==18 || _PP.ID()==19 ) {
125
126 _ObsBlock.extract(_PP);
127
128 if (_ObsBlock.valid()) {
129 decoded = true;
130
131 int epochWeek;
132 double epochSecs;
133 _ObsBlock.resolveEpoch(refWeek, refSecs, epochWeek, epochSecs);
134
135 for (int iSat=0; iSat < _ObsBlock.nSat; iSat++) {
136 p_obs obs = new t_obs();
137 _obsList.push_back(obs);
138 if (_ObsBlock.PRN[iSat] > 100) {
139 obs->_o.satNum = _ObsBlock.PRN[iSat] % 100;
140 obs->_o.satSys = 'R';
141 }
142 else {
143 obs->_o.satNum = _ObsBlock.PRN[iSat];
144 obs->_o.satSys = 'G';
145 }
146 obs->_o.GPSWeek = epochWeek;
147 obs->_o.GPSWeeks = epochSecs;
148 obs->_o.C1 = _ObsBlock.rng_C1[iSat];
149 obs->_o.P1 = _ObsBlock.rng_P1[iSat];
150 obs->_o.P2 = _ObsBlock.rng_P2[iSat];
151 obs->_o.L1 = _ObsBlock.resolvedPhase_L1(iSat);
152 obs->_o.L2 = _ObsBlock.resolvedPhase_L2(iSat);
153 obs->_o.slip_cnt_L1 = _ObsBlock.slip_L1[iSat];
154 obs->_o.slip_cnt_L2 = _ObsBlock.slip_L2[iSat];
155 obs->_o.lock_timei_L1 = -1;
156 obs->_o.lock_timei_L2 = -1;
157 }
158 _ObsBlock.clear();
159 }
160 }
161
162 else if ( _PP.ID() == 20 || _PP.ID() == 21 ) {
163 _msg2021.extract(_PP);
164
165 if (_msg2021.valid()) {
166 decoded = true;
167 translateCorr2Obs();
168 }
169 }
170
171 else if ( _PP.ID() == 3 ) {
172 _msg03.extract(_PP);
173 }
174
175 else if ( _PP.ID() == 22 ) {
176 _msg22.extract(_PP);
177 }
178 }
179 return success;
180}
181
182
183
184void RTCM2Decoder::storeEph(const gpsephemeris& gpseph) {
185 t_ephGPS eph; eph.set(&gpseph);
186
187 storeEph(eph);
188}
189
190
191void RTCM2Decoder::storeEph(const t_ephGPS& gpseph) {
192 t_ephGPS* eph = new t_ephGPS(gpseph);
193
194 string prn = eph->prn();
195
196 t_pairMap::iterator ip = _ephPair.find(prn);
197 if (ip == _ephPair.end() ) {
198 ip = _ephPair.insert(pair<string, t_ephPair*>(prn, new t_ephPair)).first;
199 }
200 t_ephPair* pair = ip->second;
201
202 if ( !pair->eph || eph->isNewerThan(pair->eph) ) {
203 delete pair->oldEph;
204 pair->oldEph = pair->eph;
205 pair->eph = eph;
206
207 return;
208 }
209
210 delete eph;
211}
212
213
214void RTCM2Decoder::translateCorr2Obs() {
215
216 if ( !_msg03.validMsg || !_msg2021.valid() ) {
217 return;
218 }
219
220 double stax = _msg03.x + (_msg22.validMsg ? _msg22.dL1[0] : 0.0);
221 double stay = _msg03.y + (_msg22.validMsg ? _msg22.dL1[1] : 0.0);
222 double staz = _msg03.z + (_msg22.validMsg ? _msg22.dL1[2] : 0.0);
223
224 int refWeek;
225 double refSecs;
226 currentGPSWeeks(refWeek, refSecs);
227
228 // Resolve receiver time of measurement (see RTCM 2.3, page 4-42, Message 18, Note 1)
229 // ----------------------------------------------------------------------------------
230 double hoursec_est = _msg2021.hoursec(); // estimated time of measurement
231 double hoursec_rcv = rint(hoursec_est * 1e2) / 1e2; // receiver clock reading at hoursec_est
232 double rcv_clk_bias = (hoursec_est - hoursec_rcv) * c_light;
233
234 int GPSWeek;
235 double GPSWeeks;
236 resolveEpoch(hoursec_est, refWeek, refSecs,
237 GPSWeek, GPSWeeks);
238
239 int GPSWeek_rcv;
240 double GPSWeeks_rcv;
241 resolveEpoch(hoursec_rcv, refWeek, refSecs,
242 GPSWeek_rcv, GPSWeeks_rcv);
243
244 // Loop over all satellites
245 // ------------------------
246 for (RTCM2_2021::data_iterator icorr = _msg2021.data.begin();
247 icorr != _msg2021.data.end(); icorr++) {
248 const RTCM2_2021::HiResCorr* corr = icorr->second;
249
250 ostringstream oPRN; oPRN.fill('0');
251
252 oPRN << (corr->PRN < 200 ? 'G' : 'R')
253 << setw(2) << (corr->PRN < 200 ? corr->PRN : corr->PRN - 200);
254
255 string PRN(oPRN.str());
256
257 t_pairMap::const_iterator ieph = _ephPair.find(PRN);
258 const t_eph* eph0 = 0;
259 const t_eph* eph1 = 0;
260
261 if ( ieph != _ephPair.end() ) {
262 eph0 = ieph->second->eph;
263 eph1 = ieph->second->oldEph;
264 }
265
266 if ( !eph0 && !eph1 ) {
267 continue;
268 }
269
270 double L1 = 0;
271 double L2 = 0;
272 double P1 = 0;
273 double P2 = 0;
274 string obsT = "";
275
276 // new observation
277 p_obs new_obs = 0;
278
279 for (unsigned ii = 0; ii < 4; ii++) {
280 int IODcorr = 0;
281 double corrVal = 0;
282 const t_eph* eph = 0;
283 double* obsVal = 0;
284
285 switch (ii) {
286 case 0: // --- L1 ---
287 IODcorr = corr->IODp1;
288 corrVal = corr->phase1 * LAMBDA_1;
289 obsVal = &L1;
290 obsT = "L1";
291 break;
292 case 1: // --- L2 ---
293 IODcorr = corr->IODp2;
294 corrVal = corr->phase2 * LAMBDA_2;
295 obsVal = &L2;
296 obsT = "L2";
297 break;
298 case 2: // --- P1 ---
299 IODcorr = corr->IODr1;
300 corrVal = corr->range1;
301 obsVal = &P1;
302 obsT = "P1";
303 break;
304 case 3: // --- P2 ---
305 IODcorr = corr->IODr2;
306 corrVal = corr->range2;
307 obsVal = &P2;
308 obsT = "P2";
309 break;
310 default:
311 continue;
312 }
313
314 eph = 0;
315 if ( eph0 && eph0->IOD() == IODcorr )
316 eph = eph0;
317 else if ( eph1 && eph1->IOD() == IODcorr )
318 eph = eph1;
319 if ( eph && corr ) {
320 int GPSWeek_tot;
321 double GPSWeeks_tot;
322 double rho, xSat, ySat, zSat, clkSat;
323 cmpRho(eph, stax, stay, staz,
324 GPSWeek, GPSWeeks,
325 rho, GPSWeek_tot, GPSWeeks_tot,
326 xSat, ySat, zSat, clkSat);
327
328 *obsVal = rho - corrVal + rcv_clk_bias - clkSat;
329
330 if ( *obsVal == 0 ) *obsVal = ZEROVALUE;
331
332 // Allocate new memory
333 // -------------------
334 if ( !new_obs ) {
335 new_obs = new t_obs();
336
337 new_obs->_o.StatID[0] = '\x0';
338 new_obs->_o.satSys = (corr->PRN < 200 ? 'G' : 'R');
339 new_obs->_o.satNum = (corr->PRN < 200 ? corr->PRN : corr->PRN - 200);
340
341 new_obs->_o.GPSWeek = GPSWeek_rcv;
342 new_obs->_o.GPSWeeks = GPSWeeks_rcv;
343 }
344
345 // Store estimated measurements
346 // ----------------------------
347 switch (ii) {
348 case 0: // --- L1 ---
349 new_obs->_o.L1 = *obsVal / LAMBDA_1;
350 new_obs->_o.slip_cnt_L1 = corr->lock1;
351 new_obs->_o.lock_timei_L1 = -1;
352 break;
353 case 1: // --- L2 ---
354 new_obs->_o.L2 = *obsVal / LAMBDA_2;
355 new_obs->_o.slip_cnt_L2 = corr->lock2;
356 new_obs->_o.lock_timei_L2 = -1;
357 break;
358 case 2: // --- C1 / P1 ---
359 if ( corr->Pind1 )
360 new_obs->_o.P1 = *obsVal;
361 else
362 new_obs->_o.C1 = *obsVal;
363 break;
364 case 3: // --- C2 / P2 ---
365 if ( corr->Pind2 )
366 new_obs->_o.P2 = *obsVal;
367 else
368 new_obs->_o.C2 = *obsVal;
369 break;
370 default:
371 continue;
372 }
373 }
374 } // loop over frequencies
375
376 if ( new_obs ) {
377 _obsList.push_back( new_obs );
378 }
379 }
380}
Note: See TracBrowser for help on using the repository browser.