source: ntrip/trunk/BNC/src/PPP_free/bncpppclient.cpp@ 6068

Last change on this file since 6068 was 6068, checked in by mervart, 10 years ago
File size: 14.2 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 "pppOptions.h"
51
52using namespace BNC_PPP;
53using namespace std;
54
55// Constructor
56////////////////////////////////////////////////////////////////////////////
57bncPPPclient::bncPPPclient(QByteArray staID, const t_pppOptions* opt) : bncEphUser(false) {
58
59 _opt = opt;
60 _staID = staID;
61 _model = new bncModel(this);
62}
63
64// Destructor
65////////////////////////////////////////////////////////////////////////////
66bncPPPclient::~bncPPPclient() {
67 while (!_epoData.empty()) {
68 delete _epoData.front();
69 _epoData.pop();
70 }
71 QMapIterator<QString, t_corr*> ic(_corr);
72 while (ic.hasNext()) {
73 ic.next();
74 delete ic.value();
75 }
76 QMapIterator<QString, t_bias*> ib(_bias);
77 while (ib.hasNext()) {
78 ib.next();
79 delete ib.value();
80 }
81 delete _model;
82}
83
84//
85////////////////////////////////////////////////////////////////////////////
86void bncPPPclient::putNewObs(const t_obs& obs, t_output* output) {
87 QMutexLocker locker(&_mutex);
88
89 if (obs.satSys == 'R') {
90 if (!_opt->useSystem('R')) return;
91 }
92 else if (obs.satSys == 'E') {
93 if (!_opt->useSystem('E')) return;
94 }
95 else if (obs.satSys != 'G') {
96 return;
97 }
98
99 t_satData* satData = new t_satData();
100 satData->tt = bncTime(obs.GPSWeek, obs.GPSWeeks);
101
102 // Satellite Number
103 // ----------------
104 satData->prn = QString("%1%2").arg(obs.satSys).arg(obs.satNum,2,10,QChar('0'));
105
106 // Check Slips
107 // -----------
108 slipInfo& sInfo = _slips[satData->prn];
109 if ( sInfo.slipCntL1 == obs.slip_cnt_L1 &&
110 sInfo.slipCntL2 == obs.slip_cnt_L2 &&
111 sInfo.slipCntL5 == obs.slip_cnt_L5 ) {
112 satData->slipFlag = false;
113 }
114 else {
115 satData->slipFlag = true;
116 }
117 sInfo.slipCntL1 = obs.slip_cnt_L1;
118 sInfo.slipCntL2 = obs.slip_cnt_L2;
119
120 // Handle Code Biases
121 // ------------------
122 t_bias* bb = 0;
123 if (_bias.contains(satData->prn)) {
124 bb = _bias.value(satData->prn);
125 }
126
127 // Add new epoch, process the older ones
128 // -------------------------------------
129 if (_epoData.size() == 0) {
130 _epoData.push(new t_epoData());
131 _epoData.back()->tt = satData->tt;
132 }
133 else if (satData->tt != _epoData.back()->tt) {
134 processEpochs(output);
135 _epoData.push(new t_epoData());
136 _epoData.back()->tt = satData->tt;
137 }
138
139 // Set Observations GPS and Glonass
140 // --------------------------------
141 if (obs.satSys == 'G' || obs.satSys == 'R') {
142 const QByteArray preferredTypes("WPC");
143 for (int ii = preferredTypes.length()-1; ii >= 0; ii--) {
144 for (int iPhase = 0; iPhase <= 1; iPhase++) {
145 for (int iFreq = 1; iFreq <= 2; iFreq++) {
146
147 char rnxStr[4]; rnxStr[3] = '\0';
148 double* p_value = 0;
149 if (iPhase == 0 && iFreq == 1) {
150 rnxStr[0] = 'C';
151 rnxStr[1] = '1';
152 p_value = &satData->P1;
153 }
154 else if (iPhase == 0 && iFreq == 2) {
155 rnxStr[0] = 'C';
156 rnxStr[1] = '2';
157 p_value = &satData->P2;
158 }
159 else if (iPhase == 1 && iFreq == 1) {
160 rnxStr[0] = 'L';
161 rnxStr[1] = '1';
162 p_value = &satData->L1;
163 }
164 else if (iPhase == 1 && iFreq == 2) {
165 rnxStr[0] = 'L';
166 rnxStr[1] = '2';
167 p_value = &satData->L2;
168 }
169
170 rnxStr[2] = preferredTypes[ii];
171
172 double measdata = obs.measdata(rnxStr, 3.0);
173 if (measdata != 0.0) {
174 *p_value = measdata;
175 if (rnxStr[0] == 'C' && bb) {
176 char biasStr[3];
177 biasStr[0] = rnxStr[1];
178 biasStr[1] = rnxStr[2];
179 biasStr[2] = '\0';
180 *p_value += bb->value(biasStr);
181 }
182 }
183 }
184 }
185 }
186
187 if (satData->P1 != 0.0 && satData->P2 != 0.0 &&
188 satData->L1 != 0.0 && satData->L2 != 0.0 ) {
189 t_frequency::type fType1 = t_lc::toFreq(obs.satSys,t_lc::l1);
190 t_frequency::type fType2 = t_lc::toFreq(obs.satSys,t_lc::l2);
191 double f1 = t_CST::freq(fType1, obs.slotNum);
192 double f2 = t_CST::freq(fType2, obs.slotNum);
193 double a1 = f1 * f1 / (f1 * f1 - f2 * f2);
194 double a2 = - f2 * f2 / (f1 * f1 - f2 * f2);
195 satData->L1 = satData->L1 * t_CST::c / f1;
196 satData->L2 = satData->L2 * t_CST::c / f2;
197 satData->P3 = a1 * satData->P1 + a2 * satData->P2;
198 satData->L3 = a1 * satData->L1 + a2 * satData->L2;
199 satData->lambda3 = a1 * t_CST::c / f1 + a2 * t_CST::c / f2;
200 _epoData.back()->satData[satData->prn] = satData;
201 }
202 else {
203 delete satData;
204 }
205 }
206
207 // Set Observations Galileo
208 // ------------------------
209 else if (obs.satSys == 'E') {
210 satData->P1 = obs.measdata("C1", 3.0);
211 satData->L1 = obs.measdata("L1", 3.0);
212 satData->P5 = obs.measdata("C5", 3.0);
213 satData->L5 = obs.measdata("L5", 3.0);
214 if (satData->P1 != 0.0 && satData->P5 != 0.0 &&
215 satData->L1 != 0.0 && satData->L5 != 0.0 ) {
216 double f1 = t_CST::freq(t_frequency::E1, 0);
217 double f5 = t_CST::freq(t_frequency::E5, 0);
218 double a1 = f1 * f1 / (f1 * f1 - f5 * f5);
219 double a5 = - f5 * f5 / (f1 * f1 - f5 * f5);
220 satData->L1 = satData->L1 * t_CST::c / f1;
221 satData->L5 = satData->L5 * t_CST::c / f5;
222 satData->P3 = a1 * satData->P1 + a5 * satData->P5;
223 satData->L3 = a1 * satData->L1 + a5 * satData->L5;
224 satData->lambda3 = a1 * t_CST::c / f1 + a5 * t_CST::c / f5;
225 _epoData.back()->satData[satData->prn] = satData;
226 }
227 else {
228 delete satData;
229 }
230 }
231}
232
233//
234////////////////////////////////////////////////////////////////////////////
235void bncPPPclient::putNewCorrections(QList<QString> corrList) {
236 QMutexLocker locker(&_mutex);
237
238 // Check the Mountpoint (source of corrections)
239 // --------------------------------------------
240 if (!_opt->_corrMount.empty()) {
241 QMutableListIterator<QString> itm(corrList);
242 while (itm.hasNext()) {
243 QStringList hlp = itm.next().split(" ");
244 if (hlp.size() > 0) {
245 QString mountpoint = hlp[hlp.size()-1];
246 if (mountpoint != QString(_opt->_corrMount.c_str())) {
247 itm.remove();
248 }
249 }
250 }
251 }
252
253 if (corrList.size() == 0) {
254 return;
255 }
256
257 QListIterator<QString> it(corrList);
258 while (it.hasNext()) {
259 QString line = it.next();
260
261 QTextStream in(&line);
262 int messageType;
263 int updateInterval;
264 int GPSweek;
265 double GPSweeks;
266 QString prn;
267 in >> messageType >> updateInterval >> GPSweek >> GPSweeks >> prn;
268
269 if ( t_corr::relevantMessageType(messageType) ) {
270 t_corr* cc = 0;
271 if (_corr.contains(prn)) {
272 cc = _corr.value(prn);
273 }
274 else {
275 cc = new t_corr();
276 _corr[prn] = cc;
277 }
278 cc->readLine(line);
279 _corr_tt = cc->tClk;
280 }
281 else if ( messageType == BTYPE_GPS || messageType == BTYPE_GLONASS ) {
282 t_bias* bb = 0;
283 if (_bias.contains(prn)) {
284 bb = _bias.value(prn);
285 }
286 else {
287 bb = new t_bias();
288 _bias[prn] = bb;
289 }
290 bb->readLine(line);
291 }
292 }
293}
294
295// Satellite Position
296////////////////////////////////////////////////////////////////////////////
297t_irc bncPPPclient::getSatPos(const bncTime& tt, const QString& prn,
298 ColumnVector& xc, ColumnVector& vv) {
299
300 const double MAXAGE = 120.0;
301
302 if (_eph.contains(prn)) {
303
304 if (_opt->useOrbClkCorr() && prn[0] != 'E') {
305 if (_corr.contains(prn)) {
306 t_corr* cc = _corr.value(prn);
307 if (cc->ready() && tt - cc->tClk < MAXAGE) {
308 t_eph* eLast = _eph.value(prn)->last;
309 t_eph* ePrev = _eph.value(prn)->prev;
310 if (eLast && eLast->IOD() == cc->iod) {
311 eLast->position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data());
312 return applyCorr(tt, cc, xc, vv);
313 }
314 else if (ePrev && ePrev->IOD() == cc->iod) {
315 ePrev->position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data());
316 return applyCorr(tt, cc, xc, vv);
317 }
318 }
319 }
320 }
321
322 else {
323 t_eph* ee = _eph.value(prn)->last;
324 ee->position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data());
325 return success;
326 }
327 }
328
329 return failure;
330}
331
332//
333////////////////////////////////////////////////////////////////////////////
334t_irc bncPPPclient::applyCorr(const bncTime& tt, const t_corr* cc,
335 ColumnVector& xc, ColumnVector& vv) {
336
337 double dtRao = tt - cc->tRao;
338
339 // Position
340 // --------
341 ColumnVector raoHlp = cc->rao + cc->dotRao * dtRao;
342
343 if (raoHlp.norm_Frobenius() > 20.0) {
344 return failure;
345 }
346
347 ColumnVector dx(3);
348 RSW_to_XYZ(xc.Rows(1,3), vv, raoHlp, dx);
349 xc[0] -= dx[0];
350 xc[1] -= dx[1];
351 xc[2] -= dx[2];
352
353 // Velocity
354 // --------
355 ColumnVector dotRaoHlp = cc->dotRao;
356
357 ColumnVector dv(3);
358 RSW_to_XYZ(xc.Rows(1,3), vv, dotRaoHlp, dv);
359 vv[0] -= dv[0];
360 vv[1] -= dv[1];
361 vv[2] -= dv[2];
362
363 // Clocks
364 // ------
365 double dtClk = tt - cc->tClk;
366
367 xc[3] += cc->dClk + cc->dotDClk * dtClk + cc->dotDotDClk * dtClk * dtClk
368 + cc->hrClk;
369
370 return success;
371}
372
373// Correct Time of Transmission
374////////////////////////////////////////////////////////////////////////////
375t_irc bncPPPclient::cmpToT(t_satData* satData) {
376
377 double prange = satData->P3;
378 if (prange == 0.0) {
379 return failure;
380 }
381
382 double clkSat = 0.0;
383 for (int ii = 1; ii <= 10; ii++) {
384
385 bncTime ToT = satData->tt - prange / t_CST::c - clkSat;
386
387 ColumnVector xc(4);
388 ColumnVector vv(3);
389 if (getSatPos(ToT, satData->prn, xc, vv) != success) {
390 return failure;
391 }
392
393 double clkSatOld = clkSat;
394 clkSat = xc(4);
395
396 if ( fabs(clkSat-clkSatOld) * t_CST::c < 1.e-4 ) {
397 satData->xx = xc.Rows(1,3);
398 satData->vv = vv;
399 satData->clk = clkSat * t_CST::c;
400 return success;
401 }
402 }
403
404 return failure;
405}
406
407//
408////////////////////////////////////////////////////////////////////////////
409void bncPPPclient::processFrontEpoch(t_output* output) {
410
411#ifdef BNC_DEBUG
412 QString msg = "List of Corrections\n";
413 QMapIterator<QString, t_corr*> itC(_corr);
414 while (itC.hasNext()) {
415 itC.next();
416 QString src = itC.key();
417 const t_corr* corr = itC.value();
418 msg += QString("%1 %2 %3 %4\n")
419 .arg(corr->prn)
420 .arg(corr->iod)
421 .arg(QString(corr->tClk.datestr().c_str()) + "_" + QString(corr->tClk.timestr().c_str()))
422 .arg(QString(corr->tRao.datestr().c_str()) + "_" + QString(corr->tRao.timestr().c_str()));
423 }
424
425 msg += "List of Ephemeris\n";
426 QMapIterator<QString, t_ephPair*> itE(_eph);
427 while (itE.hasNext()) {
428 itE.next();
429 QString prn = itE.key();
430 const t_ephPair* ephPair = itE.value();
431 if (ephPair->prev) {
432 msg += QString("%1 %2 %3 %4 %5\n")
433 .arg(prn)
434 .arg(ephPair->last->IOD())
435 .arg(QString(ephPair->last->TOC().datestr().c_str()) + "_" +
436 QString(ephPair->last->TOC().timestr().c_str()))
437 .arg(ephPair->prev->IOD())
438 .arg(QString(ephPair->prev->TOC().datestr().c_str()) + "_" +
439 QString(ephPair->prev->TOC().timestr().c_str()));
440 }
441 else {
442 msg += QString("%1 %2 %3\n")
443 .arg(prn)
444 .arg(ephPair->last->IOD())
445 .arg(QString(ephPair->last->TOC().datestr().c_str()) + "_" +
446 QString(ephPair->last->TOC().timestr().c_str()));
447 }
448 }
449
450 LOG << msg.toAscii() << endl;
451#endif // BNC_DEBUG
452
453 // Data Pre-Processing
454 // -------------------
455 QMutableMapIterator<QString, t_satData*> it(_epoData.front()->satData);
456 while (it.hasNext()) {
457 it.next();
458 QString prn = it.key();
459 t_satData* satData = it.value();
460
461 if (cmpToT(satData) != success) {
462 delete satData;
463 it.remove();
464 continue;
465 }
466 }
467
468 // Filter Solution
469 // ---------------
470 if (_model->update(_epoData.front()) == success) {
471 /// emit newPosition(_model->time(), _model->x(), _model->y(), _model->z());
472 }
473}
474
475//
476////////////////////////////////////////////////////////////////////////////
477void bncPPPclient::processEpochs(t_output* output) {
478
479 // Make sure the buffer does not grow beyond any limit
480 // ---------------------------------------------------
481 const unsigned MAX_EPODATA_SIZE = 120;
482 if (_epoData.size() > MAX_EPODATA_SIZE) {
483 delete _epoData.front();
484 _epoData.pop();
485 }
486
487 // Loop over all unprocessed epochs
488 // --------------------------------
489 while (!_epoData.empty()) {
490
491 t_epoData* frontEpoData = _epoData.front();
492
493 // No corrections yet, skip the epoch
494 // ----------------------------------
495 if (_opt->useOrbClkCorr() && !_corr_tt.valid()) {
496 return;
497 }
498
499 // Process the front epoch
500 // -----------------------
501 if (_opt->_corrWaitTime == 0.0 || frontEpoData->tt - _corr_tt >= _opt->_corrWaitTime) {
502 processFrontEpoch(output);
503 delete _epoData.front();
504 _epoData.pop();
505 }
506 else {
507 return;
508 }
509 }
510}
Note: See TracBrowser for help on using the repository browser.