source: ntrip/trunk/BNC/bncpppclient.cpp@ 2779

Last change on this file since 2779 was 2779, checked in by mervart, 13 years ago
File size: 17.5 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 "bncsettings.h"
51
52extern "C" {
53#include "clock_orbit_rtcm.h"
54}
55
56using namespace std;
57
58// Constructor
59////////////////////////////////////////////////////////////////////////////
60bncPPPclient::bncPPPclient(QByteArray staID) {
61
62 bncSettings settings;
63
64 if ( Qt::CheckState(settings.value("pppGLONASS").toInt()) == Qt::Checked) {
65 _useGlonass = true;
66 }
67 else {
68 _useGlonass = false;
69 }
70
71 _useGalileo = true; // TODO
72
73 if (settings.value("pppSPP").toString() == "PPP") {
74 _pppMode = true;
75 }
76 else {
77 _pppMode = false;
78 }
79
80 connect(this, SIGNAL(newMessage(QByteArray,bool)),
81 ((bncApp*)qApp), SLOT(slotMessage(const QByteArray,bool)));
82
83 connect(((bncApp*)qApp), SIGNAL(newEphGPS(gpsephemeris)),
84 this, SLOT(slotNewEphGPS(gpsephemeris)));
85
86 connect(((bncApp*)qApp), SIGNAL(newEphGlonass(glonassephemeris)),
87 this, SLOT(slotNewEphGlonass(glonassephemeris)));
88
89 connect(((bncApp*)qApp), SIGNAL(newEphGalileo(galileoephemeris)),
90 this, SLOT(slotNewEphGalileo(galileoephemeris)));
91
92 connect(((bncApp*)qApp), SIGNAL(newCorrections(QList<QString>)),
93 this, SLOT(slotNewCorrections(QList<QString>)));
94
95 _staID = staID;
96 _epoData = 0;
97 _model = new bncModel(staID);
98 connect(_model, SIGNAL(newNMEAstr(QByteArray)),
99 this, SIGNAL(newNMEAstr(QByteArray)));
100}
101
102// Destructor
103////////////////////////////////////////////////////////////////////////////
104bncPPPclient::~bncPPPclient() {
105 delete _model;
106 delete _epoData;
107 QMapIterator<QString, t_ephPair*> it(_eph);
108 while (it.hasNext()) {
109 it.next();
110 delete it.value();
111 }
112 QMapIterator<QString, t_corr*> ic(_corr);
113 while (ic.hasNext()) {
114 ic.next();
115 delete ic.value();
116 }
117 QMapIterator<QString, t_bias*> ib(_bias);
118 while (ib.hasNext()) {
119 ib.next();
120 delete ib.value();
121 }
122}
123
124//
125////////////////////////////////////////////////////////////////////////////
126void bncPPPclient::putNewObs(const t_obs& obs) {
127 QMutexLocker locker(&_mutex);
128
129 if (obs.satSys == 'R') {
130 if (!_useGlonass) return;
131 }
132 else if (obs.satSys == 'E') {
133 if (!_useGalileo) return;
134 }
135 else if (obs.satSys != 'G') {
136 return;
137 }
138
139 t_satData* satData = new t_satData();
140
141 // Satellite Number
142 // ----------------
143 satData->prn = QString("%1%2").arg(obs.satSys).arg(obs.satNum,2,10,QChar('0'));
144
145 // Check Slips
146 // -----------
147 slipInfo& sInfo = _slips[satData->prn];
148 if ( sInfo.slipCntL1 == obs.slip_cnt_L1 &&
149 sInfo.slipCntL2 == obs.slip_cnt_L2 &&
150 sInfo.slipCntL5 == obs.slip_cnt_L5 ) {
151 satData->slipFlag = false;
152 }
153 else {
154 satData->slipFlag = true;
155 }
156 sInfo.slipCntL1 = obs.slip_cnt_L1;
157 sInfo.slipCntL2 = obs.slip_cnt_L2;
158
159 // Handle Code Biases
160 // ------------------
161 t_bias* bb = 0;
162 if (_bias.contains(satData->prn)) {
163 bb = _bias.value(satData->prn);
164 }
165
166 // Set Code Observations - P1 or C1
167 // --------------------------------
168 bool haveP1 = false;
169 if (obs.P1) {
170 satData->P1 = obs.P1 + (bb ? bb->p1 : 0.0);
171 satData->codeTypeF1 = t_satData::P_CODE;
172 haveP1 = true;
173 }
174 else if (obs.C1) {
175 satData->P1 = obs.C1 + (bb ? bb->c1 : 0.0);
176 satData->codeTypeF1 = t_satData::C_CODE;
177 haveP1 = true;
178 }
179
180 if (!haveP1) {
181 delete satData;
182 return;
183 }
184
185 // P2 or C2, and C5
186 // ----------------
187 bool haveP2 = false;
188 if (obs.P2) {
189 satData->P2 = obs.P2 + (bb ? bb->p2 : 0.0);
190 satData->codeTypeF2 = t_satData::P_CODE;
191 haveP2 = true;
192 }
193 else if (obs.C2) {
194 satData->P2 = obs.C2;
195 satData->codeTypeF2 = t_satData::C_CODE;
196 haveP2 = true;
197 }
198
199 bool haveP5 = false;
200 if (obs.C5) {
201 satData->P5 = obs.C5;
202 satData->codeTypeF2 = t_satData::P_CODE;
203 haveP5 = true;
204 }
205
206 if (!haveP2 && !haveP5) {
207 delete satData;
208 return;
209 }
210
211 // Add new Satellite to the epoch
212 // ------------------------------
213 bncTime tt(obs.GPSWeek, obs.GPSWeeks);
214
215 if (!_epoData) {
216 _epoData = new t_epoData();
217 _epoData->tt = tt;
218 }
219 else if (tt != _epoData->tt) {
220 processEpoch();
221 delete _epoData;
222 _epoData = new t_epoData();
223 _epoData->tt = tt;
224 }
225
226 // Set Ionosphere-Free Combinations
227 // --------------------------------
228 if (obs.satSys == 'G') {
229 double f1 = t_CST::freq1;
230 double f2 = t_CST::freq2;
231 double c1 = f1 * f1 / (f1 * f1 - f2 * f2);
232 double c2 = - f2 * f2 / (f1 * f1 - f2 * f2);
233
234 if (obs.L1() && obs.L2()) {
235 satData->L1 = obs.L1() * t_CST::c / f1;
236 satData->L2 = obs.L2() * t_CST::c / f2;
237 }
238 else {
239 delete satData;
240 return;
241 }
242
243 satData->P3 = c1 * satData->P1 + c2 * satData->P2;
244 satData->L3 = c1 * satData->L1 + c2 * satData->L2;
245 satData->lambda3 = c1 * t_CST::c / f1 + c2 * t_CST::c / f2;
246
247 _epoData->satDataGPS[satData->prn] = satData;
248 }
249 else if (obs.satSys == 'R') {
250 double f1 = 1602000000.0 + 562500.0 * obs.slotNum;
251 double f2 = 1246000000.0 + 437500.0 * obs.slotNum;
252 double c1 = f1 * f1 / (f1 * f1 - f2 * f2);
253 double c2 = - f2 * f2 / (f1 * f1 - f2 * f2);
254
255 if (obs.L1() && obs.L2()) {
256 satData->L1 = obs.L1() * t_CST::c / f1;
257 satData->L2 = obs.L2() * t_CST::c / f2;
258 }
259 else {
260 delete satData;
261 return;
262 }
263
264 satData->P3 = c1 * satData->P1 + c2 * satData->P2;
265 satData->L3 = c1 * satData->L1 + c2 * satData->L2;
266 satData->lambda3 = c1 * t_CST::c / f1 + c2 * t_CST::c / f2;
267
268 _epoData->satDataGlo[satData->prn] = satData;
269 }
270 else if (obs.satSys == 'E') {
271 double f1 = t_CST::freq1;
272 double f5 = t_CST::freq5;
273 double c1 = f1 * f1 / (f1 * f1 - f5 * f5);
274 double c5 = - f5 * f5 / (f1 * f1 - f5 * f5);
275
276 if (obs.L1() && obs.L5) {
277 satData->L1 = obs.L1() * t_CST::c / f1;
278 satData->L5 = obs.L5 * t_CST::c / f5;
279 }
280 else {
281 delete satData;
282 return;
283 }
284
285 satData->P3 = c1 * satData->P1 + c5 * satData->P5;
286 satData->L3 = c1 * satData->L1 + c5 * satData->L5;
287 satData->lambda3 = c1 * t_CST::c / f1 + c5 * t_CST::c / f5;
288
289 _epoData->satDataGal[satData->prn] = satData;
290 }
291}
292
293//
294////////////////////////////////////////////////////////////////////////////
295void bncPPPclient::slotNewEphGPS(gpsephemeris gpseph) {
296 QMutexLocker locker(&_mutex);
297
298 QString prn = QString("G%1").arg(gpseph.satellite, 2, 10, QChar('0'));
299
300 if (_eph.contains(prn)) {
301 t_ephGPS* eLast = static_cast<t_ephGPS*>(_eph.value(prn)->last);
302 if ( (eLast->GPSweek() < gpseph.GPSweek) ||
303 (eLast->GPSweek() == gpseph.GPSweek &&
304 eLast->TOC() < gpseph.TOC) ) {
305 delete static_cast<t_ephGPS*>(_eph.value(prn)->prev);
306 _eph.value(prn)->prev = _eph.value(prn)->last;
307 _eph.value(prn)->last = new t_ephGPS();
308 static_cast<t_ephGPS*>(_eph.value(prn)->last)->set(&gpseph);
309 }
310 }
311 else {
312 t_ephGPS* eLast = new t_ephGPS();
313 eLast->set(&gpseph);
314 _eph.insert(prn, new t_ephPair());
315 _eph[prn]->last = eLast;
316 }
317}
318
319//
320////////////////////////////////////////////////////////////////////////////
321void bncPPPclient::slotNewEphGlonass(glonassephemeris gloeph) {
322 QMutexLocker locker(&_mutex);
323
324 QString prn = QString("R%1").arg(gloeph.almanac_number, 2, 10, QChar('0'));
325
326 if (_eph.contains(prn)) {
327 int ww = gloeph.GPSWeek;
328 int tow = gloeph.GPSTOW;
329 updatetime(&ww, &tow, gloeph.tb*1000, 0); // Moscow -> GPS
330 t_ephGlo* eLast = static_cast<t_ephGlo*>(_eph.value(prn)->last);
331 if (eLast->GPSweek() < ww ||
332 (eLast->GPSweek() == ww && eLast->GPSweeks() < tow)) {
333 delete static_cast<t_ephGlo*>(_eph.value(prn)->prev);
334 _eph.value(prn)->prev = _eph.value(prn)->last;
335 _eph.value(prn)->last = new t_ephGlo();
336 static_cast<t_ephGlo*>(_eph.value(prn)->last)->set(&gloeph);
337 }
338 }
339 else {
340 t_ephGlo* eLast = new t_ephGlo();
341 eLast->set(&gloeph);
342 _eph.insert(prn, new t_ephPair());
343 _eph[prn]->last = eLast;
344 }
345}
346
347//
348////////////////////////////////////////////////////////////////////////////
349void bncPPPclient::slotNewEphGalileo(galileoephemeris galeph) {
350 QMutexLocker locker(&_mutex);
351
352 QString prn = QString("E%1").arg(galeph.satellite, 2, 10, QChar('0'));
353
354 if (_eph.contains(prn)) {
355 t_ephGal* eLast = static_cast<t_ephGal*>(_eph.value(prn)->last);
356 if ( (eLast->GPSweek() < galeph.Week) ||
357 (eLast->GPSweek() == galeph.Week &&
358 eLast->TOC() < galeph.TOC) ) {
359 delete static_cast<t_ephGal*>(_eph.value(prn)->prev);
360 _eph.value(prn)->prev = _eph.value(prn)->last;
361 _eph.value(prn)->last = new t_ephGal();
362 static_cast<t_ephGal*>(_eph.value(prn)->last)->set(&galeph);
363 }
364 }
365 else {
366 t_ephGal* eLast = new t_ephGal();
367 eLast->set(&galeph);
368 _eph.insert(prn, new t_ephPair());
369 _eph[prn]->last = eLast;
370 }
371}
372
373//
374////////////////////////////////////////////////////////////////////////////
375void bncPPPclient::slotNewCorrections(QList<QString> corrList) {
376 QMutexLocker locker(&_mutex);
377
378 if (corrList.size() == 0) {
379 return;
380 }
381
382 // Remove All Corrections
383 // ----------------------
384 QMapIterator<QString, t_corr*> ic(_corr);
385 while (ic.hasNext()) {
386 ic.next();
387 delete ic.value();
388 }
389 _corr.clear();
390
391 QListIterator<QString> it(corrList);
392 while (it.hasNext()) {
393 QTextStream in(it.next().toAscii());
394 int messageType;
395 int updateInterval;
396 int GPSweek;
397 double GPSweeks;
398 QString prn;
399 in >> messageType >> updateInterval >> GPSweek >> GPSweeks >> prn;
400 if ( messageType == COTYPE_GPSCOMBINED ||
401 messageType == COTYPE_GLONASSCOMBINED ||
402 messageType == COTYPE_GPSORBIT ||
403 messageType == COTYPE_GPSCLOCK ||
404 messageType == COTYPE_GLONASSORBIT ||
405 messageType == COTYPE_GLONASSCLOCK ) {
406
407 t_corr* cc = 0;
408 if (_corr.contains(prn)) {
409 cc = _corr.value(prn);
410 }
411 else {
412 cc = new t_corr();
413 _corr[prn] = cc;
414 }
415
416 cc->tt.set(GPSweek, GPSweeks);
417
418 if ( messageType == COTYPE_GPSCOMBINED ||
419 messageType == COTYPE_GLONASSCOMBINED ) {
420 cc->rao.ReSize(3); cc->rao = 0.0;
421 cc->dotRao.ReSize(3); cc->dotRao = 0.0;
422 cc->dotDotRao.ReSize(3); cc->dotDotRao = 0.0;
423 cc->dClk = 0.0;
424 cc->dotDClk = 0.0;
425 cc->dotDotDClk = 0.0;
426 in >> cc->iod
427 >> cc->dClk >> cc->rao[0] >> cc->rao[1] >> cc->rao[2]
428 >> cc->dotDClk >> cc->dotRao[0] >> cc->dotRao[1] >> cc->dotRao[2]
429 >> cc->dotDotDClk >> cc->dotDotRao[0] >> cc->dotDotRao[1] >> cc->dotDotRao[2];
430 cc->dClk /= t_CST::c;
431 cc->dotDClk /= t_CST::c;
432 cc->dotDotDClk /= t_CST::c;
433 cc->raoSet = true;
434 cc->dClkSet = true;
435 }
436 else if ( messageType == COTYPE_GPSORBIT ||
437 messageType == COTYPE_GLONASSORBIT ) {
438 cc->rao.ReSize(3); cc->rao = 0.0;
439 cc->dotRao.ReSize(3); cc->dotRao = 0.0;
440 cc->dotDotRao.ReSize(3); cc->dotDotRao = 0.0;
441 in >> cc->iod
442 >> cc->rao[0] >> cc->rao[1] >> cc->rao[2]
443 >> cc->dotRao[0] >> cc->dotRao[1] >> cc->dotRao[2]
444 >> cc->dotDotRao[0] >> cc->dotDotRao[1] >> cc->dotDotRao[2];
445 cc->raoSet = true;
446 }
447 else if ( messageType == COTYPE_GPSCLOCK ||
448 messageType == COTYPE_GLONASSCLOCK ) {
449 int dummyIOD;
450 cc->dClk = 0.0;
451 cc->dotDClk = 0.0;
452 cc->dotDotDClk = 0.0;
453 in >> dummyIOD >> cc->dClk >> cc->dotDClk >> cc->dotDotDClk;
454 cc->dClk /= t_CST::c;
455 cc->dotDClk /= t_CST::c;
456 cc->dotDotDClk /= t_CST::c;
457 cc->dClkSet = true;
458 }
459 }
460 else if ( messageType == BTYPE_GPS ) {
461
462 t_bias* bb = 0;
463 if (_bias.contains(prn)) {
464 bb = _bias.value(prn);
465 }
466 else {
467 bb = new t_bias();
468 _bias[prn] = bb;
469 }
470
471 bb->tt.set(GPSweek, GPSweeks);
472
473 int numBiases;
474 in >> numBiases;
475 for (int ii = 0; ii < numBiases; ++ii) {
476 int bType;
477 double bValue;
478 in >> bType >> bValue;
479 if (bType == CODETYPEGPS_L1_Z) {
480 bb->p1 = bValue;
481 }
482 else if (bType == CODETYPEGPS_L1_CA) {
483 bb->c1 = bValue;
484 }
485 else if (bType == CODETYPEGPS_L2_Z) {
486 bb->p2 = bValue;
487 }
488 }
489 }
490 }
491
492 QMutableMapIterator<QString, t_corr*> im(_corr);
493 while (im.hasNext()) {
494 im.next();
495 t_corr* cc = im.value();
496 if (!cc->ready()) {
497 delete cc;
498 im.remove();
499 }
500 }
501}
502
503// Satellite Position
504////////////////////////////////////////////////////////////////////////////
505t_irc bncPPPclient::getSatPos(const bncTime& tt, const QString& prn,
506 ColumnVector& xc, ColumnVector& vv) {
507
508 const double MAXAGE = 120.0;
509
510 if (_eph.contains(prn)) {
511
512 if (_pppMode) {
513 if (_corr.contains(prn)) {
514 t_corr* cc = _corr.value(prn);
515 if (tt - cc->tt < MAXAGE) {
516 t_eph* eLast = _eph.value(prn)->last;
517 t_eph* ePrev = _eph.value(prn)->prev;
518 if (eLast && eLast->IOD() == cc->iod) {
519 eLast->position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data());
520 applyCorr(tt, cc, xc, vv);
521 return success;
522 }
523 else if (ePrev && ePrev->IOD() == cc->iod) {
524 ePrev->position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data());
525 applyCorr(tt, cc, xc, vv);
526 return success;
527 }
528 }
529 }
530 }
531
532 else {
533 t_eph* ee = _eph.value(prn)->last;
534 ee->position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data());
535 return success;
536 }
537 }
538
539 return failure;
540}
541
542//
543////////////////////////////////////////////////////////////////////////////
544void bncPPPclient::applyCorr(const bncTime& tt, const t_corr* cc,
545 ColumnVector& xc, ColumnVector& vv) {
546 ColumnVector dx(3);
547
548 double dt = tt - cc->tt;
549 ColumnVector raoHlp = cc->rao + cc->dotRao * dt + cc->dotDotRao * dt * dt;
550
551 RSW_to_XYZ(xc.Rows(1,3), vv, raoHlp, dx);
552
553 xc[0] -= dx[0];
554 xc[1] -= dx[1];
555 xc[2] -= dx[2];
556 xc[3] += cc->dClk + cc->dotDClk * dt + cc->dotDotDClk * dt * dt;
557}
558
559// Correct Time of Transmission
560////////////////////////////////////////////////////////////////////////////
561t_irc bncPPPclient::cmpToT(t_satData* satData) {
562
563 double prange = satData->P3;
564 if (prange == 0.0) {
565 return failure;
566 }
567
568 double clkSat = 0.0;
569 for (int ii = 1; ii <= 10; ii++) {
570
571 bncTime ToT = _epoData->tt - prange / t_CST::c - clkSat;
572
573 ColumnVector xc(4);
574 ColumnVector vv(3);
575 if (getSatPos(ToT, satData->prn, xc, vv) != success) {
576 return failure;
577 }
578
579 double clkSatOld = clkSat;
580 clkSat = xc(4);
581
582 if ( fabs(clkSat-clkSatOld) * t_CST::c < 1.e-4 ) {
583 satData->xx = xc.Rows(1,3);
584 satData->vv = vv;
585 satData->clk = clkSat * t_CST::c;
586 return success;
587 }
588 }
589
590 return failure;
591}
592
593//
594////////////////////////////////////////////////////////////////////////////
595void bncPPPclient::processEpoch() {
596
597 // Data Pre-Processing
598 // -------------------
599 QMutableMapIterator<QString, t_satData*> iGPS(_epoData->satDataGPS);
600 while (iGPS.hasNext()) {
601 iGPS.next();
602 QString prn = iGPS.key();
603 t_satData* satData = iGPS.value();
604
605 if (cmpToT(satData) != success) {
606 delete satData;
607 iGPS.remove();
608 continue;
609 }
610 }
611
612 QMutableMapIterator<QString, t_satData*> iGlo(_epoData->satDataGlo);
613 while (iGlo.hasNext()) {
614 iGlo.next();
615 QString prn = iGlo.key();
616 t_satData* satData = iGlo.value();
617
618 if (cmpToT(satData) != success) {
619 delete satData;
620 iGlo.remove();
621 continue;
622 }
623 }
624
625 QMutableMapIterator<QString, t_satData*> iGal(_epoData->satDataGal);
626 while (iGal.hasNext()) {
627 iGal.next();
628 QString prn = iGal.key();
629 t_satData* satData = iGal.value();
630
631 if (cmpToT(satData) != success) {
632 delete satData;
633 iGal.remove();
634 continue;
635 }
636 }
637
638 // Filter Solution
639 // ---------------
640 if (_model->update(_epoData) == success) {
641 emit newPosition(_model->time(), _model->x(), _model->y(), _model->z());
642 }
643}
Note: See TracBrowser for help on using the repository browser.