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

Last change on this file since 7528 was 7528, checked in by stuerze, 4 years ago

bug fixed in RTCM2 decoder

File size: 11.6 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          obs._obs.push_back(frqObs1P);
158
159          t_frqObs* frqObs2P = new t_frqObs;
160          frqObs2P->_rnxType2ch = (sys == 'G') ? "2W" : "2P";
161          frqObs2P->_codeValid = true;
162          frqObs2P->_code = _ObsBlock.rng_P2[iSat];
163          frqObs2P->_phaseValid = true;
164          frqObs2P->_phase = _ObsBlock.resolvedPhase_L2(iSat);
165          frqObs2P->_slipCounter = _ObsBlock.slip_L2[iSat];
166          obs._obs.push_back(frqObs2P);
167
168          _obsList.push_back(obs);
169        }
170        _ObsBlock.clear();
171      }
172    }
173
174    else if (_PP.ID() == 20 || _PP.ID() == 21) {
175      _msg2021.extract(_PP);
176
177      if (_msg2021.valid()) {
178        decoded = true;
179        translateCorr2Obs(errmsg);
180      }
181    }
182
183    else if (_PP.ID() == 3) {
184      _msg03.extract(_PP);
185    }
186
187    else if (_PP.ID() == 22) {
188      _msg22.extract(_PP);
189    }
190
191    else if (_PP.ID() == 23) {
192      _msg23.extract(_PP);
193    }
194
195    else if (_PP.ID() == 24) {
196      _msg24.extract(_PP);
197    }
198
199    // Output for RTCM scan
200    if (_PP.ID() == 3) {
201      if (_msg03.validMsg) {
202        _antList.push_back(t_antInfo());
203
204        this->getStaCrd(_antList.back().xx, _antList.back().yy,
205            _antList.back().zz);
206
207        _antList.back().type = t_antInfo::APC;
208        _antList.back().message = _PP.ID();
209      }
210    } else if (_PP.ID() == 23) {
211      if (_msg23.validMsg) {
212        _antType.push_back(_msg23.antType.c_str());
213      }
214    } else if (_PP.ID() == 24) {
215      if (_msg24.validMsg) {
216        _antList.push_back(t_antInfo());
217
218        _antList.back().xx = _msg24.x;
219        _antList.back().yy = _msg24.y;
220        _antList.back().zz = _msg24.z;
221
222        _antList.back().height_f = true;
223        _antList.back().height = _msg24.h;
224
225        _antList.back().type = t_antInfo::ARP;
226        _antList.back().message = _PP.ID();
227      }
228    }
229  }
230  return success;
231}
232
233void RTCM2Decoder::translateCorr2Obs(vector<string>& errmsg) {
234
235  QMutexLocker locker(&_mutex);
236
237  if (!_msg03.validMsg || !_msg2021.valid()) {
238    return;
239  }
240
241  double stax = _msg03.x + (_msg22.validMsg ? _msg22.dL1[0] : 0.0);
242  double stay = _msg03.y + (_msg22.validMsg ? _msg22.dL1[1] : 0.0);
243  double staz = _msg03.z + (_msg22.validMsg ? _msg22.dL1[2] : 0.0);
244
245  int refWeek;
246  double refSecs;
247  currentGPSWeeks(refWeek, refSecs);
248
249  // Resolve receiver time of measurement (see RTCM 2.3, page 4-42, Message 18, Note 1)
250  // ----------------------------------------------------------------------------------
251  double hoursec_est = _msg2021.hoursec();      // estimated time of measurement
252  double hoursec_rcv = rint(hoursec_est * 1e2) / 1e2; // receiver clock reading at hoursec_est
253  double rcv_clk_bias = (hoursec_est - hoursec_rcv) * c_light;
254
255  int GPSWeek;
256  double GPSWeeks;
257  resolveEpoch(hoursec_est, refWeek, refSecs, GPSWeek, GPSWeeks);
258
259  int GPSWeek_rcv;
260  double GPSWeeks_rcv;
261  resolveEpoch(hoursec_rcv, refWeek, refSecs, GPSWeek_rcv, GPSWeeks_rcv);
262
263  // Loop over all satellites
264  // ------------------------
265  for (RTCM2_2021::data_iterator icorr = _msg2021.data.begin();
266      icorr != _msg2021.data.end(); icorr++) {
267    const RTCM2_2021::HiResCorr* corr = icorr->second;
268
269    // beg test
270    if (corr->PRN >= 200) {
271      continue;
272    }
273    // end test
274
275    QString prn;
276    char sys;
277    if (corr->PRN < 200) {
278      sys = 'G';
279      prn = sys + QString("%1_0").arg(corr->PRN, 2, 10, QChar('0'));
280    } else {
281      sys = 'R';
282      prn = sys + QString("%1_0").arg(corr->PRN - 200, 2, 10, QChar('0'));
283    }
284
285    double L1 = 0;
286    double L2 = 0;
287    double P1 = 0;
288    double P2 = 0;
289    string obsT = "";
290
291    // new observation
292    t_satObs new_obs;
293
294    t_frqObs* frqObs1C = new t_frqObs;
295    frqObs1C->_rnxType2ch = "1C";
296    new_obs._obs.push_back(frqObs1C);
297
298    t_frqObs* frqObs1P = new t_frqObs;
299    frqObs1P->_rnxType2ch = (sys == 'G') ? "1W" : "1P";
300    new_obs._obs.push_back(frqObs1P);
301
302    t_frqObs* frqObs2P = new t_frqObs;
303    frqObs2P->_rnxType2ch = (sys == 'G') ? "2W" : "2P";
304    new_obs._obs.push_back(frqObs2P);
305
306    // missing IOD
307    vector<string> missingIOD;
308    vector<string> hasIOD;
309    for (unsigned ii = 0; ii < 4; ii++) {
310      unsigned int IODcorr = 0;
311      double corrVal = 0;
312      const t_eph* eph = 0;
313      double* obsVal = 0;
314
315      switch (ii) {
316        case 0: // --- L1 ---
317          IODcorr = corr->IODp1;
318          corrVal = corr->phase1 * LAMBDA_1;
319          obsVal = &L1;
320          obsT = "L1";
321          break;
322        case 1: // --- L2 ---
323          IODcorr = corr->IODp2;
324          corrVal = corr->phase2 * LAMBDA_2;
325          obsVal = &L2;
326          obsT = "L2";
327          break;
328        case 2: // --- P1 ---
329          IODcorr = corr->IODr1;
330          corrVal = corr->range1;
331          obsVal = &P1;
332          obsT = "P1";
333          break;
334        case 3: // --- P2 ---
335          IODcorr = corr->IODr2;
336          corrVal = corr->range2;
337          obsVal = &P2;
338          obsT = "P2";
339          break;
340        default:
341          continue;
342      }
343
344      // Select corresponding ephemerides
345      const t_eph* ephLast = _ephUser.ephLast(prn);
346      const t_eph* ephPrev = _ephUser.ephPrev(prn);
347      if (ephLast && ephLast->IOD() == IODcorr) {
348        eph = ephLast;
349      } else if (ephPrev && ephPrev->IOD() == IODcorr) {
350        eph = ephPrev;
351      }
352
353      if (eph) {
354        ostringstream msg;
355        msg << obsT << ':' << setw(3) << eph->IOD();
356        hasIOD.push_back(msg.str());
357
358        int GPSWeek_tot;
359        double GPSWeeks_tot;
360        double rho, xSat, ySat, zSat, clkSat;
361        cmpRho(eph, stax, stay, staz, GPSWeek, GPSWeeks, rho, GPSWeek_tot,
362            GPSWeeks_tot, xSat, ySat, zSat, clkSat);
363
364        *obsVal = rho - corrVal + rcv_clk_bias - clkSat;
365
366        if (*obsVal == 0)
367          *obsVal = ZEROVALUE;
368
369        if (corr->PRN < 200) {
370          new_obs._prn.set('G', corr->PRN);
371        } else {
372          new_obs._prn.set('R', corr->PRN - 200);
373        }
374        new_obs._time.set(GPSWeek_rcv, GPSWeeks_rcv);
375
376        // Store estimated measurements
377        // ----------------------------
378        switch (ii) {
379          case 0: // --- L1 ---
380            frqObs1P->_phaseValid = true;
381            frqObs1P->_phase = *obsVal / LAMBDA_1;
382            frqObs1P->_slipCounter = corr->lock1;
383            break;
384          case 1: // --- L2 ---
385            frqObs2P->_phaseValid = true;
386            frqObs2P->_phase = *obsVal / LAMBDA_2;
387            frqObs2P->_slipCounter = corr->lock2;
388            break;
389          case 2: // --- C1 / P1 ---
390            if (corr->Pind1) {
391              frqObs1P->_codeValid = true;
392              frqObs1P->_code = *obsVal;
393            } else {
394              frqObs1C->_codeValid = true;
395              frqObs1C->_code = *obsVal;
396            }
397            break;
398          case 3: // --- C2 / P2 ---
399            if (corr->Pind2) {
400              frqObs2P->_codeValid = true;
401              frqObs2P->_code = *obsVal;
402            }
403            break;
404          default:
405            continue;
406        }
407      } else if (IODcorr != 0) {
408        ostringstream msg;
409        msg << obsT << ':' << setw(3) << IODcorr;
410        missingIOD.push_back(msg.str());
411      }
412    } // loop over frequencies
413
414    // Error report
415    if (missingIOD.size()) {
416      ostringstream missingIODstr;
417
418      copy(missingIOD.begin(), missingIOD.end(),
419          ostream_iterator<string>(missingIODstr, "   "));
420
421      errmsg.push_back(
422          "missing eph for " + string(prn.toAscii().data()) + " , IODs "
423              + missingIODstr.str());
424    }
425
426    // Store new observation
427    if (new_obs._time.mjd() > 0) {
428      _obsList.push_back(new_obs);
429    }
430  }
431}
Note: See TracBrowser for help on using the repository browser.