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

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

* empty log message *
File size: 14.1 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 if ( Qt::CheckState(settings.value("beClocks1").toInt()) == Qt::Checked) {
382 xx(4) -= xx(5) / 299792458.0;
383 }
384
385 t_ephPair* pair = _ephList[prn];
386 pair->xx = xx;
387 ep = pair->eph;
388 }
389 }
390 else {
391 prn = prns[ii];
392 if ( _ephList.contains(prn) ) {
393 t_ephPair* pair = _ephList[prn];
394 prn = pair->eph->prn();
395 xx = pair->xx;
396 if (oldEph) {
397 ep = pair->oldEph;
398 }
399 else {
400 ep = pair->eph;
401 }
402 }
403 }
404
405 if (ep != 0) {
406 struct ClockOrbit::SatData* sd = 0;
407 if (prn[0] == 'G') {
408 sd = co.Sat + co.NumberOfGPSSat;
409 ++co.NumberOfGPSSat;
410 }
411 else if (prn[0] == 'R') {
412 sd = co.Sat + CLOCKORBIT_NUMGPS + co.NumberOfGLONASSSat;
413 ++co.NumberOfGLONASSSat;
414 }
415 if (sd) {
416 QString outLine;
417 processSatellite(oldEph, ic, _caster.at(ic)->crdTrafo(), ep,
418 GPSweek, GPSweeks, prn, xx, sd, outLine);
419 _caster.at(ic)->printAscii(outLine);
420 }
421 }
422 }
423
424 if ( _caster.at(ic)->usedSocket() &&
425 (co.NumberOfGPSSat > 0 || co.NumberOfGLONASSSat > 0) ) {
426 char obuffer[CLOCKORBIT_BUFFERSIZE];
427 int len = MakeClockOrbit(&co, COTYPE_AUTO, 0, obuffer, sizeof(obuffer));
428 if (len > 0) {
429 if (_caster.at(ic)->ic() == 1) { emit(newOutBytes1(len));}
430 if (_caster.at(ic)->ic() == 2) { emit(newOutBytes2(len));}
431 _caster.at(ic)->write(obuffer, len);
432 }
433 }
434 }
435 }
436 }
437}
438
439//
440////////////////////////////////////////////////////////////////////////////
441void t_bns::processSatellite(int oldEph, int iCaster, bool trafo, t_eph* ep,
442 int GPSweek, double GPSweeks, const QString& prn,
443 const ColumnVector& xx,
444 struct ClockOrbit::SatData* sd,
445 QString& outLine) {
446
447 ColumnVector xB(4);
448 ColumnVector vv(3);
449
450 ep->position(GPSweek, GPSweeks, xB, vv);
451
452 ColumnVector xyz = xx.Rows(1,3);
453 if (trafo) {
454 crdTrafo(GPSweek, xyz);
455 }
456
457 ColumnVector dx = xyz - xB.Rows(1,3);
458
459 double dClk = (xx(4) - xB(4)) * 299792458.0;
460 ColumnVector rsw(3);
461
462 XYZ_to_RSW(xB.Rows(1,3), vv, dx, rsw);
463
464 if (sd) {
465 sd->ID = prn.mid(1).toInt();
466 sd->IOD = ep->IOD();
467 sd->Clock.DeltaA0 = dClk;
468 sd->Orbit.DeltaRadial = rsw(1);
469 sd->Orbit.DeltaAlongTrack = rsw(2);
470 sd->Orbit.DeltaCrossTrack = rsw(3);
471 }
472
473 char oldCh = (oldEph ? '!' : ' ');
474 outLine.sprintf("%c %d %.1f %s %3d %10.3f %8.3f %8.3f %8.3f\n",
475 oldCh, GPSweek, GPSweeks, ep->prn().toAscii().data(),
476 ep->IOD(), dClk, rsw(1), rsw(2), rsw(3));
477
478 if (!oldEph && iCaster == 0) {
479 if (_rnx) {
480 _rnx->write(GPSweek, GPSweeks, prn, xx);
481 }
482 if (_sp3) {
483 _sp3->write(GPSweek, GPSweeks, prn, xx);
484 }
485 }
486}
487
488//
489////////////////////////////////////////////////////////////////////////////
490void t_bns::slotMoveSocket(QThread* tt) {
491 _clkSocket->setParent(0);
492 _clkSocket->moveToThread(tt);
493//slotMessage("bns::slotMoveSocket");
494 slotMessage("Clocks & orbits port: Socket moved to thread"); // weber
495}
496
497// Transform Coordinates IGS -> ETRF
498////////////////////////////////////////////////////////////////////////////
499void t_bns::crdTrafo(int GPSWeek, ColumnVector& xyz) {
500
501 // Current epoch minus 2000.0 in years
502 // ------------------------------------
503 double dt = (GPSWeek - (1042.0+6.0/7.0)) / 365.2422 * 7.0;
504
505 ColumnVector dx(3);
506 dx(1) = 0.0541 - dt * 0.0002;
507 dx(2) = 0.0502 + dt * 0.0001;
508 dx(3) = -0.0538 - dt * 0.0018;
509
510 static const double arcSec = 180.0 * 3600.0 / M_PI;
511 double ox = ( 0.000891 + dt * 0.000081) / arcSec;
512 double oy = ( 0.005390 + dt * 0.000490) / arcSec;
513 double oz = (-0.008712 - dt * 0.000792) / arcSec;
514
515 double sc = 1.0 + 0.4e-9 + dt * 0.08e-9;
516
517 Matrix rMat(3,3);
518 rMat(1,1) = 1.0;
519 rMat(1,2) = -oz;
520 rMat(1,3) = oy;
521 rMat(2,1) = oz;
522 rMat(2,2) = 1.0;
523 rMat(2,3) = -ox;
524 rMat(3,1) = -oy;
525 rMat(3,2) = ox;
526 rMat(3,3) = 1.0;
527
528
529 xyz = sc * rMat * xyz + dx;
530}
Note: See TracBrowser for help on using the repository browser.