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

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