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

Last change on this file since 1049 was 1045, checked in by weber, 16 years ago

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