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

Last change on this file since 6075 was 6075, checked in by mervart, 8 years ago
File size: 10.7 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// Constructor
57////////////////////////////////////////////////////////////////////////////
58bncPPPclient::bncPPPclient(QByteArray staID, const t_pppOptions* opt) : bncEphUser(false) {
59
60 _opt = opt;
61 _staID = staID;
62 _model = new bncModel(this);
63 _epoData = new t_epoData();
64}
65
66// Destructor
67////////////////////////////////////////////////////////////////////////////
68bncPPPclient::~bncPPPclient() {
69 _epoData->clear();
70
71 QMapIterator<QString, t_corr*> ic(_corr);
72 while (ic.hasNext()) {
73 ic.next();
74 delete ic.value();
75 }
76
77 QMapIterator<QString, t_bias*> ib(_bias);
78 while (ib.hasNext()) {
79 ib.next();
80 delete ib.value();
81 }
82
83 delete _model;
84}
85
86//
87////////////////////////////////////////////////////////////////////////////
88void bncPPPclient::processEpoch(const vector<t_satObs*>& satObs, t_output* output) {
89 QMutexLocker locker(&_mutex);
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 satData->tt = obs->_time;
98 satData->prn = QString(obs->_prn.toString().c_str());
99 satData->slipFlag = false;
100 satData->P1 = 0.0;
101 satData->P2 = 0.0;
102 satData->P5 = 0.0;
103 satData->L1 = 0.0;
104 satData->L2 = 0.0;
105 satData->L5 = 0.0;
106 for (unsigned ifrq = 0; ifrq < obs->_obs.size(); ifrq++) {
107 t_frqObs* frqObs = obs->_obs[ifrq];
108 if (frqObs->_rnxType2ch[0] == '1') {
109 if (frqObs->_codeValid) satData->P1 = frqObs->_code;
110 if (frqObs->_phaseValid) satData->L1 = frqObs->_phase;
111 if (frqObs->_slip) satData->slipFlag = true;
112 }
113 else if (frqObs->_rnxType2ch[0] == '2') {
114 if (frqObs->_codeValid) satData->P2 = frqObs->_code;
115 if (frqObs->_phaseValid) satData->L2 = frqObs->_phase;
116 if (frqObs->_slip) satData->slipFlag = true;
117 }
118 else if (frqObs->_rnxType2ch[0] == '5') {
119 if (frqObs->_codeValid) satData->P5 = frqObs->_code;
120 if (frqObs->_phaseValid) satData->L5 = frqObs->_phase;
121 if (frqObs->_slip) satData->slipFlag = true;
122 }
123 }
124 putNewObs(satData);
125 }
126
127 // Data Pre-Processing
128 // -------------------
129 QMutableMapIterator<QString, t_satData*> it(_epoData->satData);
130 while (it.hasNext()) {
131 it.next();
132 QString prn = it.key();
133 t_satData* satData = it.value();
134
135 if (cmpToT(satData) != success) {
136 delete satData;
137 it.remove();
138 continue;
139 }
140 }
141
142 // Filter Solution
143 // ---------------
144 if (_model->update(_epoData) == success) {
145 output->_error = false;
146 }
147 else {
148 output->_error = true;
149 }
150
151 // Set Output
152 // ----------
153 output->_epoTime = _model->time();
154 output->_xyzRover[0] = _model->x();
155 output->_xyzRover[1] = _model->y();
156 output->_xyzRover[2] = _model->z();
157 output->_numSat = 0;
158 output->_pDop = 0.0;
159 output->_log = LOG.str();
160}
161
162//
163////////////////////////////////////////////////////////////////////////////
164void bncPPPclient::putNewObs(t_satData* satData) {
165
166 // Set Observations GPS and Glonass
167 // --------------------------------
168 if (satData->system() == 'G' || satData->system() == 'R') {
169 if (satData->P1 != 0.0 && satData->P2 != 0.0 &&
170 satData->L1 != 0.0 && satData->L2 != 0.0 ) {
171
172 int channel = 0;
173 if (satData->system() == 'R') {
174 cerr << "not yet implemented" << endl;
175 exit(0);
176 }
177
178 t_frequency::type fType1 = t_lc::toFreq(satData->system(), t_lc::l1);
179 t_frequency::type fType2 = t_lc::toFreq(satData->system(), t_lc::l2);
180 double f1 = t_CST::freq(fType1, channel);
181 double f2 = t_CST::freq(fType2, channel);
182 double a1 = f1 * f1 / (f1 * f1 - f2 * f2);
183 double a2 = - f2 * f2 / (f1 * f1 - f2 * f2);
184 satData->P3 = a1 * satData->P1 + a2 * satData->P2;
185 satData->L3 = a1 * satData->L1 + a2 * satData->L2;
186 satData->lambda3 = a1 * t_CST::c / f1 + a2 * t_CST::c / f2;
187 _epoData->satData[satData->prn] = satData;
188 }
189 else {
190 delete satData;
191 }
192 }
193
194 // Set Observations Galileo
195 // ------------------------
196 else if (satData->system() == 'E') {
197 if (satData->P1 != 0.0 && satData->P5 != 0.0 &&
198 satData->L1 != 0.0 && satData->L5 != 0.0 ) {
199 double f1 = t_CST::freq(t_frequency::E1, 0);
200 double f5 = t_CST::freq(t_frequency::E5, 0);
201 double a1 = f1 * f1 / (f1 * f1 - f5 * f5);
202 double a5 = - f5 * f5 / (f1 * f1 - f5 * f5);
203 satData->P3 = a1 * satData->P1 + a5 * satData->P5;
204 satData->L3 = a1 * satData->L1 + a5 * satData->L5;
205 satData->lambda3 = a1 * t_CST::c / f1 + a5 * t_CST::c / f5;
206 _epoData->satData[satData->prn] = satData;
207 }
208 else {
209 delete satData;
210 }
211 }
212}
213
214//
215////////////////////////////////////////////////////////////////////////////
216void bncPPPclient::putNewCorrections(QList<QString> corrList) {
217 QMutexLocker locker(&_mutex);
218
219 // Check the Mountpoint (source of corrections)
220 // --------------------------------------------
221 if (!_opt->_corrMount.empty()) {
222 QMutableListIterator<QString> itm(corrList);
223 while (itm.hasNext()) {
224 QStringList hlp = itm.next().split(" ");
225 if (hlp.size() > 0) {
226 QString mountpoint = hlp[hlp.size()-1];
227 if (mountpoint != QString(_opt->_corrMount.c_str())) {
228 itm.remove();
229 }
230 }
231 }
232 }
233
234 if (corrList.size() == 0) {
235 return;
236 }
237
238 QListIterator<QString> it(corrList);
239 while (it.hasNext()) {
240 QString line = it.next();
241
242 QTextStream in(&line);
243 int messageType;
244 int updateInterval;
245 int GPSweek;
246 double GPSweeks;
247 QString prn;
248 in >> messageType >> updateInterval >> GPSweek >> GPSweeks >> prn;
249
250 if ( t_corr::relevantMessageType(messageType) ) {
251 t_corr* cc = 0;
252 if (_corr.contains(prn)) {
253 cc = _corr.value(prn);
254 }
255 else {
256 cc = new t_corr();
257 _corr[prn] = cc;
258 }
259 cc->readLine(line);
260 _corr_tt = cc->tClk;
261 }
262 else if ( messageType == BTYPE_GPS || messageType == BTYPE_GLONASS ) {
263 t_bias* bb = 0;
264 if (_bias.contains(prn)) {
265 bb = _bias.value(prn);
266 }
267 else {
268 bb = new t_bias();
269 _bias[prn] = bb;
270 }
271 bb->readLine(line);
272 }
273 }
274}
275
276// Satellite Position
277////////////////////////////////////////////////////////////////////////////
278t_irc bncPPPclient::getSatPos(const bncTime& tt, const QString& prn,
279 ColumnVector& xc, ColumnVector& vv) {
280
281 const double MAXAGE = 120.0;
282
283 if (_eph.contains(prn)) {
284
285 if (_opt->useOrbClkCorr() && prn[0] != 'E') {
286 if (_corr.contains(prn)) {
287 t_corr* cc = _corr.value(prn);
288 if (cc->ready() && tt - cc->tClk < MAXAGE) {
289 t_eph* eLast = _eph.value(prn)->last;
290 t_eph* ePrev = _eph.value(prn)->prev;
291 if (eLast && eLast->IOD() == cc->iod) {
292 eLast->position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data());
293 return applyCorr(tt, cc, xc, vv);
294 }
295 else if (ePrev && ePrev->IOD() == cc->iod) {
296 ePrev->position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data());
297 return applyCorr(tt, cc, xc, vv);
298 }
299 }
300 }
301 }
302
303 else {
304 t_eph* ee = _eph.value(prn)->last;
305 ee->position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data());
306 return success;
307 }
308 }
309
310 return failure;
311}
312
313//
314////////////////////////////////////////////////////////////////////////////
315t_irc bncPPPclient::applyCorr(const bncTime& tt, const t_corr* cc,
316 ColumnVector& xc, ColumnVector& vv) {
317
318 double dtRao = tt - cc->tRao;
319
320 // Position
321 // --------
322 ColumnVector raoHlp = cc->rao + cc->dotRao * dtRao;
323
324 if (raoHlp.norm_Frobenius() > 20.0) {
325 return failure;
326 }
327
328 ColumnVector dx(3);
329 RSW_to_XYZ(xc.Rows(1,3), vv, raoHlp, dx);
330 xc[0] -= dx[0];
331 xc[1] -= dx[1];
332 xc[2] -= dx[2];
333
334 // Velocity
335 // --------
336 ColumnVector dotRaoHlp = cc->dotRao;
337
338 ColumnVector dv(3);
339 RSW_to_XYZ(xc.Rows(1,3), vv, dotRaoHlp, dv);
340 vv[0] -= dv[0];
341 vv[1] -= dv[1];
342 vv[2] -= dv[2];
343
344 // Clocks
345 // ------
346 double dtClk = tt - cc->tClk;
347
348 xc[3] += cc->dClk + cc->dotDClk * dtClk + cc->dotDotDClk * dtClk * dtClk
349 + cc->hrClk;
350
351 return success;
352}
353
354// Correct Time of Transmission
355////////////////////////////////////////////////////////////////////////////
356t_irc bncPPPclient::cmpToT(t_satData* satData) {
357
358 double prange = satData->P3;
359 if (prange == 0.0) {
360 return failure;
361 }
362
363 double clkSat = 0.0;
364 for (int ii = 1; ii <= 10; ii++) {
365
366 bncTime ToT = satData->tt - prange / t_CST::c - clkSat;
367
368 ColumnVector xc(4);
369 ColumnVector vv(3);
370 if (getSatPos(ToT, satData->prn, xc, vv) != success) {
371 return failure;
372 }
373
374 double clkSatOld = clkSat;
375 clkSat = xc(4);
376
377 if ( fabs(clkSat-clkSatOld) * t_CST::c < 1.e-4 ) {
378 satData->xx = xc.Rows(1,3);
379 satData->vv = vv;
380 satData->clk = clkSat * t_CST::c;
381 return success;
382 }
383 }
384
385 return failure;
386}
387
Note: See TracBrowser for help on using the repository browser.