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

Last change on this file since 2809 was 2809, checked in by mervart, 13 years ago
File size: 18.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 "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 _model = new bncModel(staID);
102 connect(_model, SIGNAL(newNMEAstr(QByteArray)),
103 this, SIGNAL(newNMEAstr(QByteArray)));
104}
105
106// Destructor
107////////////////////////////////////////////////////////////////////////////
108bncPPPclient::~bncPPPclient() {
109 delete _model;
110 while (!_epoData.empty()) {
111 delete _epoData.front();
112 _epoData.pop();
113 }
114 QMapIterator<QString, t_ephPair*> it(_eph);
115 while (it.hasNext()) {
116 it.next();
117 delete it.value();
118 }
119 QMapIterator<QString, t_corr*> ic(_corr);
120 while (ic.hasNext()) {
121 ic.next();
122 delete ic.value();
123 }
124 QMapIterator<QString, t_bias*> ib(_bias);
125 while (ib.hasNext()) {
126 ib.next();
127 delete ib.value();
128 }
129}
130
131//
132////////////////////////////////////////////////////////////////////////////
133void bncPPPclient::putNewObs(const t_obs& obs) {
134 QMutexLocker locker(&_mutex);
135
136 if (obs.satSys == 'R') {
137 if (!_useGlonass) return;
138 }
139 else if (obs.satSys == 'E') {
140 if (!_useGalileo) return;
141 }
142 else if (obs.satSys != 'G') {
143 return;
144 }
145
146 t_satData* satData = new t_satData();
147 satData->tt = bncTime(obs.GPSWeek, obs.GPSWeeks);
148
149 // Satellite Number
150 // ----------------
151 satData->prn = QString("%1%2").arg(obs.satSys).arg(obs.satNum,2,10,QChar('0'));
152
153 // Check Slips
154 // -----------
155 slipInfo& sInfo = _slips[satData->prn];
156 if ( sInfo.slipCntL1 == obs.slip_cnt_L1 &&
157 sInfo.slipCntL2 == obs.slip_cnt_L2 &&
158 sInfo.slipCntL5 == obs.slip_cnt_L5 ) {
159 satData->slipFlag = false;
160 }
161 else {
162 satData->slipFlag = true;
163 }
164 sInfo.slipCntL1 = obs.slip_cnt_L1;
165 sInfo.slipCntL2 = obs.slip_cnt_L2;
166
167 // Handle Code Biases
168 // ------------------
169 t_bias* bb = 0;
170 if (_bias.contains(satData->prn)) {
171 bb = _bias.value(satData->prn);
172 }
173
174 // Add new epoch, process the older ones
175 // -------------------------------------
176 if (_epoData.size() == 0) {
177 _epoData.push(new t_epoData());
178 _epoData.back()->tt = satData->tt;
179 }
180 else if (satData->tt != _epoData.back()->tt) {
181 processEpochs();
182 _epoData.push(new t_epoData());
183 _epoData.back()->tt = satData->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.back()->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.back()->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.back()->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
384 if ( messageType == COTYPE_GPSCOMBINED ||
385 messageType == COTYPE_GLONASSCOMBINED ||
386 messageType == COTYPE_GPSORBIT ||
387 messageType == COTYPE_GPSCLOCK ||
388 messageType == COTYPE_GLONASSORBIT ||
389 messageType == COTYPE_GLONASSCLOCK ) {
390
391 t_corr* cc = 0;
392 if (_corr.contains(prn)) {
393 cc = _corr.value(prn);
394 }
395 else {
396 cc = new t_corr();
397 _corr[prn] = cc;
398 }
399
400 cc->tt.set(GPSweek, GPSweeks);
401 _corr_tt.set(GPSweek, GPSweeks);
402
403 if ( messageType == COTYPE_GPSCOMBINED ||
404 messageType == COTYPE_GLONASSCOMBINED ) {
405 cc->rao.ReSize(3); cc->rao = 0.0;
406 cc->dotRao.ReSize(3); cc->dotRao = 0.0;
407 cc->dotDotRao.ReSize(3); cc->dotDotRao = 0.0;
408 cc->dClk = 0.0;
409 cc->dotDClk = 0.0;
410 cc->dotDotDClk = 0.0;
411 in >> cc->iod
412 >> cc->dClk >> cc->rao[0] >> cc->rao[1] >> cc->rao[2]
413 >> cc->dotDClk >> cc->dotRao[0] >> cc->dotRao[1] >> cc->dotRao[2]
414 >> cc->dotDotDClk >> cc->dotDotRao[0] >> cc->dotDotRao[1] >> cc->dotDotRao[2];
415 cc->dClk /= t_CST::c;
416 cc->dotDClk /= t_CST::c;
417 cc->dotDotDClk /= t_CST::c;
418 cc->raoSet = true;
419 cc->dClkSet = true;
420 }
421 else if ( messageType == COTYPE_GPSORBIT ||
422 messageType == COTYPE_GLONASSORBIT ) {
423 cc->rao.ReSize(3); cc->rao = 0.0;
424 cc->dotRao.ReSize(3); cc->dotRao = 0.0;
425 cc->dotDotRao.ReSize(3); cc->dotDotRao = 0.0;
426 in >> cc->iod
427 >> cc->rao[0] >> cc->rao[1] >> cc->rao[2]
428 >> cc->dotRao[0] >> cc->dotRao[1] >> cc->dotRao[2]
429 >> cc->dotDotRao[0] >> cc->dotDotRao[1] >> cc->dotDotRao[2];
430 cc->raoSet = true;
431 }
432 else if ( messageType == COTYPE_GPSCLOCK ||
433 messageType == COTYPE_GLONASSCLOCK ) {
434 int dummyIOD;
435 cc->dClk = 0.0;
436 cc->dotDClk = 0.0;
437 cc->dotDotDClk = 0.0;
438 in >> dummyIOD >> cc->dClk >> cc->dotDClk >> cc->dotDotDClk;
439 cc->dClk /= t_CST::c;
440 cc->dotDClk /= t_CST::c;
441 cc->dotDotDClk /= t_CST::c;
442 cc->dClkSet = true;
443 }
444 }
445 else if ( messageType == BTYPE_GPS ) {
446
447 t_bias* bb = 0;
448 if (_bias.contains(prn)) {
449 bb = _bias.value(prn);
450 }
451 else {
452 bb = new t_bias();
453 _bias[prn] = bb;
454 }
455
456 bb->tt.set(GPSweek, GPSweeks);
457
458 int numBiases;
459 in >> numBiases;
460 for (int ii = 0; ii < numBiases; ++ii) {
461 int bType;
462 double bValue;
463 in >> bType >> bValue;
464 if (bType == CODETYPEGPS_L1_Z) {
465 bb->p1 = bValue;
466 }
467 else if (bType == CODETYPEGPS_L1_CA) {
468 bb->c1 = bValue;
469 }
470 else if (bType == CODETYPEGPS_L2_Z) {
471 bb->p2 = bValue;
472 }
473 }
474 }
475 }
476
477 QMutableMapIterator<QString, t_corr*> im(_corr);
478 while (im.hasNext()) {
479 im.next();
480 t_corr* cc = im.value();
481 if (!cc->ready()) {
482 delete cc;
483 im.remove();
484 }
485 }
486}
487
488// Satellite Position
489////////////////////////////////////////////////////////////////////////////
490t_irc bncPPPclient::getSatPos(const bncTime& tt, const QString& prn,
491 ColumnVector& xc, ColumnVector& vv) {
492
493 const double MAXAGE = 120.0;
494
495 if (_eph.contains(prn)) {
496
497 if (_pppMode) {
498 if (_corr.contains(prn)) {
499 t_corr* cc = _corr.value(prn);
500 if (tt - cc->tt < MAXAGE) {
501 t_eph* eLast = _eph.value(prn)->last;
502 t_eph* ePrev = _eph.value(prn)->prev;
503 if (eLast && eLast->IOD() == cc->iod) {
504 eLast->position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data());
505 applyCorr(tt, cc, xc, vv);
506 return success;
507 }
508 else if (ePrev && ePrev->IOD() == cc->iod) {
509 ePrev->position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data());
510 applyCorr(tt, cc, xc, vv);
511 return success;
512 }
513 }
514 }
515 }
516
517 else {
518 t_eph* ee = _eph.value(prn)->last;
519 ee->position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data());
520 return success;
521 }
522 }
523
524 return failure;
525}
526
527//
528////////////////////////////////////////////////////////////////////////////
529void bncPPPclient::applyCorr(const bncTime& tt, const t_corr* cc,
530 ColumnVector& xc, ColumnVector& vv) {
531 ColumnVector dx(3);
532
533 double dt = tt - cc->tt;
534 ColumnVector raoHlp = cc->rao + cc->dotRao * dt + cc->dotDotRao * dt * dt;
535
536 RSW_to_XYZ(xc.Rows(1,3), vv, raoHlp, dx);
537
538 xc[0] -= dx[0];
539 xc[1] -= dx[1];
540 xc[2] -= dx[2];
541 xc[3] += cc->dClk + cc->dotDClk * dt + cc->dotDotDClk * dt * dt;
542}
543
544// Correct Time of Transmission
545////////////////////////////////////////////////////////////////////////////
546t_irc bncPPPclient::cmpToT(t_satData* satData) {
547
548 double prange = satData->P3;
549 if (prange == 0.0) {
550 return failure;
551 }
552
553 double clkSat = 0.0;
554 for (int ii = 1; ii <= 10; ii++) {
555
556 bncTime ToT = satData->tt - prange / t_CST::c - clkSat;
557
558 ColumnVector xc(4);
559 ColumnVector vv(3);
560 if (getSatPos(ToT, satData->prn, xc, vv) != success) {
561 return failure;
562 }
563
564 double clkSatOld = clkSat;
565 clkSat = xc(4);
566
567 if ( fabs(clkSat-clkSatOld) * t_CST::c < 1.e-4 ) {
568 satData->xx = xc.Rows(1,3);
569 satData->vv = vv;
570 satData->clk = clkSat * t_CST::c;
571 return success;
572 }
573 }
574
575 return failure;
576}
577
578//
579////////////////////////////////////////////////////////////////////////////
580void bncPPPclient::processFrontEpoch() {
581
582 // Data Pre-Processing
583 // -------------------
584 QMutableMapIterator<QString, t_satData*> iGPS(_epoData.front()->satDataGPS);
585 while (iGPS.hasNext()) {
586 iGPS.next();
587 QString prn = iGPS.key();
588 t_satData* satData = iGPS.value();
589
590 if (cmpToT(satData) != success) {
591 delete satData;
592 iGPS.remove();
593 continue;
594 }
595 }
596
597 QMutableMapIterator<QString, t_satData*> iGlo(_epoData.front()->satDataGlo);
598 while (iGlo.hasNext()) {
599 iGlo.next();
600 QString prn = iGlo.key();
601 t_satData* satData = iGlo.value();
602
603 if (cmpToT(satData) != success) {
604 delete satData;
605 iGlo.remove();
606 continue;
607 }
608 }
609
610 QMutableMapIterator<QString, t_satData*> iGal(_epoData.front()->satDataGal);
611 while (iGal.hasNext()) {
612 iGal.next();
613 QString prn = iGal.key();
614 t_satData* satData = iGal.value();
615
616 if (cmpToT(satData) != success) {
617 delete satData;
618 iGal.remove();
619 continue;
620 }
621 }
622
623 // Filter Solution
624 // ---------------
625 if (_model->update(_epoData.front()) == success) {
626 emit newPosition(_model->time(), _model->x(), _model->y(), _model->z());
627 }
628}
629
630//
631////////////////////////////////////////////////////////////////////////////
632void bncPPPclient::processEpochs() {
633
634 double maxDt = 0.0;
635 if (!_pppMode) {
636 maxDt = 0.0;
637 }
638
639 while (!_epoData.empty()) {
640 t_epoData* frontEpoData = _epoData.front();
641
642 // No corrections yet, skip the epoch
643 // ----------------------------------
644 if (maxDt != 0.0 && !_corr_tt.valid()) {
645 delete _epoData.front();
646 _epoData.pop();
647 continue;
648 }
649
650 if (maxDt == 0 ||
651 (frontEpoData->tt - _corr_tt) < maxDt ) {
652 processFrontEpoch();
653 delete _epoData.front();
654 _epoData.pop();
655 }
656 else {
657 return;
658 }
659 }
660}
Note: See TracBrowser for help on using the repository browser.