source: ntrip/branches/BNC_2.11.0/src/bncpppclient.cpp@ 10501

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