source: ntrip/trunk/BNC/src/PPP_SSR_I/pppClient.cpp@ 10599

Last change on this file since 10599 was 10599, checked in by stuerze, 21 hours ago

ADDED: consideration of NAV type in all applications
  • Property svn:keywords set to Author Date Id Rev URL;svn:eol-style=native
  • Property svn:mime-type set to text/plain
File size: 14.3 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: t_pppClient
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 "pppClient.h"
46#include "pppUtils.h"
47#include "bncephuser.h"
48#include "bncutils.h"
49
50using namespace BNC_PPP;
51using namespace std;
52
53// Constructor
54////////////////////////////////////////////////////////////////////////////
55t_pppClient::t_pppClient(const t_pppOptions* opt) {
56 _opt = new t_pppOptions(*opt);
57 _filter = new t_pppFilter(this);
58 _epoData = new t_epoData();
59 _log = new ostringstream();
60 _ephUser = new bncEphUser(false);
61 _pppUtils = new t_pppUtils();
62 _newEph = 0;
63}
64
65// Destructor
66////////////////////////////////////////////////////////////////////////////
67t_pppClient::~t_pppClient() {
68 delete _filter;
69 delete _epoData;
70 delete _opt;
71 delete _ephUser;
72 delete _log;
73 delete _pppUtils;
74 if (_newEph)
75 delete _newEph;
76}
77
78//
79////////////////////////////////////////////////////////////////////////////
80void t_pppClient::processEpoch(const vector<t_satObs*>& satObs, t_output* output) {
81
82 // Convert and store observations
83 // ------------------------------
84 _epoData->clear();
85 for (unsigned ii = 0; ii < satObs.size(); ii++) {
86 const t_satObs* obs = satObs[ii];
87 t_prn prn = obs->_prn;
88 t_satData* satData = new t_satData();
89
90 if (_epoData->tt.undef()) {
91 _epoData->tt = obs->_time;
92 }
93
94 satData->tt = obs->_time;
95 satData->prn = QString(prn.toInternalString().c_str());
96 satData->slipFlag = false;
97 satData->P1 = 0.0;
98 satData->P2 = 0.0;
99 satData->P5 = 0.0;
100 satData->P6 = 0.0;
101 satData->P7 = 0.0;
102 satData->L1 = 0.0;
103 satData->L2 = 0.0;
104 satData->L5 = 0.0;
105 satData->L6 = 0.0;
106 satData->L7 = 0.0;
107 for (unsigned ifrq = 0; ifrq < obs->_obs.size(); ifrq++) {
108 t_frqObs* frqObs = obs->_obs[ifrq];
109 double cb = 0.0;
110 const t_satCodeBias* satCB = _pppUtils->satCodeBias(prn);
111 if (satCB && satCB->_bias.size()) {
112 for (unsigned ii = 0; ii < satCB->_bias.size(); ii++) {
113
114 const t_frqCodeBias& bias = satCB->_bias[ii];
115 if (frqObs && frqObs->_rnxType2ch == bias._rnxType2ch) {
116 cb = bias._value;
117 }
118 // FIXME: use C/Q bias for X observations
119 // qDebug() << satData->prn << frqObs->_rnxType2ch.c_str();
120 }
121 }
122 if (frqObs->_rnxType2ch[0] == '1') {
123 if (frqObs->_codeValid) satData->P1 = frqObs->_code + cb;
124 if (frqObs->_phaseValid) satData->L1 = frqObs->_phase;
125 if (frqObs->_slip) satData->slipFlag = true;
126 }
127 else if (frqObs->_rnxType2ch[0] == '2') {
128 if (frqObs->_codeValid) satData->P2 = frqObs->_code + cb;
129 if (frqObs->_phaseValid) satData->L2 = frqObs->_phase;
130 if (frqObs->_slip) satData->slipFlag = true;
131 }
132 else if (frqObs->_rnxType2ch[0] == '5') {
133 if (frqObs->_codeValid) satData->P5 = frqObs->_code + cb;
134 if (frqObs->_phaseValid) satData->L5 = frqObs->_phase;
135 if (frqObs->_slip) satData->slipFlag = true;
136 }
137 else if (frqObs->_rnxType2ch[0] == '6') {
138 if (frqObs->_codeValid) satData->P6 = frqObs->_code + cb;
139 if (frqObs->_phaseValid) satData->L6 = frqObs->_phase;
140 if (frqObs->_slip) satData->slipFlag = true;
141 }
142 else if (frqObs->_rnxType2ch[0] == '7') {
143 if (frqObs->_codeValid) satData->P7 = frqObs->_code + cb;
144 if (frqObs->_phaseValid) satData->L7 = frqObs->_phase;
145 if (frqObs->_slip) satData->slipFlag = true;
146 }
147 }
148 putNewObs(satData);
149 }
150
151 // Data Pre-Processing
152 // -------------------
153 QMutableMapIterator<QString, t_satData*> it(_epoData->satData);
154 while (it.hasNext()) {
155 it.next();
156 QString prn = it.key();
157 t_satData* satData = it.value();
158
159 if (cmpToT(satData) != success) {
160 delete satData;
161 it.remove();
162 continue;
163 }
164
165 }
166
167 // Filter Solution
168 // ---------------
169 if (_filter->update(_epoData) == success) {
170 output->_error = false;
171 output->_epoTime = _filter->time();
172 output->_xyzRover[0] = _filter->x();
173 output->_xyzRover[1] = _filter->y();
174 output->_xyzRover[2] = _filter->z();
175 copy(&_filter->Q().data()[0], &_filter->Q().data()[6], output->_covMatrix);
176 output->_neu[0] = _filter->neu()[0];
177 output->_neu[1] = _filter->neu()[1];
178 output->_neu[2] = _filter->neu()[2];
179 output->_numSat = _filter->numSat();
180 output->_hDop = _filter->HDOP();
181 output->_trp0 = _filter->trp0();
182 output->_trp = _filter->trp();
183 output->_trpStdev = _filter->trpStdev();
184 }
185 else {
186 output->_error = true;
187 }
188
189 output->_log = _log->str();
190 delete _log; _log = new ostringstream();
191}
192
193//
194////////////////////////////////////////////////////////////////////////////
195void t_pppClient::putNewObs(t_satData* satData) {
196
197 // Set Observations GPS
198 // --------------------
199 if (satData->system() == 'G') {
200 if (satData->P1 != 0.0 && satData->P2 != 0.0 &&
201 satData->L1 != 0.0 && satData->L2 != 0.0 ) {
202 t_frequency::type fType1 = t_lc::toFreq(satData->system(), t_lc::l1);
203 t_frequency::type fType2 = t_lc::toFreq(satData->system(), t_lc::l2);
204 double f1 = t_CST::freq(fType1, 0);
205 double f2 = t_CST::freq(fType2, 0);
206 double a1 = f1 * f1 / (f1 * f1 - f2 * f2);
207 double a2 = - f2 * f2 / (f1 * f1 - f2 * f2);
208 satData->L1 = satData->L1 * t_CST::c / f1;
209 satData->L2 = satData->L2 * t_CST::c / f2;
210 satData->P3 = a1 * satData->P1 + a2 * satData->P2;
211 satData->L3 = a1 * satData->L1 + a2 * satData->L2;
212 satData->lambda3 = a1 * t_CST::c / f1 + a2 * t_CST::c / f2;
213 satData->lkA = a1;
214 satData->lkB = a2;
215 _epoData->satData[satData->prn] = satData;
216 }
217 else {
218 delete satData;
219 }
220 }
221
222 // Set Observations Glonass
223 // ------------------------
224 else if (satData->system() == 'R' && _opt->useSystem('R')) {
225 if (satData->P1 != 0.0 && satData->P2 != 0.0 &&
226 satData->L1 != 0.0 && satData->L2 != 0.0 ) {
227
228 int channel = 0;
229 if (satData->system() == 'R') {
230 const t_eph* eph = _ephUser->ephLast(satData->prn);
231 if (eph) {
232 channel = eph->slotNum();
233 }
234 else {
235 delete satData;
236 return;
237 }
238 }
239
240 t_frequency::type fType1 = t_lc::toFreq(satData->system(), t_lc::l1);
241 t_frequency::type fType2 = t_lc::toFreq(satData->system(), t_lc::l2);
242 double f1 = t_CST::freq(fType1, channel);
243 double f2 = t_CST::freq(fType2, channel);
244 double a1 = f1 * f1 / (f1 * f1 - f2 * f2);
245 double a2 = - f2 * f2 / (f1 * f1 - f2 * f2);
246 satData->L1 = satData->L1 * t_CST::c / f1;
247 satData->L2 = satData->L2 * t_CST::c / f2;
248 satData->P3 = a1 * satData->P1 + a2 * satData->P2;
249 satData->L3 = a1 * satData->L1 + a2 * satData->L2;
250 satData->lambda3 = a1 * t_CST::c / f1 + a2 * t_CST::c / f2;
251 satData->lkA = a1;
252 satData->lkB = a2;
253 _epoData->satData[satData->prn] = satData;
254 }
255 else {
256 delete satData;
257 }
258 }
259
260 // Set Observations Galileo
261 // ------------------------
262 else if (satData->system() == 'E' && _opt->useSystem('E')) {
263 if (satData->P1 != 0.0 && satData->P5 != 0.0 &&
264 satData->L1 != 0.0 && satData->L5 != 0.0 ) {
265 double f1 = t_CST::freq(t_frequency::E1, 0);
266 double f5 = t_CST::freq(t_frequency::E5, 0);
267 double a1 = f1 * f1 / (f1 * f1 - f5 * f5);
268 double a5 = - f5 * f5 / (f1 * f1 - f5 * f5);
269 satData->L1 = satData->L1 * t_CST::c / f1;
270 satData->L5 = satData->L5 * t_CST::c / f5;
271 satData->P3 = a1 * satData->P1 + a5 * satData->P5;
272 satData->L3 = a1 * satData->L1 + a5 * satData->L5;
273 satData->lambda3 = a1 * t_CST::c / f1 + a5 * t_CST::c / f5;
274 satData->lkA = a1;
275 satData->lkB = a5;
276 _epoData->satData[satData->prn] = satData;
277 }
278 else {
279 delete satData;
280 }
281 }
282
283 // Set Observations BDS
284 // ---------------------
285 else if (satData->system() == 'C' && _opt->useSystem('C')) {
286 if (satData->P2 != 0.0 && satData->P6 != 0.0 &&
287 satData->L2 != 0.0 && satData->L6 != 0.0 ) {
288 double f2 = t_CST::freq(t_frequency::C2, 0);
289 double f6 = t_CST::freq(t_frequency::C6, 0);
290 double a2 = f2 * f2 / (f2 * f2 - f6 * f6);
291 double a6 = - f6 * f6 / (f2 * f2 - f6 * f6);
292 satData->L2 = satData->L2 * t_CST::c / f2;
293 satData->L6 = satData->L6 * t_CST::c / f6;
294 satData->P3 = a2 * satData->P2 + a6 * satData->P6;
295 satData->L3 = a2 * satData->L2 + a6 * satData->L6;
296 satData->lambda3 = a2 * t_CST::c / f2 + a6 * t_CST::c / f6;
297 satData->lkA = a2;
298 satData->lkB = a6;
299 _epoData->satData[satData->prn] = satData;
300 }
301 else {
302 delete satData;
303 }
304 }
305 else {
306 delete satData;
307 }
308}
309
310//
311////////////////////////////////////////////////////////////////////////////
312void t_pppClient::putOrbCorrections(const std::vector<t_orbCorr*>& corr) {
313 for (unsigned ii = 0; ii < corr.size(); ii++) {
314 QString prn = QString(corr[ii]->_prn.toInternalString().c_str());
315 t_eph* eLast = _ephUser->ephLast(prn);
316 t_eph* ePrev = _ephUser->ephPrev(prn);
317 if (eLast && eLast->IOD() == corr[ii]->_iod) {
318 eLast->setOrbCorr(corr[ii]);
319 }
320 else if (ePrev && ePrev->IOD() == corr[ii]->_iod) {
321 ePrev->setOrbCorr(corr[ii]);
322 }
323 }
324}
325
326//
327////////////////////////////////////////////////////////////////////////////
328void t_pppClient::putClkCorrections(const std::vector<t_clkCorr*>& corr) {
329 for (unsigned ii = 0; ii < corr.size(); ii++) {
330 QString prn = QString(corr[ii]->_prn.toInternalString().c_str());
331 t_eph* eLast = _ephUser->ephLast(prn);
332 t_eph* ePrev = _ephUser->ephPrev(prn);
333 if (eLast && eLast->IOD() == corr[ii]->_iod) {
334 eLast->setClkCorr(corr[ii]);
335 }
336 else if (ePrev && ePrev->IOD() == corr[ii]->_iod) {
337 ePrev->setClkCorr(corr[ii]);
338 }
339 }
340}
341
342//
343//////////////////////////////////////////////////////////////////////////////
344void t_pppClient::putCodeBiases(const std::vector<t_satCodeBias*>& satCodeBias) {
345 for (unsigned ii = 0; ii < satCodeBias.size(); ii++) {
346 _pppUtils->putCodeBias(new t_satCodeBias(*satCodeBias[ii]));
347 }
348}
349
350//
351//////////////////////////////////////////////////////////////////////////////
352void t_pppClient::putPhaseBiases(const std::vector<t_satPhaseBias*>& /*satPhaseBias*/) {
353}
354
355//
356//////////////////////////////////////////////////////////////////////////////
357void t_pppClient::putTec(const t_vTec* /*vTec*/) {
358}
359
360//
361//////////////////////////////////////////////////////////////////////////////
362void t_pppClient::putEphemeris(const t_eph* eph) {
363 if (_newEph)
364 delete _newEph;
365 _newEph = 0;
366 const t_ephGPS* ephGPS = dynamic_cast<const t_ephGPS*>(eph);
367 const t_ephGlo* ephGlo = dynamic_cast<const t_ephGlo*>(eph);
368 const t_ephGal* ephGal = dynamic_cast<const t_ephGal*>(eph);
369 const t_ephBDS* ephBDS = dynamic_cast<const t_ephBDS*>(eph);
370 if (ephGPS) {
371 _newEph = new t_ephGPS(*ephGPS);
372 }
373 else if (ephGlo) {
374 _newEph = new t_ephGlo(*ephGlo);
375 }
376 else if (ephGal) {
377 _newEph = new t_ephGal(*ephGal);
378 }
379 else if (ephBDS) {
380 _newEph = new t_ephBDS(*ephBDS);
381 }
382
383 if (_newEph) {
384 _ephUser->putNewEph(_newEph, _opt->_realTime);
385 }
386}
387
388// Satellite Position
389////////////////////////////////////////////////////////////////////////////
390t_irc t_pppClient::getSatPos(const bncTime& tt, const QString& prn,
391 ColumnVector& xc, ColumnVector& vv) {
392
393 t_eph* eLast = _ephUser->ephLast(prn);
394 t_eph* ePrev = _ephUser->ephPrev(prn);
395 if (eLast && eLast->getCrd(tt, xc, vv, _opt->useOrbClkCorr()) == success) {
396 return success;
397 }
398 else if (ePrev && ePrev->getCrd(tt, xc, vv, _opt->useOrbClkCorr()) == success) {
399 return success;
400 }
401 return failure;
402}
403
404// Correct Time of Transmission
405////////////////////////////////////////////////////////////////////////////
406t_irc t_pppClient::cmpToT(t_satData* satData) {
407
408 double prange = satData->P3;
409 if (prange == 0.0) {
410 return failure;
411 }
412
413 double clkSat = 0.0;
414 for (int ii = 1; ii <= 10; ii++) {
415
416 bncTime ToT = satData->tt - prange / t_CST::c - clkSat;
417
418 ColumnVector xc(6);
419 ColumnVector vv(3);
420 if (getSatPos(ToT, satData->prn, xc, vv) != success) {
421 return failure;
422 }
423
424 double clkSatOld = clkSat;
425 clkSat = xc(4);
426
427 if ( fabs(clkSat-clkSatOld) * t_CST::c < 1.e-4 ) {
428 satData->xx = xc.Rows(1,3);
429 satData->vv = vv;
430 satData->clk = clkSat * t_CST::c;
431 return success;
432 }
433 }
434
435 return failure;
436}
437
438//
439////////////////////////////////////////////////////////////////////////////
440void t_pppClient::reset() {
441
442 // to delete all parameters
443 delete _filter;
444 _filter = new t_pppFilter(this);
445
446 // to delete old orbit and clock corrections
447 delete _ephUser;
448 _ephUser = new bncEphUser(false);
449
450 // to delete old code biases
451 delete _pppUtils;
452 _pppUtils = new t_pppUtils();
453
454}
Note: See TracBrowser for help on using the repository browser.