source: ntrip/trunk/BNC/src/RTRover/bncrtrover.cpp@ 4783

Last change on this file since 4783 was 4783, checked in by mervart, 12 years ago
File size: 12.5 KB
Line 
1
2#include <iostream>
3#include <iomanip>
4#include <string.h>
5
6#include "bncrtrover.h"
7#include "bncapp.h"
8#include "bncsettings.h"
9#include "bnctime.h"
10
11#include "rtrover_interface.h"
12
13using namespace std;
14
15// Constructor
16////////////////////////////////////////////////////////////////////////////
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() {
29 bncSettings settings;
30
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);
39
40 // Define Input Options
41 // --------------------
42 rtrover_opt opt;
43 rtrover_initOptions(&opt);
44
45 opt._roverName = strdup(_roverMount.data());
46 opt._baseName = strdup(_baseMount.data());
47 opt._xyzAprRover[0] = settings.value("rtroverRoverRefCrdX").toDouble();
48 opt._xyzAprRover[1] = settings.value("rtroverRoverRefCrdY").toDouble();
49 opt._xyzAprRover[2] = settings.value("rtroverRoverRefCrdZ").toDouble();
50 opt._xyzAprBase[0] = settings.value("rtroverBaseRefCrdX").toDouble();
51 opt._xyzAprBase[1] = settings.value("rtroverBaseRefCrdY").toDouble();
52 opt._xyzAprBase[2] = settings.value("rtroverBaseRefCrdZ").toDouble();
53
54 rtrover_setOptions(&opt);
55
56 free(opt._roverName);
57 free(opt._baseName);
58
59 // Connect to BNC Signals
60 // ----------------------
61 connect(((bncApp*)qApp), SIGNAL(newCorrections(QList<QString>)),
62 this, SLOT(slotNewCorrections(QList<QString>)));
63
64 connect(((bncApp*)qApp), SIGNAL(newEphGPS(gpsephemeris)),
65 this, SLOT(slotNewEphGPS(gpsephemeris)));
66
67 connect(((bncApp*)qApp), SIGNAL(newEphGlonass(glonassephemeris)),
68 this, SLOT(slotNewEphGlonass(glonassephemeris)));
69
70 connect(((bncApp*)qApp), SIGNAL(newEphGalileo(galileoephemeris)),
71 this, SLOT(slotNewEphGalileo(galileoephemeris)));
72
73 // Start processing loop
74 // ---------------------
75 QThread::exec();
76}
77
78//
79////////////////////////////////////////////////////////////////////////////
80void t_bncRtrover::slotNewEphGPS(gpsephemeris gpseph) {
81 QMutexLocker locker(&_mutex);
82
83 bncTime toc(gpseph.GPSweek, gpseph.TOC);
84 bncTime toe(gpseph.GPSweek, gpseph.TOE);
85
86 rtrover_ephGPS eph;
87 eph._satellite._system = 'G';
88 eph._satellite._number = gpseph.satellite;
89 eph._TOC._mjd = toc.mjd();
90 eph._TOC._sec = toc.daysec();
91 eph._TOE._mjd = toe.mjd();
92 eph._TOE._sec = toe.daysec();
93 eph._IODE = gpseph.IODE;
94 eph._IODC = gpseph.IODC;
95 eph._clock_bias = gpseph.clock_bias;
96 eph._clock_drift = gpseph.clock_drift;
97 eph._clock_driftrate = gpseph.clock_driftrate;
98 eph._Crs = gpseph.Crs;
99 eph._Delta_n = gpseph.Delta_n;
100 eph._M0 = gpseph.M0;
101 eph._Cuc = gpseph.Cuc;
102 eph._e = gpseph.e;
103 eph._Cus = gpseph.Cus;
104 eph._sqrt_A = gpseph.sqrt_A;
105 eph._Cic = gpseph.Cic;
106 eph._OMEGA0 = gpseph.OMEGA0;
107 eph._Cis = gpseph.Cis;
108 eph._i0 = gpseph.i0;
109 eph._Crc = gpseph.Crc;
110 eph._omega = gpseph.omega;
111 eph._OMEGADOT = gpseph.OMEGADOT;
112 eph._IDOT = gpseph.IDOT;
113 eph._TGD = gpseph.TGD;
114 eph._health = gpseph.SVhealth;
115
116 rtrover_putGPSEphemeris(&eph);
117}
118
119//
120////////////////////////////////////////////////////////////////////////////
121void t_bncRtrover::slotNewEphGlonass(glonassephemeris gloeph) {
122 QMutexLocker locker(&_mutex);
123
124 int wwUTC = gloeph.GPSWeek;
125 int towUTC = gloeph.GPSTOW;
126 updatetime(&wwUTC, &towUTC, gloeph.tb*1000, 1); // Moscow -> UTC
127 bncTime tUTC(wwUTC,towUTC);
128
129 int wwGPS = gloeph.GPSWeek;
130 int towGPS = gloeph.GPSTOW;
131 updatetime(&wwGPS, &towGPS, gloeph.tb*1000, 0); // Moscow -> GPS
132 bncTime tGPS(wwGPS,towGPS);
133
134 rtrover_ephGlo eph;
135 eph._satellite._system = 'R';
136 eph._satellite._number = gloeph.almanac_number;
137 eph._timeUTC._mjd = tUTC.mjd();
138 eph._timeUTC._sec = tUTC.daysec();
139 eph._gps_utc = int(tGPS-tUTC);
140 eph._E = gloeph.E;
141 eph._tau = gloeph.tau;
142 eph._gamma = gloeph.gamma;
143 eph._x_pos = gloeph.x_pos;
144 eph._x_velocity = gloeph.x_velocity;
145 eph._x_acceleration = gloeph.x_acceleration;
146 eph._y_pos = gloeph.y_pos;
147 eph._y_velocity = gloeph.y_velocity;
148 eph._y_acceleration = gloeph.y_acceleration;
149 eph._z_pos = gloeph.z_pos;
150 eph._z_velocity = gloeph.z_velocity;
151 eph._z_acceleration = gloeph.z_acceleration;
152 eph._health = 0; // TODO ?
153 eph._frequency_number = gloeph.frequency_number;
154
155 rtrover_putGloEphemeris(&eph);
156}
157
158//
159////////////////////////////////////////////////////////////////////////////
160void t_bncRtrover::slotNewEphGalileo(galileoephemeris /* galeph */) {
161 // not yet implemented
162}
163
164//
165////////////////////////////////////////////////////////////////////////////
166void t_bncRtrover::slotNewCorrections(QList<QString> corrList) {
167 QMutexLocker locker(&_mutex);
168
169 if (corrList.size() == 0) {
170 return;
171 }
172
173 int numOrbCorr = 0;
174 int numClkCorr = 0;
175
176 rtrover_orbCorr orbCorr[corrList.size()];
177 rtrover_clkCorr clkCorr[corrList.size()];
178
179 QListIterator<QString> it(corrList);
180 while (it.hasNext()) {
181 QString line = it.next();
182
183 // Check the Mountpoint
184 // --------------------
185 QStringList hlp = line.split(" ");
186 if (hlp.size() > 0) {
187 QString mountpoint = hlp[hlp.size()-1];
188 if (mountpoint != _corrMount) {
189 continue;
190 }
191 }
192
193 t_corr corr;
194 if (corr.readLine(line) == success) {
195
196 if (corr.messageType == COTYPE_GPSCOMBINED ||
197 corr.messageType == COTYPE_GLONASSCOMBINED ||
198 corr.messageType == COTYPE_GPSORBIT ||
199 corr.messageType == COTYPE_GLONASSORBIT ) {
200 ++numOrbCorr;
201 rtrover_orbCorr& orbC = orbCorr[numOrbCorr-1];
202 orbC._satellite._system = corr.prn.toAscii()[0];
203 orbC._satellite._number = corr.prn.mid(1).toInt();
204 orbC._iod = corr.iod;
205 orbC._time._mjd = corr.tRao.mjd();
206 orbC._time._sec = corr.tRao.daysec();
207 for (int ii = 0; ii < 3; ii++) {
208 orbC._rao[ii] = corr.rao[ii];
209 orbC._dotRao[ii] = corr.dotRao[ii];
210 orbC._dotDotRao[ii] = corr.dotDotRao[ii];
211 }
212 }
213
214 if (corr.messageType == COTYPE_GPSCOMBINED ||
215 corr.messageType == COTYPE_GLONASSCOMBINED ||
216 corr.messageType == COTYPE_GPSCLOCK ||
217 corr.messageType == COTYPE_GLONASSCLOCK ) {
218 ++numClkCorr;
219 rtrover_clkCorr& clkC = clkCorr[numClkCorr-1];
220 clkC._satellite._system = corr.prn.toAscii()[0];
221 clkC._satellite._number = corr.prn.mid(1).toInt();
222 clkC._iod = corr.iod;
223 clkC._time._mjd = corr.tClk.mjd();
224 clkC._time._sec = corr.tClk.daysec();
225 clkC._dClk = corr.dClk;
226 clkC._dotDClk = corr.dotDClk;
227 clkC._dotDotDClk = corr.dotDotDClk;
228 }
229 }
230 }
231
232 if (numOrbCorr > 0) {
233 rtrover_putOrbCorrections(numOrbCorr, orbCorr);
234 }
235 if (numClkCorr > 0) {
236 rtrover_putClkCorrections(numClkCorr, clkCorr);
237 }
238}
239
240// Auxiliary function - copy observation data
241////////////////////////////////////////////////////////////////////////////
242void copyObs(const t_obs& obsBnc, rtrover_satObs& satObs) {
243
244 bncTime obsTime(obsBnc.GPSWeek, obsBnc.GPSWeeks);
245 satObs._satellite._system = obsBnc.satSys;
246 satObs._satellite._number = obsBnc.satNum;
247 satObs._time._mjd = obsTime.mjd();
248 satObs._time._sec = obsTime.daysec();
249 satObs._slotNumber = obsBnc.slotNum;
250
251 QMap<QByteArray, rtrover_obs> allObs;
252 for (int iEntry = 0; iEntry < GNSSENTRY_NUMBER; ++iEntry) {
253 if (obsBnc._measdata[iEntry] != 0.0) {
254 QByteArray rnxStr = obsBnc.rnxStr(iEntry).toAscii();
255 if (rnxStr.length() == 3) {
256 QByteArray codeType = rnxStr.mid(1);
257
258 bool existed = allObs.contains(codeType);
259 rtrover_obs& currObs = allObs[codeType];
260 if (!existed) {
261 rtrover_initObs(&currObs);
262 currObs._rnxType[0] = codeType[0];
263 currObs._rnxType[1] = codeType[1];
264 }
265
266 if (rnxStr[0] == 'C') {
267 currObs._code = obsBnc._measdata[iEntry];
268 currObs._codeValid = true;
269 }
270 else if (rnxStr[0] == 'L') {
271 currObs._phase = obsBnc._measdata[iEntry];
272 currObs._phaseValid = true;
273 if (codeType[0] == '1') {
274 currObs._slip = obsBnc.slipL1;
275 currObs._slipCounter = obsBnc.slip_cnt_L1;
276 }
277 else if (codeType[0] == '2') {
278 currObs._slip = obsBnc.slipL2;
279 currObs._slipCounter = obsBnc.slip_cnt_L2;
280 }
281 else if (codeType[0] == '5') {
282 currObs._slip = obsBnc.slipL5;
283 currObs._slipCounter = obsBnc.slip_cnt_L5;
284 }
285 }
286 else if (rnxStr[0] == 'D') {
287 currObs._doppler = obsBnc._measdata[iEntry];
288 currObs._dopplerValid = true;
289 }
290 else if (rnxStr[0] == 'S') {
291 currObs._snr = obsBnc._measdata[iEntry];
292 currObs._snrValid = true;
293 }
294 }
295 }
296 }
297 satObs._numObs = allObs.size();
298 satObs._obs = new rtrover_obs[satObs._numObs];
299 int iObs = -1;
300 QMapIterator<QByteArray, rtrover_obs> it(allObs);
301 while (it.hasNext()) {
302 it.next();
303 ++iObs;
304 satObs._obs[iObs] = it.value();
305 }
306}
307
308//
309////////////////////////////////////////////////////////////////////////////
310void t_bncRtrover::slotNewObs(QByteArray staID, bool /* firstObs */, t_obs obsIn) {
311 QMutexLocker locker(&_mutex);
312
313 if (staID != _roverMount && staID != _baseMount) {
314 return;
315 }
316
317 bncTime obsTime(obsIn.GPSWeek, obsIn.GPSWeeks);
318
319 // Find corresponding epoch or create a new one
320 // --------------------------------------------
321 t_epoData* epoData = 0;
322 for (unsigned ii = 0; ii < _epochs.size(); ii++) {
323 if (_epochs[ii]->_time == obsTime) {
324 epoData = _epochs[ii];
325 break;
326 }
327 }
328 if (epoData == 0) {
329 if (_epochs.size() == 0 || _epochs.back()->_time < obsTime) {
330 epoData = new t_epoData();
331 epoData->_time = obsTime;
332 _epochs.push_back(epoData);
333 }
334 else {
335 return;
336 }
337 }
338
339 // Store observation into epoch class
340 // ----------------------------------
341 if (staID == _roverMount) {
342 epoData->_obsRover.push_back(obsIn);
343 }
344 else if (staID == _baseMount) {
345 epoData->_obsBase.push_back(obsIn);
346 }
347
348 // Wait for observations
349 // ---------------------
350 const double WAITTIME = 5.0;
351 double dt = 0.0;
352 if (_epochs.size() > 1) {
353 dt = _epochs.back()->_time - _epochs.front()->_time;
354 }
355 if (dt < WAITTIME) {
356 return;
357 }
358
359 // Copy observations into rtrover_satObs structures
360 // ------------------------------------------------
361 t_epoData* frontEpoData = _epochs.front();
362 _epochs.erase(_epochs.begin());
363
364 int numSatRover = frontEpoData->_obsRover.size();
365 rtrover_satObs satObsRover[numSatRover];
366 for (int ii = 0; ii < numSatRover; ii++) {
367 const t_obs& obsBnc = frontEpoData->_obsRover[ii];
368 rtrover_satObs& satObs = satObsRover[ii];
369 copyObs(obsBnc, satObs);
370 }
371
372 int numSatBase = frontEpoData->_obsBase.size();
373 rtrover_satObs satObsBase[numSatBase];
374 for (int ii = 0; ii < numSatBase; ii++) {
375 const t_obs& obsBnc = frontEpoData->_obsBase[ii];
376 rtrover_satObs& satObs = satObsBase[ii];
377 copyObs(obsBnc, satObs);
378 }
379
380 delete frontEpoData;
381
382 // Process single epoch
383 // --------------------
384 rtrover_output output;
385 rtrover_processEpoch(numSatRover, satObsRover, numSatBase, satObsBase, &output);
386
387 // Write output
388 // ---------------------
389 _outputFile.write(output._log);
390 _outputFile.flush();
391
392 // Free memory
393 // -----------
394 rtrover_freeOutput(&output);
395 for (int ii = 0; ii < numSatRover; ii++) {
396 rtrover_satObs& satObs = satObsRover[ii];
397 delete [] satObs._obs;
398 }
399 for (int ii = 0; ii < numSatBase; ii++) {
400 rtrover_satObs& satObs = satObsBase[ii];
401 delete [] satObs._obs;
402 }
403}
Note: See TracBrowser for help on using the repository browser.