source: ntrip/trunk/BNC/src/PPP/pppThread.cpp@ 5680

Last change on this file since 5680 was 5680, checked in by mervart, 10 years ago
File size: 15.8 KB
Line 
1
2#include <iostream>
3#include <iomanip>
4#include <string.h>
5
6#include "pppThread.h"
7#include "bnccore.h"
8#include "bncsettings.h"
9#include "bnctime.h"
10
11using namespace std;
12
13// Constructor
14////////////////////////////////////////////////////////////////////////////
15t_pppThread::t_pppThread() : QThread(0) {
16}
17
18// Destructor
19////////////////////////////////////////////////////////////////////////////
20t_pppThread::~t_pppThread() {
21}
22
23// Run (virtual)
24////////////////////////////////////////////////////////////////////////////
25void t_pppThread::run() {
26 bncSettings settings;
27
28 // User Options
29 // ------------
30 _mode = settings.value("rtroverMode").toByteArray();
31 _roverMount = settings.value("rtroverRoverMount").toByteArray();
32 _baseMount = settings.value("rtroverBaseMount").toByteArray();
33 _corrMount = settings.value("rtroverCorrMount").toByteArray();
34 _outputFile.setFileName(settings.value("rtroverOutput").toString());
35 _outputFile.open(QIODevice::WriteOnly | QIODevice::Text);
36
37 // Define Input Options
38 // --------------------
39 rtrover_opt opt;
40 rtrover_initOptions(&opt);
41
42 if (_mode == "PPP_DF") {
43 opt._mode = mode_PPP_DF;
44 }
45 else if (_mode == "SPP_DF") {
46 opt._mode = mode_SPP_DF;
47 }
48 else if (_mode == "PPP_SF") {
49 opt._mode = mode_PPP_SF;
50 }
51 else if (_mode == "SPP_SF") {
52 opt._mode = mode_SPP_SF;
53 }
54 else if (_mode == "PPP_AR") {
55 opt._mode = mode_PPP_AR;
56 }
57 else if (_mode == "RTK") {
58 opt._mode = mode_RTK;
59 }
60 else if (_mode == "PPP_FTTF") {
61 opt._mode = mode_PPP_FTTF;
62 }
63
64 QByteArray antNameRover = settings.value("rtroverRoverAntenna").toByteArray();
65 QByteArray antNameBase = settings.value("rtroverBaseAntenna").toByteArray();
66 QByteArray antexFileName = settings.value("rtroverAntex").toByteArray();
67
68 if (!_roverMount.isEmpty()) opt._roverName = _roverMount.data();
69 if (!_baseMount.isEmpty()) opt._baseName = _baseMount.data();
70 if (!antNameRover.isEmpty()) opt._antNameRover = antNameRover.data();
71 if (!antNameBase.isEmpty()) opt._antNameBase = antNameBase.data();
72 if (!antexFileName.isEmpty()) opt._antexFileName = antexFileName.data();
73
74 opt._xyzAprRover[0] = settings.value("rtroverRoverRefCrdX").toDouble();
75 opt._xyzAprRover[1] = settings.value("rtroverRoverRefCrdY").toDouble();
76 opt._xyzAprRover[2] = settings.value("rtroverRoverRefCrdZ").toDouble();
77 opt._xyzAprBase[0] = settings.value("rtroverBaseRefCrdX").toDouble();
78 opt._xyzAprBase[1] = settings.value("rtroverBaseRefCrdY").toDouble();
79 opt._xyzAprBase[2] = settings.value("rtroverBaseRefCrdZ").toDouble();
80 opt._neuEccRover[0] = settings.value("rtroverRoverDN").toDouble();
81 opt._neuEccRover[1] = settings.value("rtroverRoverDE").toDouble();
82 opt._neuEccRover[2] = settings.value("rtroverRoverDU").toDouble();
83 opt._neuEccBase[0] = settings.value("rtroverBaseDN").toDouble();
84 opt._neuEccBase[1] = settings.value("rtroverBaseDE").toDouble();
85 opt._neuEccBase[2] = settings.value("rtroverBaseDU").toDouble();
86 opt._logLevel = 2;
87
88 rtrover_setOptions(&opt);
89
90 // Connect to BNC Signals
91 // ----------------------
92 connect(BNC_CORE, SIGNAL(newCorrections(QList<QString>)),
93 this, SLOT(slotNewCorrections(QList<QString>)));
94
95 connect(BNC_CORE, SIGNAL(newEphGPS(gpsephemeris)),
96 this, SLOT(slotNewEphGPS(gpsephemeris)));
97
98 connect(BNC_CORE, SIGNAL(newEphGlonass(glonassephemeris)),
99 this, SLOT(slotNewEphGlonass(glonassephemeris)));
100
101 connect(BNC_CORE, SIGNAL(newEphGalileo(galileoephemeris)),
102 this, SLOT(slotNewEphGalileo(galileoephemeris)));
103
104 // Start processing loop
105 // ---------------------
106 QThread::exec();
107}
108
109//
110////////////////////////////////////////////////////////////////////////////
111void t_pppThread::slotNewEphGPS(gpsephemeris gpseph) {
112 QMutexLocker locker(&_mutex);
113
114 bncTime toc(gpseph.GPSweek, gpseph.TOC);
115 bncTime toe(gpseph.GPSweek, gpseph.TOE);
116
117 rtrover_ephGPS eph;
118 eph._satellite._system = 'G';
119 eph._satellite._number = gpseph.satellite;
120 eph._TOC._mjd = toc.mjd();
121 eph._TOC._sec = toc.daysec();
122 eph._TOE._mjd = toe.mjd();
123 eph._TOE._sec = toe.daysec();
124 eph._IODE = gpseph.IODE;
125 eph._IODC = gpseph.IODC;
126 eph._clock_bias = gpseph.clock_bias;
127 eph._clock_drift = gpseph.clock_drift;
128 eph._clock_driftrate = gpseph.clock_driftrate;
129 eph._Crs = gpseph.Crs;
130 eph._Delta_n = gpseph.Delta_n;
131 eph._M0 = gpseph.M0;
132 eph._Cuc = gpseph.Cuc;
133 eph._e = gpseph.e;
134 eph._Cus = gpseph.Cus;
135 eph._sqrt_A = gpseph.sqrt_A;
136 eph._Cic = gpseph.Cic;
137 eph._OMEGA0 = gpseph.OMEGA0;
138 eph._Cis = gpseph.Cis;
139 eph._i0 = gpseph.i0;
140 eph._Crc = gpseph.Crc;
141 eph._omega = gpseph.omega;
142 eph._OMEGADOT = gpseph.OMEGADOT;
143 eph._IDOT = gpseph.IDOT;
144 eph._TGD = gpseph.TGD;
145 eph._health = gpseph.SVhealth;
146
147 rtrover_putGPSEphemeris(&eph);
148}
149
150//
151////////////////////////////////////////////////////////////////////////////
152void t_pppThread::slotNewEphGlonass(glonassephemeris gloeph) {
153 QMutexLocker locker(&_mutex);
154
155 int wwUTC = gloeph.GPSWeek;
156 int towUTC = gloeph.GPSTOW;
157 updatetime(&wwUTC, &towUTC, gloeph.tb*1000, 1); // Moscow -> UTC
158 bncTime tUTC(wwUTC,towUTC);
159
160 int wwGPS = gloeph.GPSWeek;
161 int towGPS = gloeph.GPSTOW;
162 updatetime(&wwGPS, &towGPS, gloeph.tb*1000, 0); // Moscow -> GPS
163 bncTime tGPS(wwGPS,towGPS);
164
165 rtrover_ephGlo eph;
166 eph._satellite._system = 'R';
167 eph._satellite._number = gloeph.almanac_number;
168 eph._timeUTC._mjd = tUTC.mjd();
169 eph._timeUTC._sec = tUTC.daysec();
170 eph._gps_utc = int(tGPS-tUTC);
171 eph._E = gloeph.E;
172 eph._tau = gloeph.tau;
173 eph._gamma = gloeph.gamma;
174 eph._x_pos = gloeph.x_pos;
175 eph._x_velocity = gloeph.x_velocity;
176 eph._x_acceleration = gloeph.x_acceleration;
177 eph._y_pos = gloeph.y_pos;
178 eph._y_velocity = gloeph.y_velocity;
179 eph._y_acceleration = gloeph.y_acceleration;
180 eph._z_pos = gloeph.z_pos;
181 eph._z_velocity = gloeph.z_velocity;
182 eph._z_acceleration = gloeph.z_acceleration;
183 eph._health = 0; // TODO ?
184 eph._frequency_number = gloeph.frequency_number;
185
186 rtrover_putGloEphemeris(&eph);
187}
188
189//
190////////////////////////////////////////////////////////////////////////////
191void t_pppThread::slotNewEphGalileo(galileoephemeris /* galeph */) {
192 // not yet implemented
193}
194
195//
196////////////////////////////////////////////////////////////////////////////
197void t_pppThread::slotNewCorrections(QList<QString> corrList) {
198 QMutexLocker locker(&_mutex);
199
200 if (corrList.size() == 0) {
201 return;
202 }
203
204 int numOrbCorr = 0;
205 int numClkCorr = 0;
206 int numBiases = 0;
207
208 rtrover_orbCorr orbCorr[corrList.size()];
209 rtrover_clkCorr clkCorr[corrList.size()];
210 rtrover_satBiases biases[corrList.size()];
211
212 QListIterator<QString> it(corrList);
213 while (it.hasNext()) {
214 QString line = it.next();
215
216 // Check the Mountpoint
217 // --------------------
218 QStringList hlp = line.split(" ");
219 if (hlp.size() > 0) {
220 QString mountpoint = hlp[hlp.size()-1];
221 if (mountpoint != _corrMount) {
222 continue;
223 }
224 }
225
226 // Orbit and clock corrections
227 // ---------------------------
228 t_corr corr;
229 if (corr.readLine(line) == success) {
230
231 if (corr.messageType == COTYPE_GPSCOMBINED ||
232 corr.messageType == COTYPE_GLONASSCOMBINED ||
233 corr.messageType == COTYPE_GPSORBIT ||
234 corr.messageType == COTYPE_GLONASSORBIT ) {
235
236 _IODs[corr.prn] = corr.iod; // remember iod;
237
238 ++numOrbCorr;
239 rtrover_orbCorr& orbC = orbCorr[numOrbCorr-1];
240 orbC._satellite._system = corr.prn.toAscii()[0];
241 orbC._satellite._number = corr.prn.mid(1).toInt();
242 orbC._iod = corr.iod;
243 orbC._time._mjd = corr.tRao.mjd();
244 orbC._time._sec = corr.tRao.daysec();
245 orbC._system = 'R';
246 for (int ii = 0; ii < 3; ii++) {
247 orbC._xr[ii] = corr.rao[ii];
248 orbC._dotXr[ii] = corr.dotRao[ii];
249 }
250 }
251
252 if (corr.messageType == COTYPE_GPSCOMBINED ||
253 corr.messageType == COTYPE_GLONASSCOMBINED ||
254 corr.messageType == COTYPE_GPSCLOCK ||
255 corr.messageType == COTYPE_GLONASSCLOCK ) {
256 ++numClkCorr;
257 rtrover_clkCorr& clkC = clkCorr[numClkCorr-1];
258 clkC._satellite._system = corr.prn.toAscii()[0];
259 clkC._satellite._number = corr.prn.mid(1).toInt();
260 if (_IODs.contains(corr.prn)) {
261 clkC._iod = _IODs[corr.prn];
262 }
263 else {
264 clkC._iod = 0;
265 }
266 clkC._time._mjd = corr.tClk.mjd();
267 clkC._time._sec = corr.tClk.daysec();
268 clkC._dClk = corr.dClk;
269 clkC._dotDClk = corr.dotDClk;
270 clkC._dotDotDClk = corr.dotDotDClk;
271 }
272 }
273
274 // Code Biases
275 // -----------
276 t_bias bias;
277 if (bias.readLine(line) == success) {
278 ++numBiases;
279 rtrover_satBiases& satBiases = biases[numBiases-1];
280 satBiases._satellite._system = bias._prn.toAscii()[0];
281 satBiases._satellite._number = bias._prn.mid(1).toInt();
282 satBiases._time._mjd = bias._time.mjd();
283 satBiases._time._sec = bias._time.daysec();
284 satBiases._numBiases = bias._value.size();
285 satBiases._biases = new rtrover_bias[satBiases._numBiases];
286 int iBias = -1;
287 QMapIterator<QByteArray, double> it(bias._value);
288 while (it.hasNext()) {
289 it.next();
290 ++iBias;
291 rtrover_bias& singleBias = satBiases._biases[iBias];
292 singleBias._rnxType3ch[0] = 'C';
293 singleBias._rnxType3ch[1] = it.key()[0];
294 singleBias._rnxType3ch[2] = it.key()[1];
295 singleBias._rnxType3ch[3] = '\0';
296 singleBias._value = it.value();
297 }
298 }
299 }
300
301 // Pass Corrections and Biases to client library
302 // ---------------------------------------------
303 if (numOrbCorr > 0) {
304 rtrover_putOrbCorrections(numOrbCorr, orbCorr);
305 }
306 if (numClkCorr > 0) {
307 rtrover_putClkCorrections(numClkCorr, clkCorr);
308 }
309 if (numBiases > 0) {
310 rtrover_putBiases(numBiases, biases);
311 }
312
313 // Clean the memory
314 // ----------------
315 for (int ii = 0; ii < numBiases; ii++) {
316 delete [] biases[ii]._biases;
317 }
318}
319
320// Auxiliary function - copy observation data
321////////////////////////////////////////////////////////////////////////////
322void copyObs(const t_obs& obsBnc, rtrover_satObs& satObs) {
323
324 bncTime obsTime(obsBnc.GPSWeek, obsBnc.GPSWeeks);
325 satObs._satellite._system = obsBnc.satSys;
326 satObs._satellite._number = obsBnc.satNum;
327 satObs._time._mjd = obsTime.mjd();
328 satObs._time._sec = obsTime.daysec();
329
330 QMap<QByteArray, rtrover_obs> allObs;
331 for (int iEntry = 0; iEntry < GNSSENTRY_NUMBER; ++iEntry) {
332 if (obsBnc._measdata[iEntry] != 0.0) {
333 QByteArray rnxStr = obsBnc.rnxStr(iEntry).toAscii();
334 if (rnxStr.length() >= 2) {
335 if (rnxStr == "L1") {
336 rnxStr = "L1C";
337 }
338 else if (rnxStr == "L2") {
339 rnxStr = "L2P";
340 }
341 QByteArray codeType = rnxStr.mid(1);
342
343 bool existed = allObs.contains(codeType);
344 rtrover_obs& currObs = allObs[codeType];
345 if (!existed) {
346 rtrover_initObs(&currObs);
347 currObs._rnxType2ch[0] = codeType[0];
348 currObs._rnxType2ch[1] = codeType[1];
349 currObs._rnxType2ch[2] = '\0';
350 }
351
352 if (rnxStr[0] == 'C') {
353 currObs._code = obsBnc._measdata[iEntry];
354 currObs._codeValid = true;
355 }
356 else if (rnxStr[0] == 'L') {
357 currObs._phase = obsBnc._measdata[iEntry];
358 currObs._phaseValid = true;
359 if (codeType[0] == '1') {
360 currObs._slip = obsBnc.slipL1;
361 currObs._slipCounter = obsBnc.slip_cnt_L1;
362 }
363 else if (codeType[0] == '2') {
364 currObs._slip = obsBnc.slipL2;
365 currObs._slipCounter = obsBnc.slip_cnt_L2;
366 }
367 else if (codeType[0] == '5') {
368 currObs._slip = obsBnc.slipL5;
369 currObs._slipCounter = obsBnc.slip_cnt_L5;
370 }
371 }
372 else if (rnxStr[0] == 'D') {
373 currObs._doppler = obsBnc._measdata[iEntry];
374 currObs._dopplerValid = true;
375 }
376 else if (rnxStr[0] == 'S') {
377 currObs._snr = obsBnc._measdata[iEntry];
378 currObs._snrValid = true;
379 }
380 }
381 }
382 }
383 satObs._numObs = allObs.size();
384 satObs._obs = new rtrover_obs[satObs._numObs];
385 int iObs = -1;
386 QMapIterator<QByteArray, rtrover_obs> it(allObs);
387 while (it.hasNext()) {
388 it.next();
389 ++iObs;
390 satObs._obs[iObs] = it.value();
391 }
392}
393
394//
395////////////////////////////////////////////////////////////////////////////
396void t_pppThread::slotNewObs(QByteArray staID, QList<t_obs> obsList) {
397 QMutexLocker locker(&_mutex);
398
399 if (staID != _roverMount && staID != _baseMount) {
400 return;
401 }
402
403 // Store Observations into epochs
404 // ------------------------------
405 QListIterator<t_obs> it(obsList);
406 while (it.hasNext()) {
407
408 const t_obs& obsIn = it.next();
409
410 bncTime obsTime(obsIn.GPSWeek, obsIn.GPSWeeks);
411
412 // Find corresponding epoch or create a new one
413 // --------------------------------------------
414 t_epoData* epoData = 0;
415 for (unsigned ii = 0; ii < _epochs.size(); ii++) {
416 if (_epochs[ii]->_time == obsTime) {
417 epoData = _epochs[ii];
418 break;
419 }
420 }
421 if (epoData == 0) {
422 if (_epochs.size() == 0 || _epochs.back()->_time < obsTime) {
423 epoData = new t_epoData();
424 epoData->_time = obsTime;
425 _epochs.push_back(epoData);
426 }
427 else {
428 continue;
429 }
430 }
431
432 // Store observation into epoch class
433 // ----------------------------------
434 if (staID == _roverMount) {
435 epoData->_obsRover.push_back(obsIn);
436 }
437 else if (staID == _baseMount) {
438 epoData->_obsBase.push_back(obsIn);
439 }
440 }
441
442 // Process Epochs
443 // --------------
444 while (_epochs.size() > 1) {
445 const double WAITTIME = 5.0;
446 double dt = _epochs.back()->_time - _epochs.front()->_time;
447 if (dt > WAITTIME) {
448 processFrontEpoch();
449 }
450 else {
451 break;
452 }
453 }
454}
455
456//
457////////////////////////////////////////////////////////////////////////////
458void t_pppThread::processFrontEpoch() {
459
460 // Copy observations into rtrover_satObs structures
461 // ------------------------------------------------
462 t_epoData* frontEpoData = _epochs.front();
463 _epochs.erase(_epochs.begin());
464
465 int numSatRover = frontEpoData->_obsRover.size();
466 rtrover_satObs satObsRover[numSatRover];
467 for (int ii = 0; ii < numSatRover; ii++) {
468 const t_obs& obsBnc = frontEpoData->_obsRover[ii];
469 rtrover_satObs& satObs = satObsRover[ii];
470 copyObs(obsBnc, satObs);
471 }
472
473 int numSatBase = frontEpoData->_obsBase.size();
474 rtrover_satObs satObsBase[numSatBase];
475 for (int ii = 0; ii < numSatBase; ii++) {
476 const t_obs& obsBnc = frontEpoData->_obsBase[ii];
477 rtrover_satObs& satObs = satObsBase[ii];
478 copyObs(obsBnc, satObs);
479 }
480
481 delete frontEpoData;
482
483 // Process single epoch
484 // --------------------
485 rtrover_output output;
486 rtrover_processEpoch(numSatRover, satObsRover, numSatBase, satObsBase, &output);
487
488 // Write output
489 // ---------------------
490 _outputFile.write(output._log);
491 _outputFile.flush();
492
493 // Free memory
494 // -----------
495 rtrover_freeOutput(&output);
496 for (int ii = 0; ii < numSatRover; ii++) {
497 rtrover_satObs& satObs = satObsRover[ii];
498 delete [] satObs._obs;
499 }
500 for (int ii = 0; ii < numSatBase; ii++) {
501 rtrover_satObs& satObs = satObsBase[ii];
502 delete [] satObs._obs;
503 }
504}
Note: See TracBrowser for help on using the repository browser.