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

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

Zdenek Lukes:
a) changed logic how the ephemerides are stored for decoding of message 20/21 RTCM 2.3
b) added some debugging output (enabled is macro DEBUG_RTCM2_2021 is defined)
File size: 10.3 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 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 p_obs obs = new t_obs();
140 _obsList.push_back(obs);
141 if (_ObsBlock.PRN[iSat] > 100) {
142 obs->_o.satNum = _ObsBlock.PRN[iSat] % 100;
143 obs->_o.satSys = 'R';
144 }
145 else {
146 obs->_o.satNum = _ObsBlock.PRN[iSat];
147 obs->_o.satSys = 'G';
148 }
149 obs->_o.GPSWeek = epochWeek;
150 obs->_o.GPSWeeks = epochSecs;
151 obs->_o.C1 = _ObsBlock.rng_C1[iSat];
152 obs->_o.P1 = _ObsBlock.rng_P1[iSat];
153 obs->_o.P2 = _ObsBlock.rng_P2[iSat];
154 obs->_o.L1 = _ObsBlock.resolvedPhase_L1(iSat);
155 obs->_o.L2 = _ObsBlock.resolvedPhase_L2(iSat);
156 obs->_o.slip_cnt_L1 = _ObsBlock.slip_L1[iSat];
157 obs->_o.slip_cnt_L2 = _ObsBlock.slip_L2[iSat];
158 obs->_o.lock_timei_L1 = -1;
159 obs->_o.lock_timei_L2 = -1;
160 }
161 _ObsBlock.clear();
162 }
163 }
164
165 else if ( _PP.ID() == 20 || _PP.ID() == 21 ) {
166 _msg2021.extract(_PP);
167
168 if (_msg2021.valid()) {
169 decoded = true;
170 translateCorr2Obs(errmsg);
171 }
172 }
173
174 else if ( _PP.ID() == 3 ) {
175 _msg03.extract(_PP);
176 }
177
178 else if ( _PP.ID() == 22 ) {
179 _msg22.extract(_PP);
180 }
181 }
182 return success;
183}
184
185
186
187bool RTCM2Decoder::storeEph(const gpsephemeris& gpseph, string& storedPRN, vector<int>& IODs) {
188 t_ephGPS eph; eph.set(&gpseph);
189
190 return storeEph(eph, storedPRN, IODs);
191}
192
193
194bool RTCM2Decoder::storeEph(const t_ephGPS& gpseph, string& storedPRN, vector<int>& IODs) {
195 t_ephGPS* eph = new t_ephGPS(gpseph);
196
197 string prn = eph->prn();
198
199 t_listMap::iterator ip = _ephList.find(prn);
200 if (ip == _ephList.end() ) {
201 ip = _ephList.insert(pair<string, t_ephList*>(prn, new t_ephList)).first;
202 }
203 t_ephList* ephList = ip->second;
204
205 bool stored = ephList->store(eph);
206
207 if ( stored ) {
208 storedPRN = eph->prn();
209 ephList->getIODs(IODs);
210 return true;
211 }
212
213 delete eph;
214
215 return false;
216}
217
218
219void RTCM2Decoder::translateCorr2Obs(vector<string>& errmsg) {
220
221 if ( !_msg03.validMsg || !_msg2021.valid() ) {
222 return;
223 }
224
225 double stax = _msg03.x + (_msg22.validMsg ? _msg22.dL1[0] : 0.0);
226 double stay = _msg03.y + (_msg22.validMsg ? _msg22.dL1[1] : 0.0);
227 double staz = _msg03.z + (_msg22.validMsg ? _msg22.dL1[2] : 0.0);
228
229 int refWeek;
230 double refSecs;
231 currentGPSWeeks(refWeek, refSecs);
232
233 // Resolve receiver time of measurement (see RTCM 2.3, page 4-42, Message 18, Note 1)
234 // ----------------------------------------------------------------------------------
235 double hoursec_est = _msg2021.hoursec(); // estimated time of measurement
236 double hoursec_rcv = rint(hoursec_est * 1e2) / 1e2; // receiver clock reading at hoursec_est
237 double rcv_clk_bias = (hoursec_est - hoursec_rcv) * c_light;
238
239 int GPSWeek;
240 double GPSWeeks;
241 resolveEpoch(hoursec_est, refWeek, refSecs,
242 GPSWeek, GPSWeeks);
243
244 int GPSWeek_rcv;
245 double GPSWeeks_rcv;
246 resolveEpoch(hoursec_rcv, refWeek, refSecs,
247 GPSWeek_rcv, GPSWeeks_rcv);
248
249 // Loop over all satellites
250 // ------------------------
251 for (RTCM2_2021::data_iterator icorr = _msg2021.data.begin();
252 icorr != _msg2021.data.end(); icorr++) {
253 const RTCM2_2021::HiResCorr* corr = icorr->second;
254
255 // beg test
256 if ( corr->PRN >= 200 ) {
257 continue;
258 }
259 // end test
260
261
262 ostringstream oPRN; oPRN.fill('0');
263
264 oPRN << (corr->PRN < 200 ? 'G' : 'R')
265 << setw(2) << (corr->PRN < 200 ? corr->PRN : corr->PRN - 200);
266
267 string PRN(oPRN.str());
268
269 t_listMap::const_iterator ieph = _ephList.find(PRN);
270
271 if ( ieph == _ephList.end() ) {
272 errmsg.push_back("missing eph for " + PRN);
273 continue;
274 }
275
276 double L1 = 0;
277 double L2 = 0;
278 double P1 = 0;
279 double P2 = 0;
280 string obsT = "";
281
282 // new observation
283 p_obs new_obs = 0;
284
285 // missing IOD
286 vector<string> missingIOD;
287 vector<string> hasIOD;
288 for (unsigned ii = 0; ii < 4; ii++) {
289 int IODcorr = 0;
290 double corrVal = 0;
291 const t_eph* eph = 0;
292 double* obsVal = 0;
293
294 switch (ii) {
295 case 0: // --- L1 ---
296 IODcorr = corr->IODp1;
297 corrVal = corr->phase1 * LAMBDA_1;
298 obsVal = &L1;
299 obsT = "L1";
300 break;
301 case 1: // --- L2 ---
302 IODcorr = corr->IODp2;
303 corrVal = corr->phase2 * LAMBDA_2;
304 obsVal = &L2;
305 obsT = "L2";
306 break;
307 case 2: // --- P1 ---
308 IODcorr = corr->IODr1;
309 corrVal = corr->range1;
310 obsVal = &P1;
311 obsT = "P1";
312 break;
313 case 3: // --- P2 ---
314 IODcorr = corr->IODr2;
315 corrVal = corr->range2;
316 obsVal = &P2;
317 obsT = "P2";
318 break;
319 default:
320 continue;
321 }
322
323 eph = ieph->second->getEph(IODcorr);
324
325 if ( eph ) {
326 ostringstream msg;
327 msg << obsT << ':' << setw(3) << eph->IOD();
328 hasIOD.push_back(msg.str());
329
330
331 int GPSWeek_tot;
332 double GPSWeeks_tot;
333 double rho, xSat, ySat, zSat, clkSat;
334 cmpRho(eph, stax, stay, staz,
335 GPSWeek, GPSWeeks,
336 rho, GPSWeek_tot, GPSWeeks_tot,
337 xSat, ySat, zSat, clkSat);
338
339 *obsVal = rho - corrVal + rcv_clk_bias - clkSat;
340
341 if ( *obsVal == 0 ) *obsVal = ZEROVALUE;
342
343 // Allocate new memory
344 // -------------------
345 if ( !new_obs ) {
346 new_obs = new t_obs();
347
348 new_obs->_o.StatID[0] = '\x0';
349 new_obs->_o.satSys = (corr->PRN < 200 ? 'G' : 'R');
350 new_obs->_o.satNum = (corr->PRN < 200 ? corr->PRN : corr->PRN - 200);
351
352 new_obs->_o.GPSWeek = GPSWeek_rcv;
353 new_obs->_o.GPSWeeks = GPSWeeks_rcv;
354 }
355
356 // Store estimated measurements
357 // ----------------------------
358 switch (ii) {
359 case 0: // --- L1 ---
360 new_obs->_o.L1 = *obsVal / LAMBDA_1;
361 new_obs->_o.slip_cnt_L1 = corr->lock1;
362 new_obs->_o.lock_timei_L1 = -1;
363 break;
364 case 1: // --- L2 ---
365 new_obs->_o.L2 = *obsVal / LAMBDA_2;
366 new_obs->_o.slip_cnt_L2 = corr->lock2;
367 new_obs->_o.lock_timei_L2 = -1;
368 break;
369 case 2: // --- C1 / P1 ---
370 if ( corr->Pind1 )
371 new_obs->_o.P1 = *obsVal;
372 else
373 new_obs->_o.C1 = *obsVal;
374 break;
375 case 3: // --- C2 / P2 ---
376 if ( corr->Pind2 )
377 new_obs->_o.P2 = *obsVal;
378 else
379 new_obs->_o.C2 = *obsVal;
380 break;
381 default:
382 continue;
383 }
384 }
385 else if ( IODcorr != 0 ) {
386 ostringstream msg;
387 msg << obsT << ':' << setw(3) << IODcorr;
388 missingIOD.push_back(msg.str());
389 }
390 } // loop over frequencies
391
392 // Error report
393 if ( missingIOD.size() ) {
394 ostringstream missingIODstr;
395
396 copy(missingIOD.begin(), missingIOD.end(), ostream_iterator<string>(missingIODstr, " "));
397
398 errmsg.push_back("missing eph for " + PRN + " , IODs " + missingIODstr.str());
399 }
400
401 // Store new observation
402 if ( new_obs ) {
403 _obsList.push_back( new_obs );
404
405 ///ostringstream hasIODstr;
406 ///copy(hasIOD.begin(), hasIOD.end(), ostream_iterator<string>(hasIODstr, " "));
407 ///errmsg.push_back("decoded PRN " + PRN + " : " + hasIODstr.str());
408 }
409 }
410}
Note: See TracBrowser for help on using the repository browser.