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

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

* empty log message *
File size: 13.9 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 // Read the first line (if not already read)
295 // -----------------------------------------
296 if (_clkLine.indexOf('*') == -1) {
297 _clkLine = _clkSocket->readLine();
298 if (_echoStream) {
299 *_echoStream << _clkLine;
300 _echoStream->flush();
301 }
302 emit(newClkBytes(_clkLine.length()));
303 }
304
305 if (_clkLine.indexOf('*') == -1) {
306 return;
307 }
308
309 QTextStream in(_clkLine);
310
311 QString hlp;
312 int year, month, day, hour, min;
313 double sec;
314 in >> hlp >> year >> month >> day >> hour >> min >> sec;
315
316 int GPSweek;
317 double GPSweeks;
318
319 GPSweekFromYMDhms(year, month, day, hour, min, sec, GPSweek, GPSweeks);
320
321 QStringList prns;
322
323 // Loop over all satellites
324 // ------------------------
325 QStringList lines;
326 for (;;) {
327 if (!_clkSocket->canReadLine()) {
328 break;
329 }
330 _clkLine = _clkSocket->readLine();
331 if (_echoStream) {
332 *_echoStream << _clkLine;
333 _echoStream->flush();
334 }
335 if (_clkLine[0] == '*') {
336 return;
337 }
338 if (_clkLine[0] == 'P') {
339 _clkLine.remove(0,1);
340 lines.push_back(_clkLine);
341 }
342 }
343
344 if (lines.size() > 0) {
345
346 QStringList prns;
347
348 for (int ic = 0; ic < _caster.size(); ic++) {
349 _caster.at(ic)->open();
350
351 for (int oldEph = 0; oldEph <= 0; oldEph++) { // TODO: handle old ephemeris
352
353 struct ClockOrbit co;
354 memset(&co, 0, sizeof(co));
355 co.GPSEpochTime = (int)GPSweeks;
356 co.GLONASSEpochTime = (int)fmod(GPSweeks, 86400.0);
357 co.ClockDataSupplied = 1;
358 co.OrbitDataSupplied = 1;
359 co.SatRefPoint = POINT_CENTER;
360 co.SatRefDatum = DATUM_ITRF;
361
362 for (int ii = 0; ii < lines.size(); ii++) {
363
364 QString prn;
365 ColumnVector xx(5);
366 t_eph* ep = 0;
367
368 if (oldEph == 0 && ic == 0) {
369 QTextStream in(lines[ii].toAscii());
370 in >> prn;
371 prns << prn;
372 if ( _ephList.contains(prn) ) {
373 in >> xx(1) >> xx(2) >> xx(3) >> xx(4);
374 xx(1) *= 1e3;
375 xx(2) *= 1e3;
376 xx(3) *= 1e3;
377 xx(4) *= 1e-6;
378
379 t_ephPair* pair = _ephList[prn];
380 pair->xx = xx;
381 ep = pair->eph;
382 }
383 }
384 else {
385 prn = prns[ii];
386 if ( _ephList.contains(prn) ) {
387 t_ephPair* pair = _ephList[prn];
388 prn = pair->eph->prn();
389 xx = pair->xx;
390 if (oldEph) {
391 ep = pair->oldEph;
392 }
393 else {
394 ep = pair->eph;
395 }
396 }
397 }
398
399 if (ep != 0) {
400 struct ClockOrbit::SatData* sd = 0;
401 if (prn[0] == 'G') {
402 sd = co.Sat + co.NumberOfGPSSat;
403 ++co.NumberOfGPSSat;
404 }
405 else if (prn[0] == 'R') {
406 sd = co.Sat + CLOCKORBIT_NUMGPS + co.NumberOfGLONASSSat;
407 ++co.NumberOfGLONASSSat;
408 }
409 if (sd) {
410 QString outLine;
411 processSatellite(oldEph, ic, _caster.at(ic)->crdTrafo(), ep,
412 GPSweek, GPSweeks, prn, xx, sd, outLine);
413 _caster.at(ic)->printAscii(outLine);
414 }
415 }
416 }
417
418 if ( _caster.at(ic)->usedSocket() &&
419 (co.NumberOfGPSSat > 0 || co.NumberOfGLONASSSat > 0) ) {
420 char obuffer[CLOCKORBIT_BUFFERSIZE];
421 int len = MakeClockOrbit(&co, COTYPE_AUTO, 0, obuffer, sizeof(obuffer));
422 if (len > 0) {
423 if (_caster.at(ic)->ic() == 1) { emit(newOutBytes1(len));}
424 if (_caster.at(ic)->ic() == 2) { emit(newOutBytes2(len));}
425 _caster.at(ic)->write(obuffer, len);
426 }
427 }
428 }
429 }
430 }
431}
432
433//
434////////////////////////////////////////////////////////////////////////////
435void t_bns::processSatellite(int oldEph, int iCaster, bool trafo, t_eph* ep,
436 int GPSweek, double GPSweeks, const QString& prn,
437 const ColumnVector& xx,
438 struct ClockOrbit::SatData* sd,
439 QString& outLine) {
440
441 ColumnVector xB(4);
442 ColumnVector vv(3);
443
444 ep->position(GPSweek, GPSweeks, xB, vv);
445
446 ColumnVector xyz = xx.Rows(1,3);
447 if (trafo) {
448 crdTrafo(GPSweek, xyz);
449 }
450
451 ColumnVector dx = xyz - xB.Rows(1,3);
452
453 double dClk = (xx(4) - xB(4)) * 299792458.0;
454 ColumnVector rsw(3);
455
456 XYZ_to_RSW(xB.Rows(1,3), vv, dx, rsw);
457
458 if (sd) {
459 sd->ID = prn.mid(1).toInt();
460 sd->IOD = ep->IOD();
461 sd->Clock.DeltaA0 = dClk;
462 sd->Orbit.DeltaRadial = rsw(1);
463 sd->Orbit.DeltaAlongTrack = rsw(2);
464 sd->Orbit.DeltaCrossTrack = rsw(3);
465 }
466
467 char oldCh = (oldEph ? '!' : ' ');
468 outLine.sprintf("%c %d %.1f %s %3d %10.3f %8.3f %8.3f %8.3f\n",
469 oldCh, GPSweek, GPSweeks, ep->prn().toAscii().data(),
470 ep->IOD(), dClk, rsw(1), rsw(2), rsw(3));
471
472 if (!oldEph && iCaster == 0) {
473 if (_rnx) {
474 _rnx->write(GPSweek, GPSweeks, prn, xx);
475 }
476 if (_sp3) {
477 _sp3->write(GPSweek, GPSweeks, prn, xx);
478 }
479 }
480}
481
482//
483////////////////////////////////////////////////////////////////////////////
484void t_bns::slotMoveSocket(QThread* tt) {
485 _clkSocket->setParent(0);
486 _clkSocket->moveToThread(tt);
487//slotMessage("bns::slotMoveSocket");
488 slotMessage("Clocks & orbits port: Socket moved to thread"); // weber
489}
490
491// Transform Coordinates IGS -> ETRF
492////////////////////////////////////////////////////////////////////////////
493void t_bns::crdTrafo(int GPSWeek, ColumnVector& xyz) {
494
495 // Current epoch minus 2000.0 in years
496 // ------------------------------------
497 double dt = (GPSWeek - (1042.0+6.0/7.0)) / 365.2422 * 7.0;
498
499 ColumnVector dx(3);
500 dx(1) = 0.0541 - dt * 0.0002;
501 dx(2) = 0.0502 + dt * 0.0001;
502 dx(3) = -0.0538 - dt * 0.0018;
503
504 static const double arcSec = 180.0 * 3600.0 / M_PI;
505 double ox = ( 0.000891 + dt * 0.000081) / arcSec;
506 double oy = ( 0.005390 + dt * 0.000490) / arcSec;
507 double oz = (-0.008712 - dt * 0.000792) / arcSec;
508
509 double sc = 1.0 + 0.4e-9 + dt * 0.08e-9;
510
511 Matrix rMat(3,3);
512 rMat(1,1) = 1.0;
513 rMat(1,2) = -oz;
514 rMat(1,3) = oy;
515 rMat(2,1) = oz;
516 rMat(2,2) = 1.0;
517 rMat(2,3) = -ox;
518 rMat(3,1) = -oy;
519 rMat(3,2) = ox;
520 rMat(3,3) = 1.0;
521
522
523 xyz = sc * rMat * xyz + dx;
524}
Note: See TracBrowser for help on using the repository browser.