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

Last change on this file since 5120 was 5070, checked in by mervart, 12 years ago
File size: 14.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 "bnccore.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 BNC_CORE, SLOT(slotMessage(const QByteArray,bool)));
75
76 connect(BNC_CORE, 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 and Glonass
160 // --------------------------------
161 if (obs.satSys == 'G' || obs.satSys == 'R') {
162 const QByteArray preferredTypes("WPC");
163 for (int ii = preferredTypes.length()-1; ii >= 0; ii--) {
164 for (int iPhase = 0; iPhase <= 1; iPhase++) {
165 for (int iFreq = 1; iFreq <= 2; iFreq++) {
166
167 char rnxStr[3];
168 double* p_value = 0;
169 if (iPhase == 0 && iFreq == 1) {
170 rnxStr[0] = 'C';
171 rnxStr[1] = '1';
172 p_value = &satData->P1;
173 }
174 else if (iPhase == 0 && iFreq == 2) {
175 rnxStr[0] = 'C';
176 rnxStr[1] = '2';
177 p_value = &satData->P2;
178 }
179 else if (iPhase == 1 && iFreq == 1) {
180 rnxStr[0] = 'L';
181 rnxStr[1] = '1';
182 p_value = &satData->L1;
183 }
184 else if (iPhase == 1 && iFreq == 2) {
185 rnxStr[0] = 'L';
186 rnxStr[1] = '2';
187 p_value = &satData->L2;
188 }
189
190 rnxStr[2] = preferredTypes[ii];
191
192 double measdata = obs.measdata(rnxStr, 3.0);
193 if (measdata != 0.0) {
194 *p_value = measdata;
195 if (rnxStr[0] == 'C' && bb) {
196 char biasStr[2];
197 biasStr[0] = rnxStr[1];
198 biasStr[1] = rnxStr[2];
199 *p_value += bb->value(biasStr);
200 }
201 }
202 }
203 }
204 }
205
206 if (satData->P1 != 0.0 && satData->P2 != 0.0 &&
207 satData->L1 != 0.0 && satData->L2 != 0.0 ) {
208 double f1 = t_CST::f1(obs.satSys, obs.slotNum);
209 double f2 = t_CST::f2(obs.satSys, obs.slotNum);
210 double a1 = f1 * f1 / (f1 * f1 - f2 * f2);
211 double a2 = - f2 * f2 / (f1 * f1 - f2 * f2);
212 satData->L1 = satData->L1 * t_CST::c / f1;
213 satData->L2 = satData->L2 * t_CST::c / f2;
214 satData->P3 = a1 * satData->P1 + a2 * satData->P2;
215 satData->L3 = a1 * satData->L1 + a2 * satData->L2;
216 satData->lambda3 = a1 * t_CST::c / f1 + a2 * t_CST::c / f2;
217 _epoData.back()->satData[satData->prn] = satData;
218 }
219 else {
220 delete satData;
221 }
222 }
223
224 // Set Observations Galileo
225 // ------------------------
226 else if (obs.satSys == 'E') {
227 satData->P1 = obs.measdata("C1", 3.0);
228 satData->L1 = obs.measdata("L1", 3.0);
229 satData->P5 = obs.measdata("C5", 3.0);
230 satData->L5 = obs.measdata("L5", 3.0);
231 if (satData->P1 != 0.0 && satData->P5 != 0.0 &&
232 satData->L1 != 0.0 && satData->L5 != 0.0 ) {
233 double f1 = t_CST::freq1;
234 double f5 = t_CST::freq5;
235 double a1 = f1 * f1 / (f1 * f1 - f5 * f5);
236 double a5 = - f5 * f5 / (f1 * f1 - f5 * f5);
237 satData->L1 = satData->L1 * t_CST::c / f1;
238 satData->L5 = satData->L5 * t_CST::c / f5;
239 satData->P3 = a1 * satData->P1 + a5 * satData->P5;
240 satData->L3 = a1 * satData->L1 + a5 * satData->L5;
241 satData->lambda3 = a1 * t_CST::c / f1 + a5 * t_CST::c / f5;
242 _epoData.back()->satData[satData->prn] = satData;
243 }
244 else {
245 delete satData;
246 }
247 }
248}
249
250//
251////////////////////////////////////////////////////////////////////////////
252void bncPPPclient::slotNewCorrections(QList<QString> corrList) {
253 QMutexLocker locker(&_mutex);
254
255 // Check the Mountpoint (source of corrections)
256 // --------------------------------------------
257 if (!_opt->pppCorrMount.isEmpty()) {
258 QMutableListIterator<QString> itm(corrList);
259 while (itm.hasNext()) {
260 QStringList hlp = itm.next().split(" ");
261 if (hlp.size() > 0) {
262 QString mountpoint = hlp[hlp.size()-1];
263 if (mountpoint != _opt->pppCorrMount) {
264 itm.remove();
265 }
266 }
267 }
268 }
269
270 if (corrList.size() == 0) {
271 return;
272 }
273
274 QListIterator<QString> it(corrList);
275 while (it.hasNext()) {
276 QString line = it.next();
277
278 QTextStream in(&line);
279 int messageType;
280 int updateInterval;
281 int GPSweek;
282 double GPSweeks;
283 QString prn;
284 in >> messageType >> updateInterval >> GPSweek >> GPSweeks >> prn;
285
286 if ( t_corr::relevantMessageType(messageType) ) {
287 t_corr* cc = 0;
288 if (_corr.contains(prn)) {
289 cc = _corr.value(prn);
290 }
291 else {
292 cc = new t_corr();
293 _corr[prn] = cc;
294 }
295 cc->readLine(line);
296 _corr_tt = cc->tClk;
297 }
298 else if ( messageType == BTYPE_GPS || messageType == BTYPE_GLONASS ) {
299 t_bias* bb = 0;
300 if (_bias.contains(prn)) {
301 bb = _bias.value(prn);
302 }
303 else {
304 bb = new t_bias();
305 _bias[prn] = bb;
306 }
307 bb->readLine(line);
308 }
309 }
310}
311
312// Satellite Position
313////////////////////////////////////////////////////////////////////////////
314t_irc bncPPPclient::getSatPos(const bncTime& tt, const QString& prn,
315 ColumnVector& xc, ColumnVector& vv) {
316
317 const double MAXAGE = 120.0;
318
319 if (_eph.contains(prn)) {
320
321 if (_opt->pppMode) {
322 if (_corr.contains(prn)) {
323 t_corr* cc = _corr.value(prn);
324 if (cc->ready() && tt - cc->tClk < MAXAGE) {
325 t_eph* eLast = _eph.value(prn)->last;
326 t_eph* ePrev = _eph.value(prn)->prev;
327 if (eLast && eLast->IOD() == cc->iod) {
328 eLast->position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data());
329 return applyCorr(tt, cc, xc, vv);
330 }
331 else if (ePrev && ePrev->IOD() == cc->iod) {
332 ePrev->position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data());
333 return applyCorr(tt, cc, xc, vv);
334 }
335 }
336 }
337 }
338
339 else {
340 t_eph* ee = _eph.value(prn)->last;
341 ee->position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data());
342 return success;
343 }
344 }
345
346 return failure;
347}
348
349//
350////////////////////////////////////////////////////////////////////////////
351t_irc bncPPPclient::applyCorr(const bncTime& tt, const t_corr* cc,
352 ColumnVector& xc, ColumnVector& vv) {
353
354 double dtRao = tt - cc->tRao;
355
356 // Position
357 // --------
358 ColumnVector raoHlp = cc->rao + cc->dotRao * dtRao
359 + 0.5 * cc->dotDotRao * dtRao * dtRao;
360
361 if (raoHlp.norm_Frobenius() > 20.0) {
362 return failure;
363 }
364
365 ColumnVector dx(3);
366 RSW_to_XYZ(xc.Rows(1,3), vv, raoHlp, dx);
367 xc[0] -= dx[0];
368 xc[1] -= dx[1];
369 xc[2] -= dx[2];
370
371 // Velocity
372 // --------
373 ColumnVector dotRaoHlp = cc->dotRao + cc->dotDotRao * dtRao;
374
375 ColumnVector dv(3);
376 RSW_to_XYZ(xc.Rows(1,3), vv, dotRaoHlp, dv);
377 vv[0] -= dv[0];
378 vv[1] -= dv[1];
379 vv[2] -= dv[2];
380
381 // Clocks
382 // ------
383 double dtClk = tt - cc->tClk;
384
385 xc[3] += cc->dClk + cc->dotDClk * dtClk + cc->dotDotDClk * dtClk * dtClk
386 + cc->hrClk;
387
388 return success;
389}
390
391// Correct Time of Transmission
392////////////////////////////////////////////////////////////////////////////
393t_irc bncPPPclient::cmpToT(t_satData* satData) {
394
395 double prange = satData->P3;
396 if (prange == 0.0) {
397 return failure;
398 }
399
400 double clkSat = 0.0;
401 for (int ii = 1; ii <= 10; ii++) {
402
403 bncTime ToT = satData->tt - prange / t_CST::c - clkSat;
404
405 ColumnVector xc(4);
406 ColumnVector vv(3);
407 if (getSatPos(ToT, satData->prn, xc, vv) != success) {
408 return failure;
409 }
410
411 double clkSatOld = clkSat;
412 clkSat = xc(4);
413
414 if ( fabs(clkSat-clkSatOld) * t_CST::c < 1.e-4 ) {
415 satData->xx = xc.Rows(1,3);
416 satData->vv = vv;
417 satData->clk = clkSat * t_CST::c;
418 return success;
419 }
420 }
421
422 return failure;
423}
424
425//
426////////////////////////////////////////////////////////////////////////////
427void bncPPPclient::processFrontEpoch() {
428
429#ifdef BNC_DEBUG
430 QString msg = "List of Corrections\n";
431 QMapIterator<QString, t_corr*> itC(_corr);
432 while (itC.hasNext()) {
433 itC.next();
434 QString src = itC.key();
435 const t_corr* corr = itC.value();
436 msg += QString("%1 %2 %3 %4\n")
437 .arg(corr->prn)
438 .arg(corr->iod)
439 .arg(QString(corr->tClk.datestr().c_str()) + "_" + QString(corr->tClk.timestr().c_str()))
440 .arg(QString(corr->tRao.datestr().c_str()) + "_" + QString(corr->tRao.timestr().c_str()));
441 }
442
443 msg += "List of Ephemeris\n";
444 QMapIterator<QString, t_ephPair*> itE(_eph);
445 while (itE.hasNext()) {
446 itE.next();
447 QString prn = itE.key();
448 const t_ephPair* ephPair = itE.value();
449 if (ephPair->prev) {
450 msg += QString("%1 %2 %3 %4 %5\n")
451 .arg(prn)
452 .arg(ephPair->last->IOD())
453 .arg(QString(ephPair->last->TOC().datestr().c_str()) + "_" +
454 QString(ephPair->last->TOC().timestr().c_str()))
455 .arg(ephPair->prev->IOD())
456 .arg(QString(ephPair->prev->TOC().datestr().c_str()) + "_" +
457 QString(ephPair->prev->TOC().timestr().c_str()));
458 }
459 else {
460 msg += QString("%1 %2 %3\n")
461 .arg(prn)
462 .arg(ephPair->last->IOD())
463 .arg(QString(ephPair->last->TOC().datestr().c_str()) + "_" +
464 QString(ephPair->last->TOC().timestr().c_str()));
465 }
466 }
467
468 emit newMessage(msg.toAscii(), false);
469#endif // BNC_DEBUG
470
471 // Data Pre-Processing
472 // -------------------
473 QMutableMapIterator<QString, t_satData*> it(_epoData.front()->satData);
474 while (it.hasNext()) {
475 it.next();
476 QString prn = it.key();
477 t_satData* satData = it.value();
478
479 if (cmpToT(satData) != success) {
480 delete satData;
481 it.remove();
482 continue;
483 }
484 }
485
486 // Filter Solution
487 // ---------------
488 if (_model->update(_epoData.front()) == success) {
489 emit newPosition(_model->time(), _model->x(), _model->y(), _model->z());
490 }
491}
492
493//
494////////////////////////////////////////////////////////////////////////////
495void bncPPPclient::processEpochs() {
496
497 // Make sure the buffer does not grow beyond any limit
498 // ---------------------------------------------------
499 const unsigned MAX_EPODATA_SIZE = 120;
500 if (_epoData.size() > MAX_EPODATA_SIZE) {
501 delete _epoData.front();
502 _epoData.pop();
503 }
504
505 // Loop over all unprocessed epochs
506 // --------------------------------
507 while (!_epoData.empty()) {
508
509 t_epoData* frontEpoData = _epoData.front();
510
511 // No corrections yet, skip the epoch
512 // ----------------------------------
513 if (_opt->corrSync != 0.0 && !_corr_tt.valid()) {
514 return;
515 }
516
517 // Process the front epoch
518 // -----------------------
519 if (_opt->corrSync == 0 || frontEpoData->tt - _corr_tt < _opt->corrSync) {
520 processFrontEpoch();
521 delete _epoData.front();
522 _epoData.pop();
523 }
524 else {
525 return;
526 }
527 }
528}
Note: See TracBrowser for help on using the repository browser.