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

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