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

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