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

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

* empty log message *
File size: 10.3 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"
25extern "C" {
26#include "RTCM/clock_orbit_rtcm.h"
27}
28
29using namespace std;
30
31// Constructor
32////////////////////////////////////////////////////////////////////////////
33t_bns::t_bns(QObject* parent) : QThread(parent) {
34
35 this->setTerminationEnabled(true);
36
37 connect(this, SIGNAL(moveSocket(QThread*)),
38 this, SLOT(slotMoveSocket(QThread*)));
39
40 // Thread that handles broadcast ephemeris
41 // ---------------------------------------
42 _bnseph = new t_bnseph(parent);
43
44 connect(_bnseph, SIGNAL(newEph(gpsEph*)), this, SLOT(slotNewEph(gpsEph*)));
45 connect(_bnseph, SIGNAL(newMessage(QByteArray)),
46 this, SLOT(slotMessage(const QByteArray)));
47 connect(_bnseph, SIGNAL(error(QByteArray)),
48 this, SLOT(slotError(const QByteArray)));
49
50 // Server listening for rtnet results
51 // ----------------------------------
52 QSettings settings;
53 _clkSocket = 0;
54 _clkServer = new QTcpServer;
55 _clkServer->listen(QHostAddress::Any, settings.value("clkPort").toInt());
56 connect(_clkServer, SIGNAL(newConnection()),this, SLOT(slotNewConnection()));
57
58 // Socket and file for outputting the results
59 // -------------------------------------------
60 _outSocket = 0;
61 _outSocketOpenTrial = 0;
62
63 QString outFileName = settings.value("outFile").toString();
64 if (outFileName.isEmpty()) {
65 _outFile = 0;
66 _outStream = 0;
67 }
68 else {
69 _outFile = new QFile(outFileName);
70 if (_outFile->open(QIODevice::WriteOnly | QIODevice::Unbuffered)) {
71 _outStream = new QTextStream(_outFile);
72 }
73 }
74
75 // Log File
76 // --------
77 QString logFileName = settings.value("logFile").toString();
78 if (logFileName.isEmpty()) {
79 _logFile = 0;
80 }
81 else {
82 _logFile = new QFile(logFileName);
83 if (_logFile->open(QIODevice::WriteOnly | QIODevice::Unbuffered)) {
84 _logStream = new QTextStream(_logFile);
85 }
86 }
87
88 // RINEX writer
89 // ------------
90 if ( settings.value("rnxPath").toString().isEmpty() ) {
91 _rnx = 0;
92 }
93 else {
94 QString prep = "BNS";
95 QString ext = ".clk";
96 QString path = settings.value("rnxPath").toString();
97 QString intr = settings.value("rnxIntr").toString();
98 int sampl = settings.value("rnxSampl").toInt();
99 _rnx = new bnsRinex(prep, ext, path, intr, sampl);
100 }
101
102 // SP3 writer
103 // ----------
104 if ( settings.value("sp3Path").toString().isEmpty() ) {
105 _sp3 = 0;
106 }
107 else {
108 QString prep = "BNS";
109 QString ext = ".sp3";
110 QString path = settings.value("sp3Path").toString();
111 QString intr = settings.value("sp3Intr").toString();
112 int sampl = settings.value("sp3Sampl").toInt();
113 _sp3 = new bnsSP3(prep, ext, path, intr, sampl);
114 }
115}
116
117// Destructor
118////////////////////////////////////////////////////////////////////////////
119t_bns::~t_bns() {
120 deleteBnsEph();
121 delete _clkServer;
122 delete _clkSocket;
123 delete _outSocket;
124 delete _outStream;
125 delete _logStream;
126 delete _outFile;
127 delete _logFile;
128 QMapIterator<QString, t_ephPair*> it(_ephList);
129 while (it.hasNext()) {
130 it.next();
131 delete it.value();
132 }
133 delete _rnx;
134 delete _sp3;
135}
136
137// Delete bns thread
138////////////////////////////////////////////////////////////////////////////
139void t_bns::deleteBnsEph() {
140 if (_bnseph) {
141 _bnseph->terminate();
142 _bnseph->wait(100);
143 delete _bnseph;
144 _bnseph = 0;
145 }
146}
147
148// Write a Program Message
149////////////////////////////////////////////////////////////////////////////
150void t_bns::slotMessage(const QByteArray msg) {
151 if (_logStream) {
152 *_logStream << msg << endl;
153 _logStream->flush();
154 }
155 emit(newMessage(msg));
156}
157
158// Write a Program Message
159////////////////////////////////////////////////////////////////////////////
160void t_bns::slotError(const QByteArray msg) {
161 if (_logStream) {
162 *_logStream << msg << endl;
163 _logStream->flush();
164 }
165 deleteBnsEph();
166 emit(error(msg));
167}
168
169// New Connection
170////////////////////////////////////////////////////////////////////////////
171void t_bns::slotNewConnection() {
172 slotMessage("t_bns::slotNewConnection");
173 delete _clkSocket;
174 _clkSocket = _clkServer->nextPendingConnection();
175}
176
177// Start the Communication with NTRIP Caster
178////////////////////////////////////////////////////////////////////////////
179void t_bns::openCaster() {
180
181 delete _outSocket; _outSocket = 0;
182
183 double minDt = exp2(_outSocketOpenTrial);
184 if (++_outSocketOpenTrial > 8) {
185 _outSocketOpenTrial = 8;
186 }
187 if (_outSocketOpenTime.isValid() &&
188 _outSocketOpenTime.secsTo(QDateTime::currentDateTime()) < minDt) {
189 return;
190 }
191 else {
192 _outSocketOpenTime = QDateTime::currentDateTime();
193 }
194
195 QSettings settings;
196 _outSocket = new QTcpSocket();
197 _outSocket->connectToHost(settings.value("outHost").toString(),
198 settings.value("outPort").toInt());
199
200 const int timeOut = 100; // 0.1 seconds
201 if (!_outSocket->waitForConnected(timeOut)) {
202 delete _outSocket;
203 _outSocket = 0;
204 emit(error("bns::openCaster Connect Timeout"));
205 return;
206 }
207
208 QString mountpoint = settings.value("mountpoint").toString();
209 QString password = settings.value("password").toString();
210
211 QByteArray msg = "SOURCE " + password.toAscii() + " /" +
212 mountpoint.toAscii() + "\r\n" +
213 "Source-Agent: NTRIP BNS/1.0\r\n\r\n";
214
215 _outSocket->write(msg);
216 _outSocket->waitForBytesWritten();
217
218 _outSocket->waitForReadyRead();
219 QByteArray ans = _outSocket->readLine();
220
221 if (ans.indexOf("OK") == -1) {
222 delete _outSocket;
223 _outSocket = 0;
224 slotMessage("bns::openCaster socket deleted");
225 }
226 else {
227 slotMessage("bns::openCaster socket OK");
228 _outSocketOpenTrial = 0;
229 }
230}
231
232//
233////////////////////////////////////////////////////////////////////////////
234void t_bns::slotNewEph(gpsEph* ep) {
235
236 QMutexLocker locker(&_mutex);
237
238 t_ephPair* pair;
239 if ( !_ephList.contains(ep->prn) ) {
240 pair = new t_ephPair();
241 _ephList.insert(ep->prn, pair);
242 }
243 else {
244 pair = _ephList[ep->prn];
245 }
246
247 if (pair->eph == 0) {
248 pair->eph = ep;
249 }
250 else {
251 if (ep->GPSweek > pair->eph->GPSweek ||
252 (ep->GPSweek == pair->eph->GPSweek && ep->TOC > pair->eph->TOC)) {
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 for (int ii = 1; ii <= numSat; ii++) {
318 line = _clkSocket->readLine();
319
320 QTextStream in(line);
321
322 QString prn;
323 ColumnVector xx(4);
324
325 in >> prn >> xx(1) >> xx(2) >> xx(3) >> xx(4);
326 xx(4) *= 1e-6;
327
328 processSatellite(GPSweek, GPSweeks, prn, xx);
329 }
330}
331
332//
333////////////////////////////////////////////////////////////////////////////
334void t_bns::processSatellite(int GPSweek, double GPSweeks, const QString& prn,
335 const ColumnVector& xx) {
336
337 // No broadcast ephemeris available
338 // --------------------------------
339 if ( !_ephList.contains(prn) ) {
340 return;
341 }
342
343 t_ephPair* pair = _ephList[prn];
344 gpsEph* ep = pair->eph;
345
346 ColumnVector xB(4);
347 ColumnVector vv(3);
348
349 satellitePosition(GPSweek, GPSweeks, ep, xB(1), xB(2), xB(3), xB(4),
350 vv(1), vv(2), vv(3));
351
352 ColumnVector dx = xx.Rows(1,3) - xB.Rows(1,3);
353 double dClk = (xx(4) - xB(4)) * 299792458.0;
354 ColumnVector rsw(3);
355
356 XYZ_to_RSW(xB.Rows(1,3), vv, dx, rsw);
357
358 QString line;
359 line.sprintf("%d %.1f %s %3d %3d %8.3f %8.3f %8.3f %8.3f\n",
360 GPSweek, GPSweeks, ep->prn.toAscii().data(),
361 int(ep->IODC), int(ep->IODE), dClk, rsw(1), rsw(2), rsw(3));
362
363 if (_outStream) {
364 *_outStream << line;
365 _outStream->flush();
366 }
367 if (_outSocket) {
368 struct ClockOrbit co;
369 memset(&co, 0, sizeof(co));
370 co.GPSEpochTime = (int)GPSweeks;
371 co.ClockDataSupplied = 1;
372 co.OrbitDataSupplied = 1;
373 co.SatRefPoint = POINT_CENTER;
374 co.SatRefDatum = DATUM_ITRF;
375 co.NumberOfGPSSat = 1;
376
377 struct ClockOrbit::SatData* sd = co.Sat;
378 /// sd->ID = prn;
379 sd->IOD = int(ep->IODE);
380 sd->Clock.DeltaA0 = dClk;
381 sd->Orbit.DeltaRadial = rsw(1);
382 sd->Orbit.DeltaAlongTrack = rsw(2);
383 sd->Orbit.DeltaCrossTrack = rsw(3);
384
385 char obuffer[CLOCKORBIT_BUFFERSIZE];
386 int len = MakeClockOrbit(&co, COTYPE_AUTO, 0, obuffer, sizeof(obuffer));
387 if (len > 0) {
388 _outSocket->write(obuffer, len);
389 _outSocket->flush();
390 }
391 }
392 if (_rnx) {
393 _rnx->write(GPSweek, GPSweeks, prn, xx);
394 }
395 if (_sp3) {
396 _sp3->write(GPSweek, GPSweeks, prn, xx);
397 }
398}
399
400//
401////////////////////////////////////////////////////////////////////////////
402void t_bns::slotMoveSocket(QThread* tt) {
403 _clkSocket->setParent(0);
404 _clkSocket->moveToThread(tt);
405 slotMessage("bns::slotMoveSocket");
406}
Note: See TracBrowser for help on using the repository browser.