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

Last change on this file since 8234 was 8234, checked in by stuerze, 18 months ago

antenna and receiver info from message type 1007, 1008 and 1033 completed

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