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

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