source: ntrip/trunk/BNC/src/PPP/pppClient.cpp@ 7203

Last change on this file since 7203 was 7203, checked in by stuerze, 9 years ago

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