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

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