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

Last change on this file since 9252 was 8542, checked in by stuerze, 7 years ago

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