source: ntrip/trunk/BNS/bns.cpp@ 1066

Last change on this file since 1066 was 1066, checked in by mervart, 16 years ago

* empty log message *
File size: 11.7 KB
Line 
1/* -------------------------------------------------------------------------
2 * BKG NTRIP Server
3 * -------------------------------------------------------------------------
4 *
5 * Class: bns
6 *
7 * Purpose: This class implements the main application behaviour
8 *
9 * Author: L. Mervart
10 *
11 * Created: 29-Mar-2008
12 *
13 * Changes:
14 *
15 * -----------------------------------------------------------------------*/
16
17#include <math.h>
18#include <iostream>
19#include <newmatio.h>
20
21#include "bns.h"
22#include "bnsutils.h"
23#include "bnsrinex.h"
24#include "bnssp3.h"
25
26using namespace std;
27
28// Constructor
29////////////////////////////////////////////////////////////////////////////
30t_bns::t_bns(QObject* parent) : QThread(parent) {
31
32 this->setTerminationEnabled(true);
33
34 connect(this, SIGNAL(moveSocket(QThread*)),
35 this, SLOT(slotMoveSocket(QThread*)));
36
37 QSettings settings;
38
39 // Set Proxy (application-wide)
40 // ----------------------------
41 QString proxyHost = settings.value("proxyHost").toString();
42 int proxyPort = settings.value("proxyPort").toInt();
43
44 QNetworkProxy proxy;
45 if (proxyHost.isEmpty()) {
46 proxy.setType(QNetworkProxy::NoProxy);
47 }
48 else {
49 proxy.setType(QNetworkProxy::Socks5Proxy);
50 proxy.setHostName(proxyHost);
51 proxy.setPort(proxyPort);
52 }
53 QNetworkProxy::setApplicationProxy(proxy);
54
55 // Thread that handles broadcast ephemeris
56 // ---------------------------------------
57 _bnseph = new t_bnseph(parent);
58
59 connect(_bnseph, SIGNAL(newEph(t_eph*, int)),
60 this, SLOT(slotNewEph(t_eph*, int)));
61 connect(_bnseph, SIGNAL(newMessage(QByteArray)),
62 this, SLOT(slotMessage(const QByteArray)));
63 connect(_bnseph, SIGNAL(error(QByteArray)),
64 this, SLOT(slotError(const QByteArray)));
65
66 // Server listening for rtnet results
67 // ----------------------------------
68 _clkSocket = 0;
69 _clkServer = new QTcpServer;
70 _clkServer->listen(QHostAddress::Any, settings.value("clkPort").toInt());
71 connect(_clkServer, SIGNAL(newConnection()),this, SLOT(slotNewConnection()));
72
73 // Socket and file for outputting the results
74 // -------------------------------------------
75 _caster.push_back(new t_bnscaster(settings.value("mountpoint").toString()));
76 connect(_caster.back(), SIGNAL(error(const QByteArray)),
77 this, SLOT(slotError(const QByteArray)));
78 connect(_caster.back(), SIGNAL(newMessage(const QByteArray)),
79 this, SLOT(slotMessage(const QByteArray)));
80
81 // Log File
82 // --------
83 QIODevice::OpenMode oMode;
84 if (Qt::CheckState(settings.value("fileAppend").toInt()) == Qt::Checked) {
85 oMode = QIODevice::WriteOnly | QIODevice::Unbuffered | QIODevice::Append;
86 }
87 else {
88 oMode = QIODevice::WriteOnly | QIODevice::Unbuffered;
89 }
90
91 QString logFileName = settings.value("logFile").toString();
92 if (logFileName.isEmpty()) {
93 _logFile = 0;
94 _logStream = 0;
95 }
96 else {
97 _logFile = new QFile(logFileName);
98 if (_logFile->open(oMode)) {
99 _logStream = new QTextStream(_logFile);
100 }
101 else {
102 _logStream = 0;
103 }
104 }
105
106 // RINEX writer
107 // ------------
108 if ( settings.value("rnxPath").toString().isEmpty() ) {
109 _rnx = 0;
110 }
111 else {
112 QString prep = "BNS";
113 QString ext = ".clk";
114 QString path = settings.value("rnxPath").toString();
115 QString intr = settings.value("rnxIntr").toString();
116 int sampl = settings.value("rnxSampl").toInt();
117 _rnx = new bnsRinex(prep, ext, path, intr, sampl);
118 }
119
120 // SP3 writer
121 // ----------
122 if ( settings.value("sp3Path").toString().isEmpty() ) {
123 _sp3 = 0;
124 }
125 else {
126 QString prep = "BNS";
127 QString ext = ".sp3";
128 QString path = settings.value("sp3Path").toString();
129 QString intr = settings.value("sp3Intr").toString();
130 int sampl = settings.value("sp3Sampl").toInt();
131 _sp3 = new bnsSP3(prep, ext, path, intr, sampl);
132 }
133}
134
135// Destructor
136////////////////////////////////////////////////////////////////////////////
137t_bns::~t_bns() {
138 deleteBnsEph();
139 delete _clkServer;
140 delete _clkSocket;
141 for (int ic = 0; ic < _caster.size(); ic++) {
142 delete _caster.at(ic);
143 }
144 delete _logStream;
145 delete _logFile;
146 QMapIterator<QString, t_ephPair*> it(_ephList);
147 while (it.hasNext()) {
148 it.next();
149 delete it.value();
150 }
151 delete _rnx;
152 delete _sp3;
153}
154
155// Delete bns thread
156////////////////////////////////////////////////////////////////////////////
157void t_bns::deleteBnsEph() {
158 if (_bnseph) {
159 _bnseph->terminate();
160 _bnseph->wait(100);
161 delete _bnseph;
162 _bnseph = 0;
163 }
164}
165
166// Write a Program Message
167////////////////////////////////////////////////////////////////////////////
168void t_bns::slotMessage(const QByteArray msg) {
169 if (_logStream) {
170 *_logStream << msg << endl;
171 _logStream->flush();
172 }
173 emit(newMessage(msg));
174}
175
176// Write a Program Message
177////////////////////////////////////////////////////////////////////////////
178void t_bns::slotError(const QByteArray msg) {
179 if (_logStream) {
180 *_logStream << msg << endl;
181 _logStream->flush();
182 }
183 deleteBnsEph();
184 emit(error(msg));
185}
186
187// New Connection
188////////////////////////////////////////////////////////////////////////////
189void t_bns::slotNewConnection() {
190 slotMessage("t_bns::slotNewConnection");
191 delete _clkSocket;
192 _clkSocket = _clkServer->nextPendingConnection();
193}
194
195//
196////////////////////////////////////////////////////////////////////////////
197void t_bns::slotNewEph(t_eph* ep, int nBytes) {
198
199 QMutexLocker locker(&_mutex);
200
201 emit(newEphBytes(nBytes));
202
203 t_ephPair* pair;
204 if ( !_ephList.contains(ep->prn()) ) {
205 pair = new t_ephPair();
206 _ephList.insert(ep->prn(), pair);
207 }
208 else {
209 pair = _ephList[ep->prn()];
210 }
211
212 if (pair->eph == 0) {
213 pair->eph = ep;
214 }
215 else {
216 if (ep->isNewerThan(pair->eph)) {
217 delete pair->oldEph;
218 pair->oldEph = pair->eph;
219 pair->eph = ep;
220 }
221 else {
222 delete ep;
223 }
224 }
225}
226
227// Start
228////////////////////////////////////////////////////////////////////////////
229void t_bns::run() {
230
231 slotMessage("============ Start BNS ============");
232
233 // Start Thread that retrieves broadcast Ephemeris
234 // -----------------------------------------------
235 _bnseph->start();
236
237 // Endless loop
238 // ------------
239 while (true) {
240
241 if (_clkSocket && _clkSocket->thread() != currentThread()) {
242 emit(moveSocket(currentThread()));
243 }
244
245 if (_clkSocket && _clkSocket->state() == QAbstractSocket::ConnectedState) {
246 if ( _clkSocket->canReadLine()) {
247 readEpoch();
248 }
249 else {
250 _clkSocket->waitForReadyRead(10);
251 }
252 }
253 else {
254 msleep(10);
255 }
256 }
257}
258
259//
260////////////////////////////////////////////////////////////////////////////
261void t_bns::readEpoch() {
262
263 QByteArray line = _clkSocket->readLine();
264
265 emit(newClkBytes(line.length()));
266
267 if (line.indexOf('*') == -1) {
268 return;
269 }
270
271 QTextStream in(line);
272
273 QString hlp;
274 int GPSweek, numSat;
275 double GPSweeks;
276
277 in >> hlp >> GPSweek >> GPSweeks >> numSat;
278
279 if (numSat > 0) {
280
281 QStringList prns;
282
283 for (int ic = 0; ic < _caster.size(); ic++) {
284 _caster.at(ic)->open();
285
286 for (int oldEph = 0; oldEph <= 0; oldEph++) { // TODO: handle old ephemeris
287
288 struct ClockOrbit co;
289 memset(&co, 0, sizeof(co));
290 co.GPSEpochTime = (int)GPSweeks;
291 co.GLONASSEpochTime = (int)fmod(GPSweeks, 86400.0);
292 co.ClockDataSupplied = 1;
293 co.OrbitDataSupplied = 1;
294 co.SatRefPoint = POINT_CENTER;
295 co.SatRefDatum = DATUM_ITRF;
296
297 for (int ii = 1; ii <= numSat; ii++) {
298
299 QString prn;
300 ColumnVector xx(5);
301 t_eph* ep = 0;
302
303 if (oldEph == 0) {
304 line = _clkSocket->readLine();
305 QTextStream in(line);
306 in >> prn;
307 prns << prn;
308 if ( _ephList.contains(prn) ) {
309 in >> xx(1) >> xx(2) >> xx(3) >> xx(4) >> xx(5); xx(4) *= 1e-6;
310
311 //// beg test (zero clock correction for Gerhard Wuebbena)
312 //// xx(4) -= xx(5) / 299792458.0;
313 //// end test
314
315 t_ephPair* pair = _ephList[prn];
316 pair->xx = xx;
317 ep = pair->eph;
318 }
319 }
320 else {
321 prn = prns[ii-1];
322 if ( _ephList.contains(prn) ) {
323 t_ephPair* pair = _ephList[prn];
324 prn = pair->eph->prn();
325 xx = pair->xx;
326 ep = pair->oldEph;
327 }
328 }
329
330 if (ep != 0) {
331 struct ClockOrbit::SatData* sd = 0;
332 if (prn[0] == 'G') {
333 sd = co.Sat + co.NumberOfGPSSat;
334 ++co.NumberOfGPSSat;
335 }
336 else if (prn[0] == 'R') {
337 sd = co.Sat + CLOCKORBIT_NUMGPS + co.NumberOfGLONASSSat;
338 ++co.NumberOfGLONASSSat;
339 }
340 if (sd) {
341 QString outLine;
342 processSatellite(oldEph, _caster.at(ic)->crdTrafo(), ep, GPSweek,
343 GPSweeks, prn, xx, sd, outLine);
344 _caster.at(ic)->printAscii(line);
345 }
346 }
347 }
348
349 if ( _caster.at(ic)->used() &&
350 (co.NumberOfGPSSat > 0 || co.NumberOfGLONASSSat > 0) ) {
351 char obuffer[CLOCKORBIT_BUFFERSIZE];
352 int len = MakeClockOrbit(&co, COTYPE_AUTO, 0, obuffer, sizeof(obuffer));
353 if (len > 0) {
354 emit(newOutBytes(len));
355 _caster.at(ic)->write(obuffer, len);
356 }
357 }
358 }
359 }
360 }
361}
362
363//
364////////////////////////////////////////////////////////////////////////////
365void t_bns::processSatellite(int oldEph, bool trafo, t_eph* ep, int GPSweek,
366 double GPSweeks, const QString& prn,
367 const ColumnVector& xx,
368 struct ClockOrbit::SatData* sd,
369 QString& outLine) {
370
371 ColumnVector xB(4);
372 ColumnVector vv(3);
373
374 ep->position(GPSweek, GPSweeks, xB, vv);
375
376 ColumnVector xyz = xx.Rows(1,3);
377 if (trafo) {
378 crdTrafo(GPSweek, xyz);
379 }
380
381 ColumnVector dx = xyz - xB.Rows(1,3);
382
383 double dClk = (xx(4) - xB(4)) * 299792458.0;
384 ColumnVector rsw(3);
385
386 XYZ_to_RSW(xB.Rows(1,3), vv, dx, rsw);
387
388 if (sd) {
389 sd->ID = prn.mid(1).toInt();
390 sd->IOD = ep->IOD();
391 sd->Clock.DeltaA0 = dClk;
392 sd->Orbit.DeltaRadial = rsw(1);
393 sd->Orbit.DeltaAlongTrack = rsw(2);
394 sd->Orbit.DeltaCrossTrack = rsw(3);
395 }
396
397 char oldCh = (oldEph ? '!' : ' ');
398 outLine.sprintf("%c %d %.1f %s %3d %10.3f %8.3f %8.3f %8.3f\n",
399 oldCh, GPSweek, GPSweeks, ep->prn().toAscii().data(),
400 ep->IOD(), dClk, rsw(1), rsw(2), rsw(3));
401
402 if (!oldEph) {
403 if (_rnx) {
404 _rnx->write(GPSweek, GPSweeks, prn, xx);
405 }
406 if (_sp3) {
407 _sp3->write(GPSweek, GPSweeks, prn, xx);
408 }
409 }
410}
411
412//
413////////////////////////////////////////////////////////////////////////////
414void t_bns::slotMoveSocket(QThread* tt) {
415 _clkSocket->setParent(0);
416 _clkSocket->moveToThread(tt);
417 slotMessage("bns::slotMoveSocket");
418}
419
420// Transform Coordinates IGS -> ETRF
421////////////////////////////////////////////////////////////////////////////
422void t_bns::crdTrafo(int GPSWeek, ColumnVector& xyz) {
423
424 ColumnVector dx(3);
425 dx(1) = 0.054;
426 dx(2) = 0.051;
427 dx(3) = -0.048;
428 const static double arcSec = 180.0 * 3600.0 / M_PI;
429 const static double ox = 0.000081 / arcSec;
430 const static double oy = 0.000490 / arcSec;
431 const static double oz = -0.000792 / arcSec;
432
433 Matrix rMat(3,3); rMat = 0.0;
434 rMat(1,2) = -oz;
435 rMat(1,3) = oy;
436 rMat(2,1) = oz;
437 rMat(2,3) = -ox;
438 rMat(3,1) = -oy;
439 rMat(3,2) = ox;
440
441 // Current epoch minus 1989.0 in years
442 // ------------------------------------
443 double dt = (GPSWeek - 469.0) / 365.2422 * 7.0;
444
445 xyz += dx + dt * rMat * xyz;
446}
Note: See TracBrowser for help on using the repository browser.