[756] | 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 |
|
---|
[858] | 17 | #include <math.h>
|
---|
[756] | 18 | #include <iostream>
|
---|
[800] | 19 | #include <newmatio.h>
|
---|
[756] | 20 |
|
---|
| 21 | #include "bns.h"
|
---|
[799] | 22 | #include "bnsutils.h"
|
---|
[847] | 23 | #include "bnsrinex.h"
|
---|
[848] | 24 | #include "bnssp3.h"
|
---|
[1668] | 25 | #include "bnssettings.h"
|
---|
[756] | 26 |
|
---|
| 27 | using namespace std;
|
---|
| 28 |
|
---|
| 29 | // Constructor
|
---|
| 30 | ////////////////////////////////////////////////////////////////////////////
|
---|
[757] | 31 | t_bns::t_bns(QObject* parent) : QThread(parent) {
|
---|
[760] | 32 |
|
---|
[764] | 33 | this->setTerminationEnabled(true);
|
---|
[828] | 34 |
|
---|
[836] | 35 | connect(this, SIGNAL(moveSocket(QThread*)),
|
---|
| 36 | this, SLOT(slotMoveSocket(QThread*)));
|
---|
| 37 |
|
---|
[1668] | 38 | bnsSettings settings;
|
---|
[979] | 39 |
|
---|
| 40 | // Set Proxy (application-wide)
|
---|
| 41 | // ----------------------------
|
---|
| 42 | QString proxyHost = settings.value("proxyHost").toString();
|
---|
| 43 | int proxyPort = settings.value("proxyPort").toInt();
|
---|
[980] | 44 |
|
---|
| 45 | QNetworkProxy proxy;
|
---|
| 46 | if (proxyHost.isEmpty()) {
|
---|
| 47 | proxy.setType(QNetworkProxy::NoProxy);
|
---|
| 48 | }
|
---|
| 49 | else {
|
---|
[979] | 50 | proxy.setType(QNetworkProxy::Socks5Proxy);
|
---|
| 51 | proxy.setHostName(proxyHost);
|
---|
| 52 | proxy.setPort(proxyPort);
|
---|
| 53 | }
|
---|
[980] | 54 | QNetworkProxy::setApplicationProxy(proxy);
|
---|
[979] | 55 |
|
---|
[828] | 56 | // Thread that handles broadcast ephemeris
|
---|
| 57 | // ---------------------------------------
|
---|
| 58 | _bnseph = new t_bnseph(parent);
|
---|
[827] | 59 |
|
---|
[1059] | 60 | connect(_bnseph, SIGNAL(newEph(t_eph*, int)),
|
---|
[1058] | 61 | this, SLOT(slotNewEph(t_eph*, int)));
|
---|
[828] | 62 | connect(_bnseph, SIGNAL(newMessage(QByteArray)),
|
---|
| 63 | this, SLOT(slotMessage(const QByteArray)));
|
---|
| 64 | connect(_bnseph, SIGNAL(error(QByteArray)),
|
---|
| 65 | this, SLOT(slotError(const QByteArray)));
|
---|
[760] | 66 |
|
---|
[827] | 67 | // Server listening for rtnet results
|
---|
| 68 | // ----------------------------------
|
---|
[828] | 69 | _clkSocket = 0;
|
---|
[827] | 70 | _clkServer = new QTcpServer;
|
---|
| 71 | _clkServer->listen(QHostAddress::Any, settings.value("clkPort").toInt());
|
---|
[828] | 72 | connect(_clkServer, SIGNAL(newConnection()),this, SLOT(slotNewConnection()));
|
---|
[827] | 73 |
|
---|
[828] | 74 | // Socket and file for outputting the results
|
---|
| 75 | // -------------------------------------------
|
---|
[1067] | 76 | for (int ic = 1; ic <= 2; ic++) {
|
---|
[827] | 77 |
|
---|
[1069] | 78 | QString mountpoint = settings.value(QString("mountpoint_%1").arg(ic)).toString();
|
---|
[1123] | 79 | QString outFileName = settings.value(QString("outFile_%1").arg(ic)).toString();
|
---|
| 80 | if (!mountpoint.isEmpty() || !outFileName.isEmpty()) {
|
---|
[1069] | 81 | QString refSys = settings.value(QString("refSys_%1").arg(ic)).toString();
|
---|
[1067] | 82 |
|
---|
[1249] | 83 | _caster.push_back(new t_bnscaster(mountpoint, outFileName, refSys, ic));
|
---|
[1068] | 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 | }
|
---|
[1067] | 89 | }
|
---|
| 90 |
|
---|
[1065] | 91 | // Log File
|
---|
| 92 | // --------
|
---|
[983] | 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 |
|
---|
[816] | 101 | QString logFileName = settings.value("logFile").toString();
|
---|
| 102 | if (logFileName.isEmpty()) {
|
---|
[947] | 103 | _logFile = 0;
|
---|
| 104 | _logStream = 0;
|
---|
[812] | 105 | }
|
---|
[816] | 106 | else {
|
---|
| 107 | _logFile = new QFile(logFileName);
|
---|
[983] | 108 | if (_logFile->open(oMode)) {
|
---|
[816] | 109 | _logStream = new QTextStream(_logFile);
|
---|
| 110 | }
|
---|
[948] | 111 | else {
|
---|
| 112 | _logStream = 0;
|
---|
| 113 | }
|
---|
[816] | 114 | }
|
---|
[847] | 115 |
|
---|
[1072] | 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 |
|
---|
[847] | 133 | // RINEX writer
|
---|
| 134 | // ------------
|
---|
| 135 | if ( settings.value("rnxPath").toString().isEmpty() ) {
|
---|
| 136 | _rnx = 0;
|
---|
| 137 | }
|
---|
| 138 | else {
|
---|
[850] | 139 | QString prep = "BNS";
|
---|
[857] | 140 | QString ext = ".clk";
|
---|
[850] | 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);
|
---|
[847] | 145 | }
|
---|
[848] | 146 |
|
---|
| 147 | // SP3 writer
|
---|
| 148 | // ----------
|
---|
| 149 | if ( settings.value("sp3Path").toString().isEmpty() ) {
|
---|
| 150 | _sp3 = 0;
|
---|
| 151 | }
|
---|
| 152 | else {
|
---|
[850] | 153 | QString prep = "BNS";
|
---|
[857] | 154 | QString ext = ".sp3";
|
---|
[850] | 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);
|
---|
[848] | 159 | }
|
---|
[756] | 160 | }
|
---|
| 161 |
|
---|
| 162 | // Destructor
|
---|
| 163 | ////////////////////////////////////////////////////////////////////////////
|
---|
[757] | 164 | t_bns::~t_bns() {
|
---|
[763] | 165 | deleteBnsEph();
|
---|
[769] | 166 | delete _clkServer;
|
---|
[837] | 167 | delete _clkSocket;
|
---|
[1066] | 168 | for (int ic = 0; ic < _caster.size(); ic++) {
|
---|
| 169 | delete _caster.at(ic);
|
---|
| 170 | }
|
---|
[816] | 171 | delete _logStream;
|
---|
[812] | 172 | delete _logFile;
|
---|
[1072] | 173 | delete _echoStream;
|
---|
| 174 | delete _echoFile;
|
---|
[779] | 175 | QMapIterator<QString, t_ephPair*> it(_ephList);
|
---|
| 176 | while (it.hasNext()) {
|
---|
| 177 | it.next();
|
---|
| 178 | delete it.value();
|
---|
| 179 | }
|
---|
[849] | 180 | delete _rnx;
|
---|
| 181 | delete _sp3;
|
---|
[756] | 182 | }
|
---|
| 183 |
|
---|
[763] | 184 | // Delete bns thread
|
---|
| 185 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 186 | void t_bns::deleteBnsEph() {
|
---|
| 187 | if (_bnseph) {
|
---|
| 188 | _bnseph->terminate();
|
---|
[764] | 189 | _bnseph->wait(100);
|
---|
[763] | 190 | delete _bnseph;
|
---|
| 191 | _bnseph = 0;
|
---|
| 192 | }
|
---|
| 193 | }
|
---|
| 194 |
|
---|
[756] | 195 | // Write a Program Message
|
---|
| 196 | ////////////////////////////////////////////////////////////////////////////
|
---|
[758] | 197 | void t_bns::slotMessage(const QByteArray msg) {
|
---|
[816] | 198 | if (_logStream) {
|
---|
[1217] | 199 | QString txt = QDateTime::currentDateTime().toUTC().toString("yy-MM-dd hh:mm:ss ");
|
---|
| 200 | *_logStream << txt << msg << endl;
|
---|
[818] | 201 | _logStream->flush();
|
---|
[812] | 202 | }
|
---|
[757] | 203 | emit(newMessage(msg));
|
---|
[756] | 204 | }
|
---|
| 205 |
|
---|
[760] | 206 | // Write a Program Message
|
---|
| 207 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 208 | void t_bns::slotError(const QByteArray msg) {
|
---|
[816] | 209 | if (_logStream) {
|
---|
| 210 | *_logStream << msg << endl;
|
---|
[818] | 211 | _logStream->flush();
|
---|
[812] | 212 | }
|
---|
[763] | 213 | deleteBnsEph();
|
---|
[760] | 214 | emit(error(msg));
|
---|
| 215 | }
|
---|
| 216 |
|
---|
[769] | 217 | // New Connection
|
---|
| 218 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 219 | void t_bns::slotNewConnection() {
|
---|
[1208] | 220 | //slotMessage("t_bns::slotNewConnection");
|
---|
| 221 | slotMessage("Clocks & orbits port: Waiting for client to connect"); // weber
|
---|
[787] | 222 | delete _clkSocket;
|
---|
[769] | 223 | _clkSocket = _clkServer->nextPendingConnection();
|
---|
| 224 | }
|
---|
| 225 |
|
---|
[784] | 226 | //
|
---|
| 227 | ////////////////////////////////////////////////////////////////////////////
|
---|
[1058] | 228 | void t_bns::slotNewEph(t_eph* ep, int nBytes) {
|
---|
[784] | 229 |
|
---|
| 230 | QMutexLocker locker(&_mutex);
|
---|
| 231 |
|
---|
[1058] | 232 | emit(newEphBytes(nBytes));
|
---|
| 233 |
|
---|
[784] | 234 | t_ephPair* pair;
|
---|
[884] | 235 | if ( !_ephList.contains(ep->prn()) ) {
|
---|
[784] | 236 | pair = new t_ephPair();
|
---|
[884] | 237 | _ephList.insert(ep->prn(), pair);
|
---|
[784] | 238 | }
|
---|
| 239 | else {
|
---|
[884] | 240 | pair = _ephList[ep->prn()];
|
---|
[784] | 241 | }
|
---|
| 242 |
|
---|
| 243 | if (pair->eph == 0) {
|
---|
| 244 | pair->eph = ep;
|
---|
| 245 | }
|
---|
| 246 | else {
|
---|
[884] | 247 | if (ep->isNewerThan(pair->eph)) {
|
---|
[784] | 248 | delete pair->oldEph;
|
---|
| 249 | pair->oldEph = pair->eph;
|
---|
| 250 | pair->eph = ep;
|
---|
| 251 | }
|
---|
| 252 | else {
|
---|
| 253 | delete ep;
|
---|
| 254 | }
|
---|
| 255 | }
|
---|
| 256 | }
|
---|
| 257 |
|
---|
[756] | 258 | // Start
|
---|
| 259 | ////////////////////////////////////////////////////////////////////////////
|
---|
[757] | 260 | void t_bns::run() {
|
---|
[769] | 261 |
|
---|
[758] | 262 | slotMessage("============ Start BNS ============");
|
---|
[770] | 263 |
|
---|
[828] | 264 | // Start Thread that retrieves broadcast Ephemeris
|
---|
| 265 | // -----------------------------------------------
|
---|
[758] | 266 | _bnseph->start();
|
---|
[769] | 267 |
|
---|
[770] | 268 | // Endless loop
|
---|
| 269 | // ------------
|
---|
[769] | 270 | while (true) {
|
---|
[836] | 271 |
|
---|
| 272 | if (_clkSocket && _clkSocket->thread() != currentThread()) {
|
---|
| 273 | emit(moveSocket(currentThread()));
|
---|
| 274 | }
|
---|
| 275 |
|
---|
[796] | 276 | if (_clkSocket && _clkSocket->state() == QAbstractSocket::ConnectedState) {
|
---|
| 277 | if ( _clkSocket->canReadLine()) {
|
---|
| 278 | readEpoch();
|
---|
| 279 | }
|
---|
[809] | 280 | else {
|
---|
| 281 | _clkSocket->waitForReadyRead(10);
|
---|
| 282 | }
|
---|
[769] | 283 | }
|
---|
| 284 | else {
|
---|
[794] | 285 | msleep(10);
|
---|
[769] | 286 | }
|
---|
| 287 | }
|
---|
[756] | 288 | }
|
---|
| 289 |
|
---|
[778] | 290 | //
|
---|
| 291 | ////////////////////////////////////////////////////////////////////////////
|
---|
[784] | 292 | void t_bns::readEpoch() {
|
---|
[778] | 293 |
|
---|
[1670] | 294 | bnsSettings settings;
|
---|
| 295 |
|
---|
[1197] | 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()));
|
---|
[1072] | 305 | }
|
---|
| 306 |
|
---|
[1197] | 307 | if (_clkLine.indexOf('*') == -1) {
|
---|
[784] | 308 | return;
|
---|
[778] | 309 | }
|
---|
| 310 |
|
---|
[1197] | 311 | QTextStream in(_clkLine);
|
---|
[784] | 312 |
|
---|
| 313 | QString hlp;
|
---|
[1197] | 314 | int year, month, day, hour, min;
|
---|
| 315 | double sec;
|
---|
| 316 | in >> hlp >> year >> month >> day >> hour >> min >> sec;
|
---|
| 317 |
|
---|
| 318 | int GPSweek;
|
---|
[798] | 319 | double GPSweeks;
|
---|
[784] | 320 |
|
---|
[1197] | 321 | GPSweekFromYMDhms(year, month, day, hour, min, sec, GPSweek, GPSweeks);
|
---|
[784] | 322 |
|
---|
[1197] | 323 | QStringList prns;
|
---|
[874] | 324 |
|
---|
[1197] | 325 | // Loop over all satellites
|
---|
| 326 | // ------------------------
|
---|
| 327 | QStringList lines;
|
---|
| 328 | for (;;) {
|
---|
| 329 | if (!_clkSocket->canReadLine()) {
|
---|
[1198] | 330 | break;
|
---|
[1197] | 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 |
|
---|
[922] | 348 | QStringList prns;
|
---|
| 349 |
|
---|
[1066] | 350 | for (int ic = 0; ic < _caster.size(); ic++) {
|
---|
| 351 | _caster.at(ic)->open();
|
---|
[927] | 352 |
|
---|
[1066] | 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 |
|
---|
[1197] | 364 | for (int ii = 0; ii < lines.size(); ii++) {
|
---|
| 365 |
|
---|
[1066] | 366 | QString prn;
|
---|
| 367 | ColumnVector xx(5);
|
---|
| 368 | t_eph* ep = 0;
|
---|
| 369 |
|
---|
[1075] | 370 | if (oldEph == 0 && ic == 0) {
|
---|
[1197] | 371 | QTextStream in(lines[ii].toAscii());
|
---|
[1066] | 372 | in >> prn;
|
---|
| 373 | prns << prn;
|
---|
| 374 | if ( _ephList.contains(prn) ) {
|
---|
[1197] | 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;
|
---|
[1670] | 380 |
|
---|
| 381 | if ( Qt::CheckState(settings.value("beClocks1").toInt()) == Qt::Checked) {
|
---|
| 382 | xx(4) -= xx(5) / 299792458.0;
|
---|
| 383 | }
|
---|
[1066] | 384 |
|
---|
| 385 | t_ephPair* pair = _ephList[prn];
|
---|
| 386 | pair->xx = xx;
|
---|
| 387 | ep = pair->eph;
|
---|
| 388 | }
|
---|
[872] | 389 | }
|
---|
[1066] | 390 | else {
|
---|
[1197] | 391 | prn = prns[ii];
|
---|
[1066] | 392 | if ( _ephList.contains(prn) ) {
|
---|
| 393 | t_ephPair* pair = _ephList[prn];
|
---|
| 394 | prn = pair->eph->prn();
|
---|
| 395 | xx = pair->xx;
|
---|
[1076] | 396 | if (oldEph) {
|
---|
| 397 | ep = pair->oldEph;
|
---|
| 398 | }
|
---|
| 399 | else {
|
---|
| 400 | ep = pair->eph;
|
---|
| 401 | }
|
---|
[1066] | 402 | }
|
---|
[873] | 403 | }
|
---|
[1066] | 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;
|
---|
[1102] | 417 | processSatellite(oldEph, ic, _caster.at(ic)->crdTrafo(), ep,
|
---|
| 418 | GPSweek, GPSweeks, prn, xx, sd, outLine);
|
---|
[1074] | 419 | _caster.at(ic)->printAscii(outLine);
|
---|
[1066] | 420 | }
|
---|
| 421 | }
|
---|
[873] | 422 | }
|
---|
[1066] | 423 |
|
---|
[1123] | 424 | if ( _caster.at(ic)->usedSocket() &&
|
---|
[1066] | 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) {
|
---|
[1262] | 429 | if (_caster.at(ic)->ic() == 1) { emit(newOutBytes1(len));}
|
---|
| 430 | if (_caster.at(ic)->ic() == 2) { emit(newOutBytes2(len));}
|
---|
[1066] | 431 | _caster.at(ic)->write(obuffer, len);
|
---|
[872] | 432 | }
|
---|
| 433 | }
|
---|
[863] | 434 | }
|
---|
| 435 | }
|
---|
[780] | 436 | }
|
---|
[778] | 437 | }
|
---|
[784] | 438 |
|
---|
| 439 | //
|
---|
| 440 | ////////////////////////////////////////////////////////////////////////////
|
---|
[1102] | 441 | void t_bns::processSatellite(int oldEph, int iCaster, bool trafo, t_eph* ep,
|
---|
| 442 | int GPSweek, double GPSweeks, const QString& prn,
|
---|
[1066] | 443 | const ColumnVector& xx,
|
---|
[1065] | 444 | struct ClockOrbit::SatData* sd,
|
---|
| 445 | QString& outLine) {
|
---|
[784] | 446 |
|
---|
[799] | 447 | ColumnVector xB(4);
|
---|
[802] | 448 | ColumnVector vv(3);
|
---|
[799] | 449 |
|
---|
[884] | 450 | ep->position(GPSweek, GPSweeks, xB, vv);
|
---|
[799] | 451 |
|
---|
[984] | 452 | ColumnVector xyz = xx.Rows(1,3);
|
---|
[1066] | 453 | if (trafo) {
|
---|
[985] | 454 | crdTrafo(GPSweek, xyz);
|
---|
[984] | 455 | }
|
---|
[927] | 456 |
|
---|
[984] | 457 | ColumnVector dx = xyz - xB.Rows(1,3);
|
---|
| 458 |
|
---|
[804] | 459 | double dClk = (xx(4) - xB(4)) * 299792458.0;
|
---|
| 460 | ColumnVector rsw(3);
|
---|
[800] | 461 |
|
---|
[806] | 462 | XYZ_to_RSW(xB.Rows(1,3), vv, dx, rsw);
|
---|
[804] | 463 |
|
---|
[863] | 464 | if (sd) {
|
---|
| 465 | sd->ID = prn.mid(1).toInt();
|
---|
[884] | 466 | sd->IOD = ep->IOD();
|
---|
[862] | 467 | sd->Clock.DeltaA0 = dClk;
|
---|
| 468 | sd->Orbit.DeltaRadial = rsw(1);
|
---|
| 469 | sd->Orbit.DeltaAlongTrack = rsw(2);
|
---|
| 470 | sd->Orbit.DeltaCrossTrack = rsw(3);
|
---|
[863] | 471 | }
|
---|
[862] | 472 |
|
---|
[1065] | 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 |
|
---|
[1102] | 478 | if (!oldEph && iCaster == 0) {
|
---|
[923] | 479 | if (_rnx) {
|
---|
| 480 | _rnx->write(GPSweek, GPSweeks, prn, xx);
|
---|
| 481 | }
|
---|
| 482 | if (_sp3) {
|
---|
| 483 | _sp3->write(GPSweek, GPSweeks, prn, xx);
|
---|
| 484 | }
|
---|
[847] | 485 | }
|
---|
[784] | 486 | }
|
---|
[836] | 487 |
|
---|
| 488 | //
|
---|
| 489 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 490 | void t_bns::slotMoveSocket(QThread* tt) {
|
---|
| 491 | _clkSocket->setParent(0);
|
---|
| 492 | _clkSocket->moveToThread(tt);
|
---|
[1209] | 493 | //slotMessage("bns::slotMoveSocket");
|
---|
| 494 | slotMessage("Clocks & orbits port: Socket moved to thread"); // weber
|
---|
[836] | 495 | }
|
---|
[984] | 496 |
|
---|
| 497 | // Transform Coordinates IGS -> ETRF
|
---|
| 498 | ////////////////////////////////////////////////////////////////////////////
|
---|
[985] | 499 | void t_bns::crdTrafo(int GPSWeek, ColumnVector& xyz) {
|
---|
[984] | 500 |
|
---|
[1243] | 501 | // Current epoch minus 2000.0 in years
|
---|
| 502 | // ------------------------------------
|
---|
| 503 | double dt = (GPSWeek - (1042.0+6.0/7.0)) / 365.2422 * 7.0;
|
---|
| 504 |
|
---|
[985] | 505 | ColumnVector dx(3);
|
---|
[1243] | 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 |
|
---|
[1100] | 510 | static const double arcSec = 180.0 * 3600.0 / M_PI;
|
---|
[1245] | 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;
|
---|
[984] | 514 |
|
---|
[1245] | 515 | double sc = 1.0 + 0.4e-9 + dt * 0.08e-9;
|
---|
[1243] | 516 |
|
---|
[1245] | 517 | Matrix rMat(3,3);
|
---|
| 518 | rMat(1,1) = 1.0;
|
---|
[985] | 519 | rMat(1,2) = -oz;
|
---|
| 520 | rMat(1,3) = oy;
|
---|
| 521 | rMat(2,1) = oz;
|
---|
[1245] | 522 | rMat(2,2) = 1.0;
|
---|
[985] | 523 | rMat(2,3) = -ox;
|
---|
| 524 | rMat(3,1) = -oy;
|
---|
| 525 | rMat(3,2) = ox;
|
---|
[1245] | 526 | rMat(3,3) = 1.0;
|
---|
[985] | 527 |
|
---|
| 528 |
|
---|
[1245] | 529 | xyz = sc * rMat * xyz + dx;
|
---|
[984] | 530 | }
|
---|