source: ntrip/trunk/BNC/src/PPP_free/bncpppclient.cpp@ 6059

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