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

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

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