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

Last change on this file since 1673 was 1673, checked in by weber, 15 years ago

* empty log message *
File size: 14.4 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#include "bnssettings.h"
26
27using namespace std;
28
29// Constructor
30////////////////////////////////////////////////////////////////////////////
31t_bns::t_bns(QObject* parent) : QThread(parent) {
32
33 this->setTerminationEnabled(true);
34
35 connect(this, SIGNAL(moveSocket(QThread*)),
36 this, SLOT(slotMoveSocket(QThread*)));
37
38 bnsSettings settings;
39
40 // Set Proxy (application-wide)
41 // ----------------------------
42 QString proxyHost = settings.value("proxyHost").toString();
43 int proxyPort = settings.value("proxyPort").toInt();
44
45 QNetworkProxy proxy;
46 if (proxyHost.isEmpty()) {
47 proxy.setType(QNetworkProxy::NoProxy);
48 }
49 else {
50 proxy.setType(QNetworkProxy::Socks5Proxy);
51 proxy.setHostName(proxyHost);
52 proxy.setPort(proxyPort);
53 }
54 QNetworkProxy::setApplicationProxy(proxy);
55
56 // Thread that handles broadcast ephemeris
57 // ---------------------------------------
58 _bnseph = new t_bnseph(parent);
59
60 connect(_bnseph, SIGNAL(newEph(t_eph*, int)),
61 this, SLOT(slotNewEph(t_eph*, int)));
62 connect(_bnseph, SIGNAL(newMessage(QByteArray)),
63 this, SLOT(slotMessage(const QByteArray)));
64 connect(_bnseph, SIGNAL(error(QByteArray)),
65 this, SLOT(slotError(const QByteArray)));
66
67 // Server listening for rtnet results
68 // ----------------------------------
69 _clkSocket = 0;
70 _clkServer = new QTcpServer;
71 _clkServer->listen(QHostAddress::Any, settings.value("clkPort").toInt());
72 connect(_clkServer, SIGNAL(newConnection()),this, SLOT(slotNewConnection()));
73
74 // Socket and file for outputting the results
75 // -------------------------------------------
76 for (int ic = 1; ic <= 2; ic++) {
77
78 QString mountpoint = settings.value(QString("mountpoint_%1").arg(ic)).toString();
79 QString outFileName = settings.value(QString("outFile_%1").arg(ic)).toString();
80 if (!mountpoint.isEmpty() || !outFileName.isEmpty()) {
81 QString refSys = settings.value(QString("refSys_%1").arg(ic)).toString();
82
83 _caster.push_back(new t_bnscaster(mountpoint, outFileName, refSys, ic));
84 connect(_caster.back(), SIGNAL(error(const QByteArray)),
85 this, SLOT(slotError(const QByteArray)));
86 connect(_caster.back(), SIGNAL(newMessage(const QByteArray)),
87 this, SLOT(slotMessage(const QByteArray)));
88 }
89 }
90
91 // Log File
92 // --------
93 QIODevice::OpenMode oMode;
94 if (Qt::CheckState(settings.value("fileAppend").toInt()) == Qt::Checked) {
95 oMode = QIODevice::WriteOnly | QIODevice::Unbuffered | QIODevice::Append;
96 }
97 else {
98 oMode = QIODevice::WriteOnly | QIODevice::Unbuffered;
99 }
100
101 QString logFileName = settings.value("logFile").toString();
102 if (logFileName.isEmpty()) {
103 _logFile = 0;
104 _logStream = 0;
105 }
106 else {
107 _logFile = new QFile(logFileName);
108 if (_logFile->open(oMode)) {
109 _logStream = new QTextStream(_logFile);
110 }
111 else {
112 _logStream = 0;
113 }
114 }
115
116 // Echo input from RTNet into a file
117 // ---------------------------------
118 QString echoFileName = settings.value("inpEcho").toString();
119 if (echoFileName.isEmpty()) {
120 _echoFile = 0;
121 _echoStream = 0;
122 }
123 else {
124 _echoFile = new QFile(echoFileName);
125 if (_echoFile->open(oMode)) {
126 _echoStream = new QTextStream(_echoFile);
127 }
128 else {
129 _echoStream = 0;
130 }
131 }
132
133 // RINEX writer
134 // ------------
135 if ( settings.value("rnxPath").toString().isEmpty() ) {
136 _rnx = 0;
137 }
138 else {
139 QString prep = "BNS";
140 QString ext = ".clk";
141 QString path = settings.value("rnxPath").toString();
142 QString intr = settings.value("rnxIntr").toString();
143 int sampl = settings.value("rnxSampl").toInt();
144 _rnx = new bnsRinex(prep, ext, path, intr, sampl);
145 }
146
147 // SP3 writer
148 // ----------
149 if ( settings.value("sp3Path").toString().isEmpty() ) {
150 _sp3 = 0;
151 }
152 else {
153 QString prep = "BNS";
154 QString ext = ".sp3";
155 QString path = settings.value("sp3Path").toString();
156 QString intr = settings.value("sp3Intr").toString();
157 int sampl = settings.value("sp3Sampl").toInt();
158 _sp3 = new bnsSP3(prep, ext, path, intr, sampl);
159 }
160}
161
162// Destructor
163////////////////////////////////////////////////////////////////////////////
164t_bns::~t_bns() {
165 deleteBnsEph();
166 delete _clkServer;
167 delete _clkSocket;
168 for (int ic = 0; ic < _caster.size(); ic++) {
169 delete _caster.at(ic);
170 }
171 delete _logStream;
172 delete _logFile;
173 delete _echoStream;
174 delete _echoFile;
175 QMapIterator<QString, t_ephPair*> it(_ephList);
176 while (it.hasNext()) {
177 it.next();
178 delete it.value();
179 }
180 delete _rnx;
181 delete _sp3;
182}
183
184// Delete bns thread
185////////////////////////////////////////////////////////////////////////////
186void t_bns::deleteBnsEph() {
187 if (_bnseph) {
188 _bnseph->terminate();
189 _bnseph->wait(100);
190 delete _bnseph;
191 _bnseph = 0;
192 }
193}
194
195// Write a Program Message
196////////////////////////////////////////////////////////////////////////////
197void t_bns::slotMessage(const QByteArray msg) {
198 if (_logStream) {
199 QString txt = QDateTime::currentDateTime().toUTC().toString("yy-MM-dd hh:mm:ss ");
200 *_logStream << txt << msg << endl;
201 _logStream->flush();
202 }
203 emit(newMessage(msg));
204}
205
206// Write a Program Message
207////////////////////////////////////////////////////////////////////////////
208void t_bns::slotError(const QByteArray msg) {
209 if (_logStream) {
210 *_logStream << msg << endl;
211 _logStream->flush();
212 }
213 deleteBnsEph();
214 emit(error(msg));
215}
216
217// New Connection
218////////////////////////////////////////////////////////////////////////////
219void t_bns::slotNewConnection() {
220//slotMessage("t_bns::slotNewConnection");
221 slotMessage("Clocks & orbits port: Waiting for client to connect"); // weber
222 delete _clkSocket;
223 _clkSocket = _clkServer->nextPendingConnection();
224}
225
226//
227////////////////////////////////////////////////////////////////////////////
228void t_bns::slotNewEph(t_eph* ep, int nBytes) {
229
230 QMutexLocker locker(&_mutex);
231
232 emit(newEphBytes(nBytes));
233
234 t_ephPair* pair;
235 if ( !_ephList.contains(ep->prn()) ) {
236 pair = new t_ephPair();
237 _ephList.insert(ep->prn(), pair);
238 }
239 else {
240 pair = _ephList[ep->prn()];
241 }
242
243 if (pair->eph == 0) {
244 pair->eph = ep;
245 }
246 else {
247 if (ep->isNewerThan(pair->eph)) {
248 delete pair->oldEph;
249 pair->oldEph = pair->eph;
250 pair->eph = ep;
251 }
252 else {
253 delete ep;
254 }
255 }
256}
257
258// Start
259////////////////////////////////////////////////////////////////////////////
260void t_bns::run() {
261
262 slotMessage("============ Start BNS ============");
263
264 // Start Thread that retrieves broadcast Ephemeris
265 // -----------------------------------------------
266 _bnseph->start();
267
268 // Endless loop
269 // ------------
270 while (true) {
271
272 if (_clkSocket && _clkSocket->thread() != currentThread()) {
273 emit(moveSocket(currentThread()));
274 }
275
276 if (_clkSocket && _clkSocket->state() == QAbstractSocket::ConnectedState) {
277 if ( _clkSocket->canReadLine()) {
278 readEpoch();
279 }
280 else {
281 _clkSocket->waitForReadyRead(10);
282 }
283 }
284 else {
285 msleep(10);
286 }
287 }
288}
289
290//
291////////////////////////////////////////////////////////////////////////////
292void t_bns::readEpoch() {
293
294 bnsSettings settings;
295
296 // Read the first line (if not already read)
297 // -----------------------------------------
298 if (_clkLine.indexOf('*') == -1) {
299 _clkLine = _clkSocket->readLine();
300 if (_echoStream) {
301 *_echoStream << _clkLine;
302 _echoStream->flush();
303 }
304 emit(newClkBytes(_clkLine.length()));
305 }
306
307 if (_clkLine.indexOf('*') == -1) {
308 return;
309 }
310
311 QTextStream in(_clkLine);
312
313 QString hlp;
314 int year, month, day, hour, min;
315 double sec;
316 in >> hlp >> year >> month >> day >> hour >> min >> sec;
317
318 int GPSweek;
319 double GPSweeks;
320
321 GPSweekFromYMDhms(year, month, day, hour, min, sec, GPSweek, GPSweeks);
322
323 QStringList prns;
324
325 // Loop over all satellites
326 // ------------------------
327 QStringList lines;
328 for (;;) {
329 if (!_clkSocket->canReadLine()) {
330 break;
331 }
332 _clkLine = _clkSocket->readLine();
333 if (_echoStream) {
334 *_echoStream << _clkLine;
335 _echoStream->flush();
336 }
337 if (_clkLine[0] == '*') {
338 return;
339 }
340 if (_clkLine[0] == 'P') {
341 _clkLine.remove(0,1);
342 lines.push_back(_clkLine);
343 }
344 }
345
346 if (lines.size() > 0) {
347
348 QStringList prns;
349
350 for (int ic = 0; ic < _caster.size(); ic++) {
351 _caster.at(ic)->open();
352
353 for (int oldEph = 0; oldEph <= 0; oldEph++) { // TODO: handle old ephemeris
354
355 struct ClockOrbit co;
356 memset(&co, 0, sizeof(co));
357 co.GPSEpochTime = (int)GPSweeks;
358 co.GLONASSEpochTime = (int)fmod(GPSweeks, 86400.0);
359 co.ClockDataSupplied = 1;
360 co.OrbitDataSupplied = 1;
361 co.SatRefPoint = POINT_CENTER;
362 co.SatRefDatum = DATUM_ITRF;
363
364 for (int ii = 0; ii < lines.size(); ii++) {
365
366 QString prn;
367 ColumnVector xx(5);
368 t_eph* ep = 0;
369
370 if (oldEph == 0 && ic == 0) {
371 QTextStream in(lines[ii].toAscii());
372 in >> prn;
373 prns << prn;
374 if ( _ephList.contains(prn) ) {
375 in >> xx(1) >> xx(2) >> xx(3) >> xx(4);
376 xx(1) *= 1e3;
377 xx(2) *= 1e3;
378 xx(3) *= 1e3;
379 xx(4) *= 1e-6;
380
381 //// in >> xx(1) >> xx(2) >> xx(3) >> xx(4) >> xx(5); xx(4) *= 1e-6;
382 ////
383 //// beg test (zero clock correction for Gerhard Wuebbena)
384 //// xx(4) -= xx(5) / 299792458.0;
385 //// end test
386 ////
387 //// Falls Clocks aus den Broadcast Ephemeris gewuenscht:
388 ////
389 //// if ( Qt::CheckState(settings.value("beClocks1 oder beClocks2").toInt()) == Qt::Checked) {
390 //// .....
391 //// }
392
393 t_ephPair* pair = _ephList[prn];
394 pair->xx = xx;
395 ep = pair->eph;
396 }
397 }
398 else {
399 prn = prns[ii];
400 if ( _ephList.contains(prn) ) {
401 t_ephPair* pair = _ephList[prn];
402 prn = pair->eph->prn();
403 xx = pair->xx;
404 if (oldEph) {
405 ep = pair->oldEph;
406 }
407 else {
408 ep = pair->eph;
409 }
410 }
411 }
412
413 if (ep != 0) {
414 struct ClockOrbit::SatData* sd = 0;
415 if (prn[0] == 'G') {
416 sd = co.Sat + co.NumberOfGPSSat;
417 ++co.NumberOfGPSSat;
418 }
419 else if (prn[0] == 'R') {
420 sd = co.Sat + CLOCKORBIT_NUMGPS + co.NumberOfGLONASSSat;
421 ++co.NumberOfGLONASSSat;
422 }
423 if (sd) {
424 QString outLine;
425 processSatellite(oldEph, ic, _caster.at(ic)->crdTrafo(), ep,
426 GPSweek, GPSweeks, prn, xx, sd, outLine);
427 _caster.at(ic)->printAscii(outLine);
428 }
429 }
430 }
431
432 if ( _caster.at(ic)->usedSocket() &&
433 (co.NumberOfGPSSat > 0 || co.NumberOfGLONASSSat > 0) ) {
434 char obuffer[CLOCKORBIT_BUFFERSIZE];
435 int len = MakeClockOrbit(&co, COTYPE_AUTO, 0, obuffer, sizeof(obuffer));
436 if (len > 0) {
437 if (_caster.at(ic)->ic() == 1) { emit(newOutBytes1(len));}
438 if (_caster.at(ic)->ic() == 2) { emit(newOutBytes2(len));}
439 _caster.at(ic)->write(obuffer, len);
440 }
441 }
442 }
443 }
444 }
445}
446
447//
448////////////////////////////////////////////////////////////////////////////
449void t_bns::processSatellite(int oldEph, int iCaster, bool trafo, t_eph* ep,
450 int GPSweek, double GPSweeks, const QString& prn,
451 const ColumnVector& xx,
452 struct ClockOrbit::SatData* sd,
453 QString& outLine) {
454
455 ColumnVector xB(4);
456 ColumnVector vv(3);
457
458 ep->position(GPSweek, GPSweeks, xB, vv);
459
460 ColumnVector xyz = xx.Rows(1,3);
461 if (trafo) {
462 crdTrafo(GPSweek, xyz);
463 }
464
465 ColumnVector dx = xyz - xB.Rows(1,3);
466
467 double dClk = (xx(4) - xB(4)) * 299792458.0;
468 ColumnVector rsw(3);
469
470 XYZ_to_RSW(xB.Rows(1,3), vv, dx, rsw);
471
472 if (sd) {
473 sd->ID = prn.mid(1).toInt();
474 sd->IOD = ep->IOD();
475 sd->Clock.DeltaA0 = dClk;
476 sd->Orbit.DeltaRadial = rsw(1);
477 sd->Orbit.DeltaAlongTrack = rsw(2);
478 sd->Orbit.DeltaCrossTrack = rsw(3);
479 }
480
481 char oldCh = (oldEph ? '!' : ' ');
482 outLine.sprintf("%c %d %.1f %s %3d %10.3f %8.3f %8.3f %8.3f\n",
483 oldCh, GPSweek, GPSweeks, ep->prn().toAscii().data(),
484 ep->IOD(), dClk, rsw(1), rsw(2), rsw(3));
485
486 if (!oldEph && iCaster == 0) {
487 if (_rnx) {
488 _rnx->write(GPSweek, GPSweeks, prn, xx);
489 }
490 if (_sp3) {
491 _sp3->write(GPSweek, GPSweeks, prn, xx);
492 }
493 }
494}
495
496//
497////////////////////////////////////////////////////////////////////////////
498void t_bns::slotMoveSocket(QThread* tt) {
499 _clkSocket->setParent(0);
500 _clkSocket->moveToThread(tt);
501//slotMessage("bns::slotMoveSocket");
502 slotMessage("Clocks & orbits port: Socket moved to thread"); // weber
503}
504
505// Transform Coordinates IGS -> ETRF
506////////////////////////////////////////////////////////////////////////////
507void t_bns::crdTrafo(int GPSWeek, ColumnVector& xyz) {
508
509 // Current epoch minus 2000.0 in years
510 // ------------------------------------
511 double dt = (GPSWeek - (1042.0+6.0/7.0)) / 365.2422 * 7.0;
512
513 ColumnVector dx(3);
514 dx(1) = 0.0541 - dt * 0.0002;
515 dx(2) = 0.0502 + dt * 0.0001;
516 dx(3) = -0.0538 - dt * 0.0018;
517
518 static const double arcSec = 180.0 * 3600.0 / M_PI;
519 double ox = ( 0.000891 + dt * 0.000081) / arcSec;
520 double oy = ( 0.005390 + dt * 0.000490) / arcSec;
521 double oz = (-0.008712 - dt * 0.000792) / arcSec;
522
523 double sc = 1.0 + 0.4e-9 + dt * 0.08e-9;
524
525 Matrix rMat(3,3);
526 rMat(1,1) = 1.0;
527 rMat(1,2) = -oz;
528 rMat(1,3) = oy;
529 rMat(2,1) = oz;
530 rMat(2,2) = 1.0;
531 rMat(2,3) = -ox;
532 rMat(3,1) = -oy;
533 rMat(3,2) = ox;
534 rMat(3,3) = 1.0;
535
536
537 xyz = sc * rMat * xyz + dx;
538}
Note: See TracBrowser for help on using the repository browser.