source: ntrip/trunk/BNC/src/PPP_free/bncpppclient.cpp@ 6083

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