source: ntrip/trunk/BNC/src/bncpppclient.cpp@ 4282

Last change on this file since 4282 was 4278, checked in by mervart, 12 years ago
File size: 14.2 KB
RevLine 
[2035]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: bncPPPclient
30 *
31 * Purpose: Precise Point Positioning
32 *
33 * Author: L. Mervart
34 *
35 * Created: 21-Nov-2009
36 *
37 * Changes:
38 *
39 * -----------------------------------------------------------------------*/
40
[2249]41#include <newmatio.h>
[2238]42#include <iomanip>
[2549]43#include <sstream>
[2238]44
[2035]45#include "bncpppclient.h"
[2113]46#include "bncapp.h"
[2043]47#include "bncutils.h"
[2044]48#include "bncconst.h"
[2058]49#include "bncmodel.h"
[3635]50#include "pppopt.h"
[2035]51
52using namespace std;
53
54// Constructor
55////////////////////////////////////////////////////////////////////////////
[3640]56bncPPPclient::bncPPPclient(QByteArray staID, t_pppOpt* opt, bool connectSlots) :
57 bncEphUser(connectSlots) {
[2113]58
[3635]59 if (opt) {
60 _opt = opt;
61 _optOwner = false;
[2226]62 }
63 else {
[3635]64 _opt = new t_pppOpt();
65 _optOwner = true;
[2226]66 }
67
[3635]68 _staID = staID;
[2776]69
[3638]70 _model = new bncModel(this);
71
[3640]72 if (connectSlots) {
73 connect(this, SIGNAL(newMessage(QByteArray,bool)),
74 ((bncApp*)qApp), SLOT(slotMessage(const QByteArray,bool)));
[2548]75
[3640]76 connect(((bncApp*)qApp), SIGNAL(newCorrections(QList<QString>)),
77 this, SLOT(slotNewCorrections(QList<QString>)));
78 }
[2035]79}
80
81// Destructor
82////////////////////////////////////////////////////////////////////////////
83bncPPPclient::~bncPPPclient() {
[2058]84 delete _model;
[2808]85 while (!_epoData.empty()) {
86 delete _epoData.front();
87 _epoData.pop();
88 }
[2035]89 QMapIterator<QString, t_corr*> ic(_corr);
90 while (ic.hasNext()) {
91 ic.next();
92 delete ic.value();
93 }
[2274]94 QMapIterator<QString, t_bias*> ib(_bias);
95 while (ib.hasNext()) {
96 ib.next();
97 delete ib.value();
98 }
[3635]99 if (_optOwner) {
100 delete _opt;
101 }
[2035]102}
103
104//
105////////////////////////////////////////////////////////////////////////////
[2711]106void bncPPPclient::putNewObs(const t_obs& obs) {
[2037]107 QMutexLocker locker(&_mutex);
[2051]108
[2777]109 if (obs.satSys == 'R') {
[3635]110 if (!_opt->useGlonass) return;
[2225]111 }
[2777]112 else if (obs.satSys == 'E') {
[3635]113 if (!_opt->useGalileo) return;
[2776]114 }
115 else if (obs.satSys != 'G') {
116 return;
117 }
[2225]118
[2051]119 t_satData* satData = new t_satData();
[2808]120 satData->tt = bncTime(obs.GPSWeek, obs.GPSWeeks);
[2051]121
[2274]122 // Satellite Number
123 // ----------------
[2777]124 satData->prn = QString("%1%2").arg(obs.satSys).arg(obs.satNum,2,10,QChar('0'));
[2274]125
[2505]126 // Check Slips
127 // -----------
128 slipInfo& sInfo = _slips[satData->prn];
[2711]129 if ( sInfo.slipCntL1 == obs.slip_cnt_L1 &&
[2778]130 sInfo.slipCntL2 == obs.slip_cnt_L2 &&
131 sInfo.slipCntL5 == obs.slip_cnt_L5 ) {
[2505]132 satData->slipFlag = false;
133 }
134 else {
135 satData->slipFlag = true;
136 }
[2711]137 sInfo.slipCntL1 = obs.slip_cnt_L1;
138 sInfo.slipCntL2 = obs.slip_cnt_L2;
[2505]139
[2274]140 // Handle Code Biases
141 // ------------------
142 t_bias* bb = 0;
143 if (_bias.contains(satData->prn)) {
144 bb = _bias.value(satData->prn);
145 }
146
[2808]147 // Add new epoch, process the older ones
148 // -------------------------------------
149 if (_epoData.size() == 0) {
150 _epoData.push(new t_epoData());
151 _epoData.back()->tt = satData->tt;
[2037]152 }
[2808]153 else if (satData->tt != _epoData.back()->tt) {
154 processEpochs();
155 _epoData.push(new t_epoData());
156 _epoData.back()->tt = satData->tt;
[2037]157 }
[2039]158
[2786]159 // Set Observations GPS
160 // --------------------
[2711]161 if (obs.satSys == 'G') {
[4264]162 if ( (obs.P1 || obs.C1) && (obs.P2 || obs.C2) && obs.l1() && obs.l2() ) {
[2786]163 double f1 = t_CST::freq1;
164 double f2 = t_CST::freq2;
165 double c1 = f1 * f1 / (f1 * f1 - f2 * f2);
166 double c2 = - f2 * f2 / (f1 * f1 - f2 * f2);
167 if (obs.P1) {
168 satData->P1 = obs.P1 + (bb ? bb->p1 : 0.0);
169 }
170 else {
171 satData->P1 = obs.C1 + (bb ? bb->c1 : 0.0);
172 }
173 if (obs.P2) {
174 satData->P2 = obs.P2 + (bb ? bb->p2 : 0.0);
175 }
176 else {
177 satData->P2 = obs.C2;
178 }
[4264]179 satData->L1 = obs.l1() * t_CST::c / f1;
180 satData->L2 = obs.l2() * t_CST::c / f2;
[2786]181 satData->P3 = c1 * satData->P1 + c2 * satData->P2;
182 satData->L3 = c1 * satData->L1 + c2 * satData->L2;
183 satData->lambda3 = c1 * t_CST::c / f1 + c2 * t_CST::c / f2;
[2778]184
[3408]185 _epoData.back()->satData[satData->prn] = satData;
[2778]186 }
187 else {
188 delete satData;
189 }
[2786]190 }
[2778]191
[2786]192 // Set Observations GLONASS
193 // ------------------------
[2711]194 else if (obs.satSys == 'R') {
[4264]195 if ( (obs.P1 || obs.C1) && (obs.P2 || obs.C2) && obs.l1() && obs.l2() ) {
[4265]196 double f1 = t_CST::f1(obs.satSys, obs.slotNum);
197 double f2 = t_CST::f1(obs.satSys, obs.slotNum);
[2786]198 double c1 = f1 * f1 / (f1 * f1 - f2 * f2);
199 double c2 = - f2 * f2 / (f1 * f1 - f2 * f2);
200 if (obs.P1) {
201 satData->P1 = obs.P1 + (bb ? bb->p1 : 0.0);
202 }
203 else {
204 satData->P1 = obs.C1 + (bb ? bb->c1 : 0.0);
205 }
206 if (obs.P2) {
207 satData->P2 = obs.P2 + (bb ? bb->p2 : 0.0);
208 }
209 else {
210 satData->P2 = obs.C2;
211 }
[4264]212 satData->L1 = obs.l1() * t_CST::c / f1;
213 satData->L2 = obs.l2() * t_CST::c / f2;
[2786]214 satData->P3 = c1 * satData->P1 + c2 * satData->P2;
215 satData->L3 = c1 * satData->L1 + c2 * satData->L2;
216 satData->lambda3 = c1 * t_CST::c / f1 + c2 * t_CST::c / f2;
[2778]217
[3408]218 _epoData.back()->satData[satData->prn] = satData;
[2778]219 }
220 else {
221 delete satData;
222 }
[2786]223 }
[2778]224
[2786]225 // Set Observations Galileo
226 // ------------------------
[2778]227 else if (obs.satSys == 'E') {
[4264]228 if ( obs.C1 && obs.C5 && obs.l1() && obs.L5) {
[2786]229 double f1 = t_CST::freq1;
230 double f5 = t_CST::freq5;
231 double c1 = f1 * f1 / (f1 * f1 - f5 * f5);
232 double c5 = - f5 * f5 / (f1 * f1 - f5 * f5);
[2778]233
[2786]234 satData->P1 = obs.C1;
235 satData->P5 = obs.C5;
[4264]236 satData->L1 = obs.l1() * t_CST::c / f1;
[2786]237 satData->L5 = obs.L5 * t_CST::c / f5;
238 satData->P3 = c1 * satData->P1 + c5 * satData->P5;
239 satData->L3 = c1 * satData->L1 + c5 * satData->L5;
240 satData->lambda3 = c1 * t_CST::c / f1 + c5 * t_CST::c / f5;
[3408]241 _epoData.back()->satData[satData->prn] = satData;
[2778]242 }
243 else {
244 delete satData;
245 }
246 }
[2035]247}
248
249//
250////////////////////////////////////////////////////////////////////////////
251void bncPPPclient::slotNewCorrections(QList<QString> corrList) {
252 QMutexLocker locker(&_mutex);
[2069]253
[2966]254 // Check the Mountpoint (source of corrections)
255 // --------------------------------------------
[3635]256 if (!_opt->pppCorrMount.isEmpty()) {
[2966]257 QMutableListIterator<QString> itm(corrList);
258 while (itm.hasNext()) {
259 QStringList hlp = itm.next().split(" ");
260 if (hlp.size() > 0) {
261 QString mountpoint = hlp[hlp.size()-1];
[3635]262 if (mountpoint != _opt->pppCorrMount) {
[2966]263 itm.remove();
264 }
265 }
266 }
267 }
268
[2069]269 if (corrList.size() == 0) {
270 return;
271 }
272
[2035]273 QListIterator<QString> it(corrList);
274 while (it.hasNext()) {
[2911]275 QString line = it.next();
276
277 QTextStream in(&line);
[2035]278 int messageType;
279 int updateInterval;
280 int GPSweek;
281 double GPSweeks;
282 QString prn;
283 in >> messageType >> updateInterval >> GPSweek >> GPSweeks >> prn;
[2809]284
[2911]285 if ( t_corr::relevantMessageType(messageType) ) {
[2035]286 t_corr* cc = 0;
287 if (_corr.contains(prn)) {
288 cc = _corr.value(prn);
289 }
290 else {
291 cc = new t_corr();
292 _corr[prn] = cc;
293 }
[2207]294
[2911]295 cc->readLine(line);
[3751]296 _corr_tt = cc->tClk;
[2035]297 }
[2317]298 else if ( messageType == BTYPE_GPS ) {
[2274]299
300 t_bias* bb = 0;
301 if (_bias.contains(prn)) {
[2341]302 bb = _bias.value(prn);
[2274]303 }
304 else {
305 bb = new t_bias();
306 _bias[prn] = bb;
307 }
308
309 bb->tt.set(GPSweek, GPSweeks);
310
311 int numBiases;
312 in >> numBiases;
313 for (int ii = 0; ii < numBiases; ++ii) {
314 int bType;
315 double bValue;
316 in >> bType >> bValue;
[2426]317 if (bType == CODETYPEGPS_L1_Z) {
[2429]318 bb->p1 = bValue;
[2274]319 }
[2426]320 else if (bType == CODETYPEGPS_L1_CA) {
[2429]321 bb->c1 = bValue;
[2426]322 }
[2365]323 else if (bType == CODETYPEGPS_L2_Z) {
[2429]324 bb->p2 = bValue;
[2274]325 }
326 }
327 }
[2035]328 }
329}
330
331// Satellite Position
332////////////////////////////////////////////////////////////////////////////
[2123]333t_irc bncPPPclient::getSatPos(const bncTime& tt, const QString& prn,
[2357]334 ColumnVector& xc, ColumnVector& vv) {
[2035]335
[2576]336 const double MAXAGE = 120.0;
[2041]337
[2035]338 if (_eph.contains(prn)) {
[2040]339
[3635]340 if (_opt->pppMode) {
[2068]341 if (_corr.contains(prn)) {
342 t_corr* cc = _corr.value(prn);
[3751]343 if (cc->ready() && tt - cc->tClk < MAXAGE) {
[2576]344 t_eph* eLast = _eph.value(prn)->last;
345 t_eph* ePrev = _eph.value(prn)->prev;
346 if (eLast && eLast->IOD() == cc->iod) {
347 eLast->position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data());
[3521]348 return applyCorr(tt, cc, xc, vv);
[2576]349 }
350 else if (ePrev && ePrev->IOD() == cc->iod) {
351 ePrev->position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data());
[3521]352 return applyCorr(tt, cc, xc, vv);
[2576]353 }
354 }
[2041]355 }
[2040]356 }
[2576]357
358 else {
359 t_eph* ee = _eph.value(prn)->last;
360 ee->position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data());
361 return success;
362 }
[2035]363 }
364
365 return failure;
366}
367
368//
369////////////////////////////////////////////////////////////////////////////
[3319]370t_irc bncPPPclient::applyCorr(const bncTime& tt, const t_corr* cc,
[3521]371 ColumnVector& xc, ColumnVector& vv) {
[2041]372
[3751]373 double dtRao = tt - cc->tRao;
374 ColumnVector raoHlp = cc->rao + cc->dotRao * dtRao
[4148]375 + 0.5 * cc->dotDotRao * dtRao * dtRao;
[2296]376
[3319]377 if (raoHlp.norm_Frobenius() > 20.0) {
378 return failure;
379 }
380
[3521]381 ColumnVector dx(3);
382 RSW_to_XYZ(xc.Rows(1,3), vv, raoHlp, dx);
383 xc[0] -= dx[0];
384 xc[1] -= dx[1];
385 xc[2] -= dx[2];
[3751]386
387 double dtClk = tt - cc->tClk;
388
389 xc[3] += cc->dClk + cc->dotDClk * dtClk + cc->dotDotDClk * dtClk * dtClk
[3521]390 + cc->hrClk;
[2296]391
[3319]392 return success;
[2041]393}
394
[2049]395// Correct Time of Transmission
396////////////////////////////////////////////////////////////////////////////
[2240]397t_irc bncPPPclient::cmpToT(t_satData* satData) {
[2049]398
[2051]399 double prange = satData->P3;
[2049]400 if (prange == 0.0) {
401 return failure;
402 }
403
404 double clkSat = 0.0;
405 for (int ii = 1; ii <= 10; ii++) {
406
[2808]407 bncTime ToT = satData->tt - prange / t_CST::c - clkSat;
[2049]408
409 ColumnVector xc(4);
410 ColumnVector vv(3);
[2357]411 if (getSatPos(ToT, satData->prn, xc, vv) != success) {
[2049]412 return failure;
413 }
414
415 double clkSatOld = clkSat;
[2050]416 clkSat = xc(4);
[2049]417
418 if ( fabs(clkSat-clkSatOld) * t_CST::c < 1.e-4 ) {
419 satData->xx = xc.Rows(1,3);
420 satData->vv = vv;
[2050]421 satData->clk = clkSat * t_CST::c;
[2049]422 return success;
423 }
424 }
425
426 return failure;
427}
428
[2041]429//
430////////////////////////////////////////////////////////////////////////////
[2808]431void bncPPPclient::processFrontEpoch() {
[2035]432
[4149]433#ifdef BNC_DEBUG
434 QString msg = "List of Corrections\n";
435 QMapIterator<QString, t_corr*> itC(_corr);
436 while (itC.hasNext()) {
437 itC.next();
438 QString src = itC.key();
439 const t_corr* corr = itC.value();
440 msg += QString("%1 %2 %3 %4\n")
441 .arg(corr->prn)
442 .arg(corr->iod)
443 .arg(QString(corr->tClk.datestr().c_str()) + "_" + QString(corr->tClk.timestr().c_str()))
444 .arg(QString(corr->tRao.datestr().c_str()) + "_" + QString(corr->tRao.timestr().c_str()));
445 }
446
447 msg += "List of Ephemeris\n";
448 QMapIterator<QString, t_ephPair*> itE(_eph);
449 while (itE.hasNext()) {
450 itE.next();
451 QString prn = itE.key();
452 const t_ephPair* ephPair = itE.value();
453 if (ephPair->prev) {
454 msg += QString("%1 %2 %3 %4 %5\n")
455 .arg(prn)
456 .arg(ephPair->last->IOD())
457 .arg(QString(ephPair->last->TOC().datestr().c_str()) + "_" +
458 QString(ephPair->last->TOC().timestr().c_str()))
459 .arg(ephPair->prev->IOD())
460 .arg(QString(ephPair->prev->TOC().datestr().c_str()) + "_" +
461 QString(ephPair->prev->TOC().timestr().c_str()));
462 }
463 else {
464 msg += QString("%1 %2 %3\n")
465 .arg(prn)
466 .arg(ephPair->last->IOD())
467 .arg(QString(ephPair->last->TOC().datestr().c_str()) + "_" +
468 QString(ephPair->last->TOC().timestr().c_str()));
469 }
470 }
471
472 emit newMessage(msg.toAscii(), false);
473#endif // BNC_DEBUG
474
[2053]475 // Data Pre-Processing
476 // -------------------
[3408]477 QMutableMapIterator<QString, t_satData*> it(_epoData.front()->satData);
478 while (it.hasNext()) {
479 it.next();
480 QString prn = it.key();
481 t_satData* satData = it.value();
[2049]482
[2240]483 if (cmpToT(satData) != success) {
[2049]484 delete satData;
[3408]485 it.remove();
[2049]486 continue;
487 }
[2053]488 }
[2035]489
[2060]490 // Filter Solution
491 // ---------------
[2808]492 if (_model->update(_epoData.front()) == success) {
[2145]493 emit newPosition(_model->time(), _model->x(), _model->y(), _model->z());
[2060]494 }
[2035]495}
[2808]496
497//
498////////////////////////////////////////////////////////////////////////////
499void bncPPPclient::processEpochs() {
[2809]500
[2810]501 // Make sure the buffer does not grow beyond any limit
502 // ---------------------------------------------------
503 const unsigned MAX_EPODATA_SIZE = 120;
504 if (_epoData.size() > MAX_EPODATA_SIZE) {
505 delete _epoData.front();
506 _epoData.pop();
507 }
508
509 // Loop over all unprocessed epochs
510 // --------------------------------
[2809]511 while (!_epoData.empty()) {
[2810]512
[2809]513 t_epoData* frontEpoData = _epoData.front();
514
515 // No corrections yet, skip the epoch
516 // ----------------------------------
[3635]517 if (_opt->corrSync != 0.0 && !_corr_tt.valid()) {
[2810]518 return;
[2809]519 }
520
[2810]521 // Process the front epoch
522 // -----------------------
[3635]523 if (_opt->corrSync == 0 || frontEpoData->tt - _corr_tt < _opt->corrSync) {
[2809]524 processFrontEpoch();
525 delete _epoData.front();
526 _epoData.pop();
527 }
528 else {
529 return;
530 }
531 }
[2808]532}
Note: See TracBrowser for help on using the repository browser.