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

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

* empty log message *
File size: 11.5 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 // Thread that handles broadcast ephemeris
38 // ---------------------------------------
39 _bnseph = new t_bnseph(parent);
40
41 connect(_bnseph, SIGNAL(newEph(t_eph*)), this, SLOT(slotNewEph(t_eph*)));
42 connect(_bnseph, SIGNAL(newMessage(QByteArray)),
43 this, SLOT(slotMessage(const QByteArray)));
44 connect(_bnseph, SIGNAL(error(QByteArray)),
45 this, SLOT(slotError(const QByteArray)));
46
47 // Server listening for rtnet results
48 // ----------------------------------
49 QSettings settings;
50 _clkSocket = 0;
51 _clkServer = new QTcpServer;
52 _clkServer->listen(QHostAddress::Any, settings.value("clkPort").toInt());
53 connect(_clkServer, SIGNAL(newConnection()),this, SLOT(slotNewConnection()));
54
55 // Socket and file for outputting the results
56 // -------------------------------------------
57 _outSocket = 0;
58 _outSocketOpenTrial = 0;
59
60 QString outFileName = settings.value("outFile").toString();
61 if (outFileName.isEmpty()) {
62 _outFile = 0;
63 _outStream = 0;
64 }
65 else {
66 _outFile = new QFile(outFileName);
67 if (_outFile->open(QIODevice::WriteOnly | QIODevice::Unbuffered)) {
68 _outStream = new QTextStream(_outFile);
69 }
70 }
71
72 // Log File
73 // --------
74 QString logFileName = settings.value("logFile").toString();
75 if (logFileName.isEmpty()) {
76 _logFile = 0;
77 _logStream = 0;
78 }
79 else {
80 _logFile = new QFile(logFileName);
81 if (_logFile->open(QIODevice::WriteOnly | QIODevice::Unbuffered)) {
82 _logStream = new QTextStream(_logFile);
83 }
84 else {
85 _logStream = 0;
86 }
87 }
88
89 // RINEX writer
90 // ------------
91 if ( settings.value("rnxPath").toString().isEmpty() ) {
92 _rnx = 0;
93 }
94 else {
95 QString prep = "BNS";
96 QString ext = ".clk";
97 QString path = settings.value("rnxPath").toString();
98 QString intr = settings.value("rnxIntr").toString();
99 int sampl = settings.value("rnxSampl").toInt();
100 _rnx = new bnsRinex(prep, ext, path, intr, sampl);
101 }
102
103 // SP3 writer
104 // ----------
105 if ( settings.value("sp3Path").toString().isEmpty() ) {
106 _sp3 = 0;
107 }
108 else {
109 QString prep = "BNS";
110 QString ext = ".sp3";
111 QString path = settings.value("sp3Path").toString();
112 QString intr = settings.value("sp3Intr").toString();
113 int sampl = settings.value("sp3Sampl").toInt();
114 _sp3 = new bnsSP3(prep, ext, path, intr, sampl);
115 }
116}
117
118// Destructor
119////////////////////////////////////////////////////////////////////////////
120t_bns::~t_bns() {
121 deleteBnsEph();
122 delete _clkServer;
123 delete _clkSocket;
124 delete _outSocket;
125 delete _outStream;
126 delete _logStream;
127 delete _outFile;
128 delete _logFile;
129 QMapIterator<QString, t_ephPair*> it(_ephList);
130 while (it.hasNext()) {
131 it.next();
132 delete it.value();
133 }
134 delete _rnx;
135 delete _sp3;
136}
137
138// Delete bns thread
139////////////////////////////////////////////////////////////////////////////
140void t_bns::deleteBnsEph() {
141 if (_bnseph) {
142 _bnseph->terminate();
143 _bnseph->wait(100);
144 delete _bnseph;
145 _bnseph = 0;
146 }
147}
148
149// Write a Program Message
150////////////////////////////////////////////////////////////////////////////
151void t_bns::slotMessage(const QByteArray msg) {
152 if (_logStream) {
153 *_logStream << msg << endl;
154 _logStream->flush();
155 }
156 emit(newMessage(msg));
157}
158
159// Write a Program Message
160////////////////////////////////////////////////////////////////////////////
161void t_bns::slotError(const QByteArray msg) {
162 if (_logStream) {
163 *_logStream << msg << endl;
164 _logStream->flush();
165 }
166 deleteBnsEph();
167 emit(error(msg));
168}
169
170// New Connection
171////////////////////////////////////////////////////////////////////////////
172void t_bns::slotNewConnection() {
173 slotMessage("t_bns::slotNewConnection");
174 delete _clkSocket;
175 _clkSocket = _clkServer->nextPendingConnection();
176}
177
178// Start the Communication with NTRIP Caster
179////////////////////////////////////////////////////////////////////////////
180void t_bns::openCaster() {
181
182 delete _outSocket; _outSocket = 0;
183
184 double minDt = exp2(_outSocketOpenTrial);
185 if (++_outSocketOpenTrial > 8) {
186 _outSocketOpenTrial = 8;
187 }
188 if (_outSocketOpenTime.isValid() &&
189 _outSocketOpenTime.secsTo(QDateTime::currentDateTime()) < minDt) {
190 return;
191 }
192 else {
193 _outSocketOpenTime = QDateTime::currentDateTime();
194 }
195
196 QSettings settings;
197 _outSocket = new QTcpSocket();
198 _outSocket->connectToHost(settings.value("outHost").toString(),
199 settings.value("outPort").toInt());
200
201 const int timeOut = 100; // 0.1 seconds
202 if (!_outSocket->waitForConnected(timeOut)) {
203 delete _outSocket;
204 _outSocket = 0;
205 emit(error("bns::openCaster Connect Timeout"));
206 return;
207 }
208
209 QString mountpoint = settings.value("mountpoint").toString();
210 QString password = settings.value("password").toString();
211
212 QByteArray msg = "SOURCE " + password.toAscii() + " /" +
213 mountpoint.toAscii() + "\r\n" +
214 "Source-Agent: NTRIP BNS/1.0\r\n\r\n";
215
216 _outSocket->write(msg);
217 _outSocket->waitForBytesWritten();
218
219 _outSocket->waitForReadyRead();
220 QByteArray ans = _outSocket->readLine();
221
222 if (ans.indexOf("OK") == -1) {
223 delete _outSocket;
224 _outSocket = 0;
225 slotMessage("bns::openCaster socket deleted");
226 }
227 else {
228 slotMessage("bns::openCaster socket OK");
229 _outSocketOpenTrial = 0;
230 }
231}
232
233//
234////////////////////////////////////////////////////////////////////////////
235void t_bns::slotNewEph(t_eph* ep) {
236
237 QMutexLocker locker(&_mutex);
238
239 t_ephPair* pair;
240 if ( !_ephList.contains(ep->prn()) ) {
241 pair = new t_ephPair();
242 _ephList.insert(ep->prn(), pair);
243 }
244 else {
245 pair = _ephList[ep->prn()];
246 }
247
248 if (pair->eph == 0) {
249 pair->eph = ep;
250 }
251 else {
252 if (ep->isNewerThan(pair->eph)) {
253 delete pair->oldEph;
254 pair->oldEph = pair->eph;
255 pair->eph = ep;
256 }
257 else {
258 delete ep;
259 }
260 }
261}
262
263// Start
264////////////////////////////////////////////////////////////////////////////
265void t_bns::run() {
266
267 slotMessage("============ Start BNS ============");
268
269 // Start Thread that retrieves broadcast Ephemeris
270 // -----------------------------------------------
271 _bnseph->start();
272
273 // Endless loop
274 // ------------
275 while (true) {
276
277 if (_clkSocket && _clkSocket->thread() != currentThread()) {
278 emit(moveSocket(currentThread()));
279 }
280
281 if (_clkSocket && _clkSocket->state() == QAbstractSocket::ConnectedState) {
282 if ( _clkSocket->canReadLine()) {
283 if (_outSocket == 0 ||
284 _outSocket->state() != QAbstractSocket::ConnectedState) {
285 openCaster();
286 }
287 readEpoch();
288 }
289 else {
290 _clkSocket->waitForReadyRead(10);
291 }
292 }
293 else {
294 msleep(10);
295 }
296 }
297}
298
299//
300////////////////////////////////////////////////////////////////////////////
301void t_bns::readEpoch() {
302
303 QByteArray line = _clkSocket->readLine();
304
305 if (line.indexOf('*') == -1) {
306 return;
307 }
308
309 QTextStream in(line);
310
311 QString hlp;
312 int GPSweek, numSat;
313 double GPSweeks;
314
315 in >> hlp >> GPSweek >> GPSweeks >> numSat;
316
317 if (numSat > 0) {
318
319 QStringList prns;
320
321 /// for (int oldEph = 0; oldEph <= 1; oldEph++) {
322 for (int oldEph = 0; oldEph <= 0; oldEph++) {
323
324 struct ClockOrbit co;
325 memset(&co, 0, sizeof(co));
326 co.GPSEpochTime = (int)GPSweeks;
327 co.GLONASSEpochTime = (int)fmod(GPSweeks, 86400.0);
328 co.ClockDataSupplied = 1;
329 co.OrbitDataSupplied = 1;
330 co.SatRefPoint = POINT_CENTER;
331 co.SatRefDatum = DATUM_ITRF;
332
333 for (int ii = 1; ii <= numSat; ii++) {
334
335 QString prn;
336 ColumnVector xx(5);
337 t_eph* ep = 0;
338
339 if (oldEph == 0) {
340 line = _clkSocket->readLine();
341 QTextStream in(line);
342 in >> prn;
343 prns << prn;
344 if ( _ephList.contains(prn) ) {
345 in >> xx(1) >> xx(2) >> xx(3) >> xx(4) >> xx(5); xx(4) *= 1e-6;
346
347 //// beg test (zero clock correction for Gerhard Wuebbena)
348 //// xx(4) -= xx(5) / 299792458.0;
349 //// end test
350
351 t_ephPair* pair = _ephList[prn];
352 pair->xx = xx;
353 ep = pair->eph;
354 }
355 }
356 else {
357 prn = prns[ii-1];
358 if ( _ephList.contains(prn) ) {
359 t_ephPair* pair = _ephList[prn];
360 prn = pair->eph->prn();
361 xx = pair->xx;
362 ep = pair->oldEph;
363 }
364 }
365
366 if (ep != 0) {
367 struct ClockOrbit::SatData* sd = 0;
368 if (prn[0] == 'G') {
369 sd = co.Sat + co.NumberOfGPSSat;
370 ++co.NumberOfGPSSat;
371 }
372 else if (prn[0] == 'R') {
373 sd = co.Sat + CLOCKORBIT_NUMGPS + co.NumberOfGLONASSSat;
374 ++co.NumberOfGLONASSSat;
375 }
376 if (sd) {
377 processSatellite(oldEph, ep, GPSweek, GPSweeks, prn, xx, sd);
378 }
379 }
380 }
381
382 if ( _outSocket &&
383 (co.NumberOfGPSSat > 0 || co.NumberOfGLONASSSat > 0) ) {
384 char obuffer[CLOCKORBIT_BUFFERSIZE];
385 int len = MakeClockOrbit(&co, COTYPE_AUTO, 0, obuffer, sizeof(obuffer));
386 if (len > 0) {
387 _outSocket->write(obuffer, len);
388 _outSocket->flush();
389 }
390 }
391 }
392 }
393}
394
395//
396////////////////////////////////////////////////////////////////////////////
397void t_bns::processSatellite(int oldEph, t_eph* ep, int GPSweek, double GPSweeks,
398 const QString& prn, const ColumnVector& xx,
399 struct ClockOrbit::SatData* sd) {
400
401 ColumnVector xB(4);
402 ColumnVector vv(3);
403
404 ep->position(GPSweek, GPSweeks, xB, vv);
405
406 ColumnVector dx = xx.Rows(1,3) - xB.Rows(1,3);
407
408 double dClk = (xx(4) - xB(4)) * 299792458.0;
409 ColumnVector rsw(3);
410
411 XYZ_to_RSW(xB.Rows(1,3), vv, dx, rsw);
412
413 if (sd) {
414 sd->ID = prn.mid(1).toInt();
415 sd->IOD = ep->IOD();
416 sd->Clock.DeltaA0 = dClk;
417 sd->Orbit.DeltaRadial = rsw(1);
418 sd->Orbit.DeltaAlongTrack = rsw(2);
419 sd->Orbit.DeltaCrossTrack = rsw(3);
420 }
421
422 if (_outStream) {
423 QString line;
424 char oldCh = (oldEph ? '!' : ' ');
425 line.sprintf("%c %d %.1f %s %3d %10.3f %8.3f %8.3f %8.3f\n",
426 oldCh, GPSweek, GPSweeks, ep->prn().toAscii().data(),
427 ep->IOD(), dClk, rsw(1), rsw(2), rsw(3));
428 *_outStream << line;
429 _outStream->flush();
430 }
431 if (!oldEph) {
432 if (_rnx) {
433 _rnx->write(GPSweek, GPSweeks, prn, xx);
434 }
435 if (_sp3) {
436 _sp3->write(GPSweek, GPSweeks, prn, xx);
437 }
438 }
439}
440
441//
442////////////////////////////////////////////////////////////////////////////
443void t_bns::slotMoveSocket(QThread* tt) {
444 _clkSocket->setParent(0);
445 _clkSocket->moveToThread(tt);
446 slotMessage("bns::slotMoveSocket");
447}
Note: See TracBrowser for help on using the repository browser.