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

Last change on this file since 7025 was 6974, checked in by stuerze, 10 years ago

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