source: ntrip/trunk/BNC/src/PPP_free/pppClient.cpp@ 6099

Last change on this file since 6099 was 6099, checked in by mervart, 10 years ago
File size: 10.5 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 "pppFilter.h"
47#include "bncephuser.h"
48#include "bncutils.h"
49
50using namespace BNC_PPP;
51using namespace std;
52
53// Global variable holding thread-specific pointers
54//////////////////////////////////////////////////////////////////////////////
55QThreadStorage<t_pppClient*> CLIENTS;
56
57// Static function returning thread-specific pointer
58//////////////////////////////////////////////////////////////////////////////
59t_pppClient* t_pppClient::instance() {
60 return CLIENTS.localData();
61}
62
63// Constructor
64////////////////////////////////////////////////////////////////////////////
65t_pppClient::t_pppClient(const t_pppOptions* opt) {
66
67 _opt = new t_pppOptions(*opt);
68 _filter = new t_pppFilter(this);
69 _epoData = new t_epoData();
70 _log = new ostringstream();
71 _ephUser = new bncEphUser(false);
72 _staID = QByteArray(_opt->_roverName.c_str());
73
74 CLIENTS.setLocalData(this); // CLIENTS takes ownership over "this"
75}
76
77// Destructor
78////////////////////////////////////////////////////////////////////////////
79t_pppClient::~t_pppClient() {
80 delete _filter;
81 delete _epoData;
82 delete _opt;
83 delete _ephUser;
84 delete _log;
85}
86
87//
88////////////////////////////////////////////////////////////////////////////
89void t_pppClient::processEpoch(const vector<t_satObs*>& satObs, t_output* output) {
90
91 // Convert and store observations
92 // ------------------------------
93 _epoData->clear();
94 for (unsigned ii = 0; ii < satObs.size(); ii++) {
95 const t_satObs* obs = satObs[ii];
96 t_satData* satData = new t_satData();
97
98 if (_epoData->tt.undef()) {
99 _epoData->tt = obs->_time;
100 }
101
102 satData->tt = obs->_time;
103 satData->prn = QString(obs->_prn.toString().c_str());
104 satData->slipFlag = false;
105 satData->P1 = 0.0;
106 satData->P2 = 0.0;
107 satData->P5 = 0.0;
108 satData->L1 = 0.0;
109 satData->L2 = 0.0;
110 satData->L5 = 0.0;
111 for (unsigned ifrq = 0; ifrq < obs->_obs.size(); ifrq++) {
112 t_frqObs* frqObs = obs->_obs[ifrq];
113 if (frqObs->_rnxType2ch[0] == '1') {
114 if (frqObs->_codeValid) satData->P1 = frqObs->_code;
115 if (frqObs->_phaseValid) satData->L1 = frqObs->_phase;
116 if (frqObs->_slip) satData->slipFlag = true;
117 }
118 else if (frqObs->_rnxType2ch[0] == '2') {
119 if (frqObs->_codeValid) satData->P2 = frqObs->_code;
120 if (frqObs->_phaseValid) satData->L2 = frqObs->_phase;
121 if (frqObs->_slip) satData->slipFlag = true;
122 }
123 else if (frqObs->_rnxType2ch[0] == '5') {
124 if (frqObs->_codeValid) satData->P5 = frqObs->_code;
125 if (frqObs->_phaseValid) satData->L5 = frqObs->_phase;
126 if (frqObs->_slip) satData->slipFlag = true;
127 }
128 }
129 putNewObs(satData);
130 }
131
132 // Data Pre-Processing
133 // -------------------
134 QMutableMapIterator<QString, t_satData*> it(_epoData->satData);
135 while (it.hasNext()) {
136 it.next();
137 QString prn = it.key();
138 t_satData* satData = it.value();
139
140 if (cmpToT(satData) != success) {
141 delete satData;
142 it.remove();
143 continue;
144 }
145 }
146
147 // Filter Solution
148 // ---------------
149 if (_filter->update(_epoData) == success) {
150 output->_error = false;
151 output->_epoTime = _filter->time();
152 output->_xyzRover[0] = _filter->x();
153 output->_xyzRover[1] = _filter->y();
154 output->_xyzRover[2] = _filter->z();
155 output->_numSat = 0;
156 output->_pDop = 0.0;
157 }
158 else {
159 output->_error = true;
160 }
161
162 output->_log = _log->str();
163 delete _log; _log = new ostringstream();
164}
165
166//
167////////////////////////////////////////////////////////////////////////////
168void t_pppClient::putNewObs(t_satData* satData) {
169
170 // Set Observations GPS and Glonass
171 // --------------------------------
172 if (satData->system() == 'G' || satData->system() == 'R') {
173 if (satData->P1 != 0.0 && satData->P2 != 0.0 &&
174 satData->L1 != 0.0 && satData->L2 != 0.0 ) {
175
176 int channel = 0;
177 if (satData->system() == 'R') {
178 const bncEphUser::t_ephPair* ephPair = _ephUser->ephPair(satData->prn);
179 if (ephPair) {
180 channel = ephPair->last->slotNum();
181 }
182 else {
183 delete satData;
184 return;
185 }
186 }
187
188 t_frequency::type fType1 = t_lc::toFreq(satData->system(), t_lc::l1);
189 t_frequency::type fType2 = t_lc::toFreq(satData->system(), t_lc::l2);
190 double f1 = t_CST::freq(fType1, channel);
191 double f2 = t_CST::freq(fType2, channel);
192 double a1 = f1 * f1 / (f1 * f1 - f2 * f2);
193 double a2 = - f2 * f2 / (f1 * f1 - f2 * f2);
194 satData->L1 = satData->L1 * t_CST::c / f1;
195 satData->L2 = satData->L2 * t_CST::c / f2;
196 satData->P3 = a1 * satData->P1 + a2 * satData->P2;
197 satData->L3 = a1 * satData->L1 + a2 * satData->L2;
198 satData->lambda3 = a1 * t_CST::c / f1 + a2 * t_CST::c / f2;
199 _epoData->satData[satData->prn] = satData;
200 }
201 else {
202 delete satData;
203 }
204 }
205
206 // Set Observations Galileo
207 // ------------------------
208 else if (satData->system() == 'E') {
209 if (satData->P1 != 0.0 && satData->P5 != 0.0 &&
210 satData->L1 != 0.0 && satData->L5 != 0.0 ) {
211 double f1 = t_CST::freq(t_frequency::E1, 0);
212 double f5 = t_CST::freq(t_frequency::E5, 0);
213 double a1 = f1 * f1 / (f1 * f1 - f5 * f5);
214 double a5 = - f5 * f5 / (f1 * f1 - f5 * f5);
215 satData->L1 = satData->L1 * t_CST::c / f1;
216 satData->L5 = satData->L5 * t_CST::c / f5;
217 satData->P3 = a1 * satData->P1 + a5 * satData->P5;
218 satData->L3 = a1 * satData->L1 + a5 * satData->L5;
219 satData->lambda3 = a1 * t_CST::c / f1 + a5 * t_CST::c / f5;
220 _epoData->satData[satData->prn] = satData;
221 }
222 else {
223 delete satData;
224 }
225 }
226}
227
228//
229////////////////////////////////////////////////////////////////////////////
230void t_pppClient::putOrbCorrections(const std::vector<t_orbCorr*>& corr) {
231 for (unsigned ii = 0; ii < corr.size(); ii++) {
232 QString prn = QString(corr[ii]->_prn.toString().c_str());
233 const bncEphUser::t_ephPair* ephPair = _ephUser->ephPair(prn);
234 if (ephPair) {
235 t_eph* eLast = ephPair->last;
236 t_eph* ePrev = ephPair->prev;
237 if (eLast && eLast->IOD() == corr[ii]->_iod) {
238 eLast->setOrbCorr(corr[ii]);
239 }
240 else if (ePrev && ePrev->IOD() == corr[ii]->_iod) {
241 ePrev->setOrbCorr(corr[ii]);
242 }
243 }
244 }
245}
246
247//
248////////////////////////////////////////////////////////////////////////////
249void t_pppClient::putClkCorrections(const std::vector<t_clkCorr*>& corr) {
250 for (unsigned ii = 0; ii < corr.size(); ii++) {
251 QString prn = QString(corr[ii]->_prn.toString().c_str());
252 const bncEphUser::t_ephPair* ephPair = _ephUser->ephPair(prn);
253 if (ephPair) {
254 t_eph* eLast = ephPair->last;
255 t_eph* ePrev = ephPair->prev;
256 if (eLast && eLast->IOD() == corr[ii]->_iod) {
257 eLast->setClkCorr(corr[ii]);
258 }
259 else if (ePrev && ePrev->IOD() == corr[ii]->_iod) {
260 ePrev->setClkCorr(corr[ii]);
261 }
262 }
263 }
264}
265
266//
267//////////////////////////////////////////////////////////////////////////////
268void t_pppClient::putBiases(const std::vector<t_satBias*>& /* satBias */) {
269}
270
271//
272//////////////////////////////////////////////////////////////////////////////
273void t_pppClient::putEphemeris(const t_eph* eph) {
274 const t_ephGPS* ephGPS = dynamic_cast<const t_ephGPS*>(eph);
275 const t_ephGlo* ephGlo = dynamic_cast<const t_ephGlo*>(eph);
276 const t_ephGal* ephGal = dynamic_cast<const t_ephGal*>(eph);
277 if (ephGPS) {
278 _ephUser->putNewEph(new t_ephGPS(*ephGPS));
279 }
280 else if (ephGlo) {
281 _ephUser->putNewEph(new t_ephGlo(*ephGlo));
282 }
283 else if (ephGal) {
284 _ephUser->putNewEph(new t_ephGal(*ephGal));
285 }
286}
287
288// Satellite Position
289////////////////////////////////////////////////////////////////////////////
290t_irc t_pppClient::getSatPos(const bncTime& tt, const QString& prn,
291 ColumnVector& xc, ColumnVector& vv) {
292
293 const bncEphUser::t_ephPair* ephPair = _ephUser->ephPair(prn);
294 if (ephPair) {
295 t_eph* eLast = ephPair->last;
296 t_eph* ePrev = ephPair->prev;
297 if (eLast && eLast->getCrd(tt, xc, vv, _opt->useOrbClkCorr()) == success) {
298 return success;
299 }
300 else if (ePrev && ePrev->getCrd(tt, xc, vv, _opt->useOrbClkCorr()) == success) {
301 return success;
302 }
303 }
304 return failure;
305}
306
307// Correct Time of Transmission
308////////////////////////////////////////////////////////////////////////////
309t_irc t_pppClient::cmpToT(t_satData* satData) {
310
311 double prange = satData->P3;
312 if (prange == 0.0) {
313 return failure;
314 }
315
316 double clkSat = 0.0;
317 for (int ii = 1; ii <= 10; ii++) {
318
319 bncTime ToT = satData->tt - prange / t_CST::c - clkSat;
320
321 ColumnVector xc(4);
322 ColumnVector vv(3);
323 if (getSatPos(ToT, satData->prn, xc, vv) != success) {
324 return failure;
325 }
326
327 double clkSatOld = clkSat;
328 clkSat = xc(4);
329
330 if ( fabs(clkSat-clkSatOld) * t_CST::c < 1.e-4 ) {
331 satData->xx = xc.Rows(1,3);
332 satData->vv = vv;
333 satData->clk = clkSat * t_CST::c;
334 return success;
335 }
336 }
337
338 return failure;
339}
340
Note: See TracBrowser for help on using the repository browser.