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

Last change on this file since 5569 was 5569, checked in by mervart, 10 years ago
File size: 14.9 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 _streamID[0] = -1;
73 _streamID[1] = -1;
74 _streamID[2] = -1;
75
76 if (connectSlots) {
77 connect(this, SIGNAL(newMessage(QByteArray,bool)),
78 BNC_CORE, SLOT(slotMessage(const QByteArray,bool)));
79
80 connect(BNC_CORE, SIGNAL(newCorrections(QList<QString>)),
81 this, SLOT(slotNewCorrections(QList<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->useGlonass) return;
115 }
116 else if (obs.satSys == 'E') {
117 if (!_opt->useGalileo) 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 double f1 = t_CST::f1(obs.satSys, obs.slotNum);
214 double f2 = t_CST::f2(obs.satSys, obs.slotNum);
215 double a1 = f1 * f1 / (f1 * f1 - f2 * f2);
216 double a2 = - f2 * f2 / (f1 * f1 - f2 * f2);
217 satData->L1 = satData->L1 * t_CST::c / f1;
218 satData->L2 = satData->L2 * t_CST::c / f2;
219 satData->P3 = a1 * satData->P1 + a2 * satData->P2;
220 satData->L3 = a1 * satData->L1 + a2 * satData->L2;
221 satData->lambda3 = a1 * t_CST::c / f1 + a2 * t_CST::c / f2;
222 _epoData.back()->satData[satData->prn] = satData;
223 }
224 else {
225 delete satData;
226 }
227 }
228
229 // Set Observations Galileo
230 // ------------------------
231 else if (obs.satSys == 'E') {
232 satData->P1 = obs.measdata("C1", 3.0);
233 satData->L1 = obs.measdata("L1", 3.0);
234 satData->P5 = obs.measdata("C5", 3.0);
235 satData->L5 = obs.measdata("L5", 3.0);
236 if (satData->P1 != 0.0 && satData->P5 != 0.0 &&
237 satData->L1 != 0.0 && satData->L5 != 0.0 ) {
238 double f1 = t_CST::freq1;
239 double f5 = t_CST::freq5;
240 double a1 = f1 * f1 / (f1 * f1 - f5 * f5);
241 double a5 = - f5 * f5 / (f1 * f1 - f5 * f5);
242 satData->L1 = satData->L1 * t_CST::c / f1;
243 satData->L5 = satData->L5 * t_CST::c / f5;
244 satData->P3 = a1 * satData->P1 + a5 * satData->P5;
245 satData->L3 = a1 * satData->L1 + a5 * satData->L5;
246 satData->lambda3 = a1 * t_CST::c / f1 + a5 * t_CST::c / f5;
247 _epoData.back()->satData[satData->prn] = satData;
248 }
249 else {
250 delete satData;
251 }
252 }
253}
254
255//
256////////////////////////////////////////////////////////////////////////////
257void bncPPPclient::slotNewCorrections(QList<QString> corrList) {
258 QMutexLocker locker(&_mutex);
259
260 // Check the Mountpoint (source of corrections)
261 // --------------------------------------------
262 if (!_opt->pppCorrMount.isEmpty()) {
263 QMutableListIterator<QString> itm(corrList);
264 while (itm.hasNext()) {
265 QStringList hlp = itm.next().split(" ");
266 if (hlp.size() > 0) {
267 QString mountpoint = hlp[hlp.size()-1];
268 if (mountpoint != _opt->pppCorrMount) {
269 itm.remove();
270 }
271 }
272 }
273 }
274
275 if (corrList.size() == 0) {
276 return;
277 }
278
279 QListIterator<QString> it(corrList);
280 while (it.hasNext()) {
281 QString line = it.next();
282
283 QTextStream in(&line);
284 int messageType;
285 int updateInterval;
286 int GPSweek;
287 double GPSweeks;
288 QString prn;
289 in >> messageType >> updateInterval >> GPSweek >> GPSweeks >> prn;
290
291 if ( t_corr::relevantMessageType(messageType) ) {
292 t_corr* cc = 0;
293 if (_corr.contains(prn)) {
294 cc = _corr.value(prn);
295 }
296 else {
297 cc = new t_corr();
298 _corr[prn] = cc;
299 }
300 cc->readLine(line);
301 checkProviderID(cc);
302 _corr_tt = cc->tClk;
303 }
304 else if ( messageType == BTYPE_GPS || messageType == BTYPE_GLONASS ) {
305 t_bias* bb = 0;
306 if (_bias.contains(prn)) {
307 bb = _bias.value(prn);
308 }
309 else {
310 bb = new t_bias();
311 _bias[prn] = bb;
312 }
313 bb->readLine(line);
314 }
315 }
316}
317
318// Satellite Position
319////////////////////////////////////////////////////////////////////////////
320t_irc bncPPPclient::getSatPos(const bncTime& tt, const QString& prn,
321 ColumnVector& xc, ColumnVector& vv) {
322
323 const double MAXAGE = 120.0;
324
325 if (_eph.contains(prn)) {
326
327 if (_opt->pppMode && prn[0] != 'E') {
328 if (_corr.contains(prn)) {
329 t_corr* cc = _corr.value(prn);
330 if (cc->ready() && tt - cc->tClk < MAXAGE) {
331 t_eph* eLast = _eph.value(prn)->last;
332 t_eph* ePrev = _eph.value(prn)->prev;
333 if (eLast && eLast->IOD() == cc->iod) {
334 eLast->position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data());
335 return applyCorr(tt, cc, xc, vv);
336 }
337 else if (ePrev && ePrev->IOD() == cc->iod) {
338 ePrev->position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data());
339 return applyCorr(tt, cc, xc, vv);
340 }
341 }
342 }
343 }
344
345 else {
346 t_eph* ee = _eph.value(prn)->last;
347 ee->position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data());
348 return success;
349 }
350 }
351
352 return failure;
353}
354
355//
356////////////////////////////////////////////////////////////////////////////
357t_irc bncPPPclient::applyCorr(const bncTime& tt, const t_corr* cc,
358 ColumnVector& xc, ColumnVector& vv) {
359
360 double dtRao = tt - cc->tRao;
361
362 // Position
363 // --------
364 ColumnVector raoHlp = cc->rao + cc->dotRao * dtRao;
365
366 if (raoHlp.norm_Frobenius() > 20.0) {
367 return failure;
368 }
369
370 ColumnVector dx(3);
371 RSW_to_XYZ(xc.Rows(1,3), vv, raoHlp, dx);
372 xc[0] -= dx[0];
373 xc[1] -= dx[1];
374 xc[2] -= dx[2];
375
376 // Velocity
377 // --------
378 ColumnVector dotRaoHlp = cc->dotRao;
379
380 ColumnVector dv(3);
381 RSW_to_XYZ(xc.Rows(1,3), vv, dotRaoHlp, dv);
382 vv[0] -= dv[0];
383 vv[1] -= dv[1];
384 vv[2] -= dv[2];
385
386 // Clocks
387 // ------
388 double dtClk = tt - cc->tClk;
389
390 xc[3] += cc->dClk + cc->dotDClk * dtClk + cc->dotDotDClk * dtClk * dtClk
391 + cc->hrClk;
392
393 return success;
394}
395
396// Correct Time of Transmission
397////////////////////////////////////////////////////////////////////////////
398t_irc bncPPPclient::cmpToT(t_satData* satData) {
399
400 double prange = satData->P3;
401 if (prange == 0.0) {
402 return failure;
403 }
404
405 double clkSat = 0.0;
406 for (int ii = 1; ii <= 10; ii++) {
407
408 bncTime ToT = satData->tt - prange / t_CST::c - clkSat;
409
410 ColumnVector xc(4);
411 ColumnVector vv(3);
412 if (getSatPos(ToT, satData->prn, xc, vv) != success) {
413 return failure;
414 }
415
416 double clkSatOld = clkSat;
417 clkSat = xc(4);
418
419 if ( fabs(clkSat-clkSatOld) * t_CST::c < 1.e-4 ) {
420 satData->xx = xc.Rows(1,3);
421 satData->vv = vv;
422 satData->clk = clkSat * t_CST::c;
423 return success;
424 }
425 }
426
427 return failure;
428}
429
430//
431////////////////////////////////////////////////////////////////////////////
432void bncPPPclient::processFrontEpoch() {
433
434#ifdef BNC_DEBUG
435 QString msg = "List of Corrections\n";
436 QMapIterator<QString, t_corr*> itC(_corr);
437 while (itC.hasNext()) {
438 itC.next();
439 QString src = itC.key();
440 const t_corr* corr = itC.value();
441 msg += QString("%1 %2 %3 %4\n")
442 .arg(corr->prn)
443 .arg(corr->iod)
444 .arg(QString(corr->tClk.datestr().c_str()) + "_" + QString(corr->tClk.timestr().c_str()))
445 .arg(QString(corr->tRao.datestr().c_str()) + "_" + QString(corr->tRao.timestr().c_str()));
446 }
447
448 msg += "List of Ephemeris\n";
449 QMapIterator<QString, t_ephPair*> itE(_eph);
450 while (itE.hasNext()) {
451 itE.next();
452 QString prn = itE.key();
453 const t_ephPair* ephPair = itE.value();
454 if (ephPair->prev) {
455 msg += QString("%1 %2 %3 %4 %5\n")
456 .arg(prn)
457 .arg(ephPair->last->IOD())
458 .arg(QString(ephPair->last->TOC().datestr().c_str()) + "_" +
459 QString(ephPair->last->TOC().timestr().c_str()))
460 .arg(ephPair->prev->IOD())
461 .arg(QString(ephPair->prev->TOC().datestr().c_str()) + "_" +
462 QString(ephPair->prev->TOC().timestr().c_str()));
463 }
464 else {
465 msg += QString("%1 %2 %3\n")
466 .arg(prn)
467 .arg(ephPair->last->IOD())
468 .arg(QString(ephPair->last->TOC().datestr().c_str()) + "_" +
469 QString(ephPair->last->TOC().timestr().c_str()));
470 }
471 }
472
473 emit newMessage(msg.toAscii(), false);
474#endif // BNC_DEBUG
475
476 // Data Pre-Processing
477 // -------------------
478 QMutableMapIterator<QString, t_satData*> it(_epoData.front()->satData);
479 while (it.hasNext()) {
480 it.next();
481 QString prn = it.key();
482 t_satData* satData = it.value();
483
484 if (cmpToT(satData) != success) {
485 delete satData;
486 it.remove();
487 continue;
488 }
489 }
490
491 // Filter Solution
492 // ---------------
493 if (_model->update(_epoData.front()) == success) {
494 emit newPosition(_model->time(), _model->x(), _model->y(), _model->z());
495 }
496}
497
498//
499////////////////////////////////////////////////////////////////////////////
500void bncPPPclient::processEpochs() {
501
502 // Make sure the buffer does not grow beyond any limit
503 // ---------------------------------------------------
504 const unsigned MAX_EPODATA_SIZE = 120;
505 if (_epoData.size() > MAX_EPODATA_SIZE) {
506 delete _epoData.front();
507 _epoData.pop();
508 }
509
510 // Loop over all unprocessed epochs
511 // --------------------------------
512 while (!_epoData.empty()) {
513
514 t_epoData* frontEpoData = _epoData.front();
515
516 // No corrections yet, skip the epoch
517 // ----------------------------------
518 if (_opt->corrSync != 0.0 && !_corr_tt.valid()) {
519 return;
520 }
521
522 // Process the front epoch
523 // -----------------------
524 if (_opt->corrSync == 0 || frontEpoData->tt - _corr_tt < _opt->corrSync) {
525 processFrontEpoch();
526 delete _epoData.front();
527 _epoData.pop();
528 }
529 else {
530 return;
531 }
532 }
533}
534
535//
536////////////////////////////////////////////////////////////////////////////
537void bncPPPclient::checkProviderID(const t_corr* corr) {
538
539 bool alreadySet = false;
540 bool different = false;
541
542 for (unsigned ii = 0; ii < 3; ii++) {
543 if (_streamID[ii] != -1) {
544 alreadySet = true;
545 }
546 if (corr->streamID[ii] != -1) {
547 if (_streamID[ii] != corr->streamID[ii]) {
548 different = true;
549 }
550 _streamID[ii] = corr->streamID[ii];
551 }
552 }
553
554 if (alreadySet && different) {
555 _model->reset();
556 }
557}
Note: See TracBrowser for help on using the repository browser.