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

Last change on this file since 2274 was 2274, checked in by mervart, 14 years ago

* empty log message *
File size: 12.8 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
44#include "bncpppclient.h"
45#include "bncapp.h"
46#include "bncutils.h"
47#include "bncconst.h"
48#include "bncmodel.h"
49#include "bncsettings.h"
50
51extern "C" {
52#include "clock_orbit_rtcm.h"
53}
54
55using namespace std;
56
57// Constructor
58////////////////////////////////////////////////////////////////////////////
59bncPPPclient::bncPPPclient(QByteArray staID) {
60
61 bncSettings settings;
62
63 if ( Qt::CheckState(settings.value("pppGLONASS").toInt()) == Qt::Checked) {
64 _useGlonass = true;
65 }
66 else {
67 _useGlonass = false;
68 }
69
70 connect(((bncApp*)qApp), SIGNAL(newEphGPS(gpsephemeris)),
71 this, SLOT(slotNewEphGPS(gpsephemeris)));
72 connect(((bncApp*)qApp), SIGNAL(newEphGlonass(glonassephemeris)),
73 this, SLOT(slotNewEphGlonass(glonassephemeris)));
74 connect(((bncApp*)qApp), SIGNAL(newCorrections(QList<QString>)),
75 this, SLOT(slotNewCorrections(QList<QString>)));
76
77 _staID = staID;
78 _epoData = 0;
79 _model = new bncModel(staID);
80 connect(_model, SIGNAL(newNMEAstr(QByteArray)),
81 this, SIGNAL(newNMEAstr(QByteArray)));
82}
83
84// Destructor
85////////////////////////////////////////////////////////////////////////////
86bncPPPclient::~bncPPPclient() {
87 delete _model;
88 delete _epoData;
89 QMapIterator<QString, t_eph*> it(_eph);
90 while (it.hasNext()) {
91 it.next();
92 delete it.value();
93 }
94 QMapIterator<QString, t_corr*> ic(_corr);
95 while (ic.hasNext()) {
96 ic.next();
97 delete ic.value();
98 }
99 QMapIterator<QString, t_bias*> ib(_bias);
100 while (ib.hasNext()) {
101 ib.next();
102 delete ib.value();
103 }
104}
105
106//
107////////////////////////////////////////////////////////////////////////////
108void bncPPPclient::putNewObs(p_obs pp) {
109 QMutexLocker locker(&_mutex);
110
111 t_obsInternal* obs = &(pp->_o);
112
113 if (obs->satSys != 'G' && !_useGlonass) {
114 return;
115 }
116
117 t_satData* satData = new t_satData();
118
119 // Satellite Number
120 // ----------------
121 if (obs->satSys == 'G') {
122 QString prn = QString("G%1").arg(obs->satNum, 2, 10, QChar('0'));
123 satData->prn = prn;
124 }
125 else if (obs->satSys == 'R') {
126 QString prn = QString("R%1").arg(obs->satNum, 2, 10, QChar('0'));
127 satData->prn = prn;
128 }
129
130 // Handle Code Biases
131 // ------------------
132 t_bias* bb = 0;
133 if (_bias.contains(satData->prn)) {
134 bb = _bias.value(satData->prn);
135 }
136
137 ////// beg test
138 //cout << satData->prn.toAscii().data();
139 //if (bb) {
140 // cout.setf(ios::fixed);
141 // cout << setprecision(3) << bb->c1 << " "
142 // << setprecision(3) << bb->p1 << " "
143 // << setprecision(3) << bb->c2 << " "
144 // << setprecision(3) << bb->p2 << endl;
145 //}
146 //else {
147 // cout << " no bias\n";
148 //}
149 //// end test
150
151 // Set Code Observations
152 // ---------------------
153 if (obs->P1) {
154 satData->P1 = obs->P1;
155 satData->codeTypeF1 = t_satData::P_CODE;
156 }
157 else if (obs->C1) {
158 satData->P1 = obs->C1;
159 satData->codeTypeF1 = t_satData::C_CODE;
160 }
161 else {
162 delete satData;
163 return;
164 }
165
166 if (obs->P2) {
167 satData->P2 = obs->P2;
168 satData->codeTypeF2 = t_satData::P_CODE;
169 }
170 else if (obs->C2) {
171 satData->P2 = obs->C2;
172 satData->codeTypeF2 = t_satData::C_CODE;
173 }
174 else {
175 delete satData;
176 return;
177 }
178
179 double f1 = t_CST::freq1;
180 double f2 = t_CST::freq2;
181
182 if (obs->satSys == 'R') {
183 f1 = 1602000000.0 + 562500.0 * obs->slot;
184 f2 = 1246000000.0 + 437500.0 * obs->slot;
185 }
186
187 // Ionosphere-Free Combination
188 // ---------------------------
189 double c1 = f1 * f1 / (f1 * f1 - f2 * f2);
190 double c2 = - f2 * f2 / (f1 * f1 - f2 * f2);
191
192 satData->P3 = c1 * satData->P1 + c2 * satData->P2;
193
194 // Set Phase Observations
195 // ----------------------
196 if (obs->L1 && obs->L2) {
197 satData->L1 = obs->L1 * t_CST::c / f1;
198 satData->L2 = obs->L2 * t_CST::c / f2;
199 }
200 else {
201 delete satData;
202 return;
203 }
204 satData->L3 = c1 * satData->L1 + c2 * satData->L2;
205
206 // Add new Satellite to the epoch
207 // ------------------------------
208 bncTime tt(obs->GPSWeek, obs->GPSWeeks);
209
210 if (!_epoData) {
211 _epoData = new t_epoData();
212 _epoData->tt = tt;
213 }
214 else if (tt != _epoData->tt) {
215 processEpoch();
216 delete _epoData;
217 _epoData = new t_epoData();
218 _epoData->tt = tt;
219 }
220
221 if (obs->satSys == 'G') {
222 _epoData->satDataGPS[satData->prn] = satData;
223 }
224 else if (obs->satSys == 'R') {
225 _epoData->satDataGlo[satData->prn] = satData;
226 }
227
228}
229
230//
231////////////////////////////////////////////////////////////////////////////
232void bncPPPclient::slotNewEphGPS(gpsephemeris gpseph) {
233 QMutexLocker locker(&_mutex);
234
235 QString prn = QString("G%1").arg(gpseph.satellite, 2, 10, QChar('0'));
236
237 if (_eph.contains(prn)) {
238 t_ephGPS* ee = static_cast<t_ephGPS*>(_eph.value(prn));
239 if ( (ee->GPSweek() < gpseph.GPSweek) ||
240 (ee->GPSweek() == gpseph.GPSweek &&
241 ee->TOC() < gpseph.TOC) ) {
242 ee->set(&gpseph);
243 }
244 }
245 else {
246 t_ephGPS* ee = new t_ephGPS();
247 ee->set(&gpseph);
248 _eph[prn] = ee;
249 }
250}
251
252//
253////////////////////////////////////////////////////////////////////////////
254void bncPPPclient::slotNewEphGlonass(glonassephemeris gloeph) {
255 QMutexLocker locker(&_mutex);
256
257 QString prn = QString("R%1").arg(gloeph.almanac_number, 2, 10, QChar('0'));
258
259 if (_eph.contains(prn)) {
260 int ww = gloeph.GPSWeek;
261 int tow = gloeph.GPSTOW;
262 updatetime(&ww, &tow, gloeph.tb*1000, 0); // Moscow -> GPS
263 t_ephGlo* ee = static_cast<t_ephGlo*>(_eph.value(prn));
264 if (ee->GPSweek() < ww ||
265 (ee->GPSweek() == ww && ee->GPSweeks() < tow)) {
266 ee->set(&gloeph);
267 }
268 }
269 else {
270 t_ephGlo* ee = new t_ephGlo();
271 ee->set(&gloeph);
272 _eph[prn] = ee;
273 }
274}
275
276//
277////////////////////////////////////////////////////////////////////////////
278void bncPPPclient::slotNewCorrections(QList<QString> corrList) {
279 QMutexLocker locker(&_mutex);
280
281 if (corrList.size() == 0) {
282 return;
283 }
284
285 // Remove All Corrections
286 // ----------------------
287 QMapIterator<QString, t_corr*> ic(_corr);
288 while (ic.hasNext()) {
289 ic.next();
290 delete ic.value();
291 }
292 _corr.clear();
293
294 QListIterator<QString> it(corrList);
295 while (it.hasNext()) {
296 QTextStream in(it.next().toAscii());
297 int messageType;
298 int updateInterval;
299 int GPSweek;
300 double GPSweeks;
301 QString prn;
302 in >> messageType >> updateInterval >> GPSweek >> GPSweeks >> prn;
303 if ( messageType == COTYPE_GPSCOMBINED ||
304 messageType == COTYPE_GLONASSCOMBINED ||
305 messageType == COTYPE_GPSORBIT ||
306 messageType == COTYPE_GPSCLOCK ||
307 messageType == COTYPE_GLONASSORBIT ||
308 messageType == COTYPE_GLONASSCLOCK ) {
309
310 t_corr* cc = 0;
311 if (_corr.contains(prn)) {
312 cc = _corr.value(prn);
313 }
314 else {
315 cc = new t_corr();
316 _corr[prn] = cc;
317 }
318
319 cc->tt.set(GPSweek, GPSweeks);
320
321 if ( messageType == COTYPE_GPSCOMBINED ||
322 messageType == COTYPE_GLONASSCOMBINED ) {
323 cc->rao.ReSize(3);
324 in >> cc->iod >> cc->dClk >> cc->rao[0] >> cc->rao[1] >> cc->rao[2];
325 cc->dClk /= t_CST::c;
326 cc->raoSet = true;
327 cc->dClkSet = true;
328 }
329 else if ( messageType == COTYPE_GPSORBIT ||
330 messageType == COTYPE_GLONASSORBIT ) {
331 cc->rao.ReSize(3);
332 in >> cc->iod >> cc->rao[0] >> cc->rao[1] >> cc->rao[2];
333 cc->raoSet = true;
334 }
335 else if ( messageType == COTYPE_GPSCLOCK ||
336 messageType == COTYPE_GLONASSCLOCK ) {
337 int dummyIOD;
338 in >> dummyIOD >> cc->dClk;
339 cc->dClk /= t_CST::c;
340 cc->dClkSet = true;
341 }
342 }
343 else if ( messageType == BTYPE_GPS ||
344 messageType == BTYPE_GLONASS ) {
345
346 t_bias* bb = 0;
347 if (_bias.contains(prn)) {
348 bb = _bias.value(prn);
349 }
350 else {
351 bb = new t_bias();
352 _bias[prn] = bb;
353 }
354
355 bb->tt.set(GPSweek, GPSweeks);
356
357 int numBiases;
358 in >> numBiases;
359 for (int ii = 0; ii < numBiases; ++ii) {
360 int bType;
361 double bValue;
362 in >> bType >> bValue;
363 if (bType == 0) {
364 bb->c1 = bValue;
365 }
366 else if (bType >= 1 && bType <= 4) {
367 bb->p1 = bValue;
368 }
369 else if (bType == 5) {
370 bb->c2 = bValue;
371 }
372 else if (bType >= 6 && bType <= 13) {
373 bb->p2 = bValue;
374 }
375 }
376 }
377 }
378
379 QMutableMapIterator<QString, t_corr*> im(_corr);
380 while (im.hasNext()) {
381 im.next();
382 t_corr* cc = im.value();
383 if (!cc->ready()) {
384 delete cc;
385 im.remove();
386 }
387 }
388}
389
390// Satellite Position
391////////////////////////////////////////////////////////////////////////////
392t_irc bncPPPclient::getSatPos(const bncTime& tt, const QString& prn,
393 ColumnVector& xc, ColumnVector& vv, bool& corr) {
394
395 const bool CORR_REQUIRED = true;
396 const double MAXAGE = 120.0;
397
398 corr = false;
399
400 if (_eph.contains(prn)) {
401 t_eph* ee = _eph.value(prn);
402 ee->position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data());
403
404 if (CORR_REQUIRED) {
405 if (_corr.contains(prn)) {
406 t_corr* cc = _corr.value(prn);
407 if (ee->IOD() == cc->iod && (tt - cc->tt) < MAXAGE) {
408 corr = true;
409 applyCorr(cc, xc, vv);
410 return success;
411 }
412 }
413 return failure;
414 }
415
416 return success;
417 }
418
419 return failure;
420}
421
422//
423////////////////////////////////////////////////////////////////////////////
424void bncPPPclient::applyCorr(const t_corr* cc, ColumnVector& xc,
425 ColumnVector& vv) {
426 ColumnVector dx(3);
427 RSW_to_XYZ(xc.Rows(1,3), vv, cc->rao, dx);
428
429 xc[0] -= dx[0];
430 xc[1] -= dx[1];
431 xc[2] -= dx[2];
432 xc[3] -= cc->dClk;
433
434 // Relativistic Correction
435 // -----------------------
436 xc[3] -= 2.0 * DotProduct(xc.Rows(1,3),vv) / t_CST::c / t_CST::c ;
437}
438
439// Correct Time of Transmission
440////////////////////////////////////////////////////////////////////////////
441t_irc bncPPPclient::cmpToT(t_satData* satData) {
442
443 double prange = satData->P3;
444 if (prange == 0.0) {
445 return failure;
446 }
447
448 double clkSat = 0.0;
449 for (int ii = 1; ii <= 10; ii++) {
450
451 bncTime ToT = _epoData->tt - prange / t_CST::c - clkSat;
452
453 ColumnVector xc(4);
454 ColumnVector vv(3);
455 bool corr = false;
456 if (getSatPos(ToT, satData->prn, xc, vv, corr) != success) {
457 return failure;
458 }
459
460 double clkSatOld = clkSat;
461 clkSat = xc(4);
462
463 if ( fabs(clkSat-clkSatOld) * t_CST::c < 1.e-4 ) {
464 satData->xx = xc.Rows(1,3);
465 satData->vv = vv;
466 satData->clk = clkSat * t_CST::c;
467 satData->clkCorr = corr;
468 return success;
469 }
470 }
471
472 return failure;
473}
474
475//
476////////////////////////////////////////////////////////////////////////////
477void bncPPPclient::processEpoch() {
478
479 // Data Pre-Processing
480 // -------------------
481 QMutableMapIterator<QString, t_satData*> iGPS(_epoData->satDataGPS);
482 while (iGPS.hasNext()) {
483 iGPS.next();
484 QString prn = iGPS.key();
485 t_satData* satData = iGPS.value();
486
487 if (cmpToT(satData) != success) {
488 delete satData;
489 iGPS.remove();
490 continue;
491 }
492 }
493
494 QMutableMapIterator<QString, t_satData*> iGlo(_epoData->satDataGlo);
495 while (iGlo.hasNext()) {
496 iGlo.next();
497 QString prn = iGlo.key();
498 t_satData* satData = iGlo.value();
499
500 if (cmpToT(satData) != success) {
501 delete satData;
502 iGlo.remove();
503 continue;
504 }
505 }
506
507 // Filter Solution
508 // ---------------
509 if (_model->update(_epoData) == success) {
510 emit newPosition(_model->time(), _model->x(), _model->y(), _model->z());
511 }
512}
513
Note: See TracBrowser for help on using the repository browser.