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

Last change on this file since 2419 was 2419, checked in by mervart, 12 years ago

* empty log message *

File size: 22.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"
25#include "bnssettings.h"
26extern "C" {
27#include "RTCM/rtcm3torinex.h"
28}
29
30using namespace std;
31
32// Error Handling
33////////////////////////////////////////////////////////////////////////////
34void RTCM3Error(const char*, ...) {
35}
36
37// Constructor
38////////////////////////////////////////////////////////////////////////////
39t_bns::t_bns(QObject* parent) : QThread(parent) {
40
41 this->setTerminationEnabled(true);
42
43 connect(this, SIGNAL(moveSocket(QThread*)),
44 this, SLOT(slotMoveSocket(QThread*)));
45
46 bnsSettings settings;
47
48 // Set Proxy (application-wide)
49 // ----------------------------
50 QString proxyHost = settings.value("proxyHost").toString();
51 int proxyPort = settings.value("proxyPort").toInt();
52
53 QNetworkProxy proxy;
54 if (proxyHost.isEmpty()) {
55 proxy.setType(QNetworkProxy::NoProxy);
56 }
57 else {
58 proxy.setType(QNetworkProxy::Socks5Proxy);
59 proxy.setHostName(proxyHost);
60 proxy.setPort(proxyPort);
61 }
62 QNetworkProxy::setApplicationProxy(proxy);
63
64 // Thread that handles broadcast ephemeris
65 // ---------------------------------------
66 _bnseph = new t_bnseph(parent);
67
68 connect(_bnseph, SIGNAL(newEph(t_eph*, int)),
69 this, SLOT(slotNewEph(t_eph*, int)));
70 connect(_bnseph, SIGNAL(newMessage(QByteArray)),
71 this, SLOT(slotMessage(const QByteArray)));
72 connect(_bnseph, SIGNAL(error(QByteArray)),
73 this, SLOT(slotError(const QByteArray)));
74
75 // Server listening for rtnet results
76 // ----------------------------------
77 _clkSocket = 0;
78 _clkServer = new QTcpServer;
79 _clkServer->listen(QHostAddress::Any, settings.value("clkPort").toInt());
80 connect(_clkServer, SIGNAL(newConnection()),this, SLOT(slotNewConnection()));
81
82 // Socket and file for outputting the results
83 // -------------------------------------------
84 for (int ic = 1; ic <= 10; ic++) {
85 QString mountpoint = settings.value(QString("mountpoint_%1").arg(ic)).toString();
86 QString outFileName = settings.value(QString("outFile_%1").arg(ic)).toString();
87 if (!mountpoint.isEmpty() || !outFileName.isEmpty()) {
88 _caster.push_back(new t_bnscaster(mountpoint, outFileName, ic));
89 connect(_caster.back(), SIGNAL(error(const QByteArray)),
90 this, SLOT(slotError(const QByteArray)));
91 connect(_caster.back(), SIGNAL(newMessage(const QByteArray)),
92 this, SLOT(slotMessage(const QByteArray)));
93 }
94 }
95
96 // Socket for outputting the Ephemerides
97 // -------------------------------------
98 QString mountpoint = settings.value("mountpoint_Eph").toString();
99 if (mountpoint.isEmpty()) {
100 _casterEph = 0;
101 }
102 else {
103 _casterEph = new t_bnscaster(mountpoint);
104 connect(_casterEph, SIGNAL(error(const QByteArray)),
105 this, SLOT(slotError(const QByteArray)));
106 connect(_casterEph, SIGNAL(newMessage(const QByteArray)),
107 this, SLOT(slotMessage(const QByteArray)));
108 }
109
110 // Log File
111 // --------
112 QIODevice::OpenMode oMode;
113 if (Qt::CheckState(settings.value("fileAppend").toInt()) == Qt::Checked) {
114 oMode = QIODevice::WriteOnly | QIODevice::Unbuffered | QIODevice::Append;
115 }
116 else {
117 oMode = QIODevice::WriteOnly | QIODevice::Unbuffered;
118 }
119
120 QString logFileName = settings.value("logFile").toString();
121 if (logFileName.isEmpty()) {
122 _logFile = 0;
123 _logStream = 0;
124 }
125 else {
126 _logFile = new QFile(logFileName);
127 if (_logFile->open(oMode)) {
128 _logStream = new QTextStream(_logFile);
129 }
130 else {
131 _logStream = 0;
132 }
133 }
134
135 // Echo input from RTNet into a file
136 // ---------------------------------
137 QString echoFileName = settings.value("inpEcho").toString();
138 if (echoFileName.isEmpty()) {
139 _echoFile = 0;
140 _echoStream = 0;
141 }
142 else {
143 _echoFile = new QFile(echoFileName);
144 if (_echoFile->open(oMode)) {
145 _echoStream = new QTextStream(_echoFile);
146 }
147 else {
148 _echoStream = 0;
149 }
150 }
151
152 // RINEX writer
153 // ------------
154 if ( settings.value("rnxPath").toString().isEmpty() ) {
155 _rnx = 0;
156 }
157 else {
158 QString prep = "BNS";
159 QString ext = ".clk";
160 QString path = settings.value("rnxPath").toString();
161 QString intr = settings.value("rnxIntr").toString();
162 int sampl = settings.value("rnxSampl").toInt();
163 _rnx = new bnsRinex(prep, ext, path, intr, sampl);
164 }
165
166 // SP3 writer
167 // ----------
168 if ( settings.value("sp3Path").toString().isEmpty() ) {
169 _sp3 = 0;
170 }
171 else {
172 QString prep = "BNS";
173 QString ext = ".sp3";
174 QString path = settings.value("sp3Path").toString();
175 QString intr = settings.value("sp3Intr").toString();
176 int sampl = settings.value("sp3Sampl").toInt();
177 _sp3 = new bnsSP3(prep, ext, path, intr, sampl);
178 }
179}
180
181// Destructor
182////////////////////////////////////////////////////////////////////////////
183t_bns::~t_bns() {
184 deleteBnsEph();
185 delete _clkServer;
186 delete _clkSocket;
187 for (int ic = 0; ic < _caster.size(); ic++) {
188 delete _caster.at(ic);
189 }
190 delete _casterEph;
191 delete _logStream;
192 delete _logFile;
193 delete _echoStream;
194 delete _echoFile;
195 QMapIterator<QString, t_ephPair*> it(_ephList);
196 while (it.hasNext()) {
197 it.next();
198 delete it.value();
199 }
200 delete _rnx;
201 delete _sp3;
202}
203
204// Delete bns thread
205////////////////////////////////////////////////////////////////////////////
206void t_bns::deleteBnsEph() {
207 if (_bnseph) {
208 _bnseph->terminate();
209 _bnseph->wait(100);
210 delete _bnseph;
211 _bnseph = 0;
212 }
213}
214
215// Write a Program Message
216////////////////////////////////////////////////////////////////////////////
217void t_bns::slotMessage(const QByteArray msg) {
218 if (_logStream) {
219 QString txt = QDateTime::currentDateTime().toUTC().toString("yy-MM-dd hh:mm:ss ");
220 *_logStream << txt << msg << endl;
221 _logStream->flush();
222 }
223 emit(newMessage(msg));
224}
225
226// Write a Program Message
227////////////////////////////////////////////////////////////////////////////
228void t_bns::slotError(const QByteArray msg) {
229 if (_logStream) {
230 *_logStream << msg << endl;
231 _logStream->flush();
232 }
233 deleteBnsEph();
234 emit(error(msg));
235}
236
237// New Connection
238////////////////////////////////////////////////////////////////////////////
239void t_bns::slotNewConnection() {
240//slotMessage("t_bns::slotNewConnection");
241 slotMessage("Clocks & orbits port: Waiting for client to connect"); // weber
242 delete _clkSocket;
243 _clkSocket = _clkServer->nextPendingConnection();
244}
245
246//
247////////////////////////////////////////////////////////////////////////////
248void t_bns::slotNewEph(t_eph* ep, int nBytes) {
249
250 QMutexLocker locker(&_mutex);
251
252 emit(newEphBytes(nBytes));
253
254 // Output Ephemerides as they are
255 // ------------------------------
256 if (_casterEph) {
257 _casterEph->open();
258 unsigned char Array[67];
259 int size = ep->RTCM3(Array);
260 if (size > 0) {
261 _casterEph->write((char*) Array, size);
262 emit(newOutEphBytes(size));
263 }
264 //// QByteArray buffer = "New Ephemeris " + ep->prn().toAscii() + "\n";
265 //// _casterEph->write(buffer.data(), buffer.length());
266 //// int len = buffer.length();
267 //// if (len > 0) {
268 //// emit(newOutEphBytes(len));
269 //// }
270 }
271
272 t_ephPair* pair;
273 if ( !_ephList.contains(ep->prn()) ) {
274 pair = new t_ephPair();
275 _ephList.insert(ep->prn(), pair);
276 }
277 else {
278 pair = _ephList[ep->prn()];
279 }
280
281 if (pair->eph == 0) {
282 pair->eph = ep;
283 }
284 else {
285 if (ep->isNewerThan(pair->eph)) {
286 delete pair->oldEph;
287 pair->oldEph = pair->eph;
288 pair->eph = ep;
289 }
290 else {
291 delete ep;
292 }
293 }
294}
295
296// Start
297////////////////////////////////////////////////////////////////////////////
298void t_bns::run() {
299
300 slotMessage("============ Start BNS ============");
301
302 // Start Thread that retrieves broadcast Ephemeris
303 // -----------------------------------------------
304 _bnseph->start();
305
306 // Endless loop
307 // ------------
308 while (true) {
309
310 if (_clkSocket && _clkSocket->thread() != currentThread()) {
311 emit(moveSocket(currentThread()));
312 }
313
314 if (_clkSocket && _clkSocket->state() == QAbstractSocket::ConnectedState) {
315 if ( _clkSocket->canReadLine()) {
316 readEpoch();
317 }
318 else {
319 _clkSocket->waitForReadyRead(10);
320 }
321 }
322 else {
323 msleep(10);
324 }
325 }
326}
327
328//
329////////////////////////////////////////////////////////////////////////////
330void t_bns::readEpoch() {
331
332 bnsSettings settings;
333
334 // Read the first line (if not already read)
335 // -----------------------------------------
336 if (_clkLine.indexOf('*') == -1) {
337 _clkLine = _clkSocket->readLine();
338 if (_echoStream) {
339 *_echoStream << _clkLine;
340 _echoStream->flush();
341 }
342 emit(newClkBytes(_clkLine.length()));
343 }
344
345 if (_clkLine.indexOf('*') == -1) {
346 return;
347 }
348
349 QTextStream in(_clkLine);
350
351 QString hlp;
352 int year, month, day, hour, min;
353 double sec;
354 in >> hlp >> year >> month >> day >> hour >> min >> sec;
355
356 int GPSweek;
357 double GPSweeks;
358
359 GPSweekFromYMDhms(year, month, day, hour, min, sec, GPSweek, GPSweeks);
360
361 QStringList prns;
362
363 // Loop over all satellites
364 // ------------------------
365 QStringList lines;
366 for (;;) {
367 if (!_clkSocket->canReadLine()) {
368 break;
369 }
370 _clkLine = _clkSocket->readLine();
371 if (_echoStream) {
372 *_echoStream << _clkLine;
373 _echoStream->flush();
374 }
375 if (_clkLine[0] == '*') {
376 return;
377 }
378 if (_clkLine[0] == 'P') {
379 _clkLine.remove(0,1);
380 lines.push_back(_clkLine);
381 }
382 }
383
384 if (lines.size() > 0) {
385
386 QStringList prns;
387
388 for (int ic = 0; ic < _caster.size(); ic++) {
389 _caster.at(ic)->open();
390
391 for (int oldEph = 0; oldEph <= 0; oldEph++) { // TODO: handle old ephemeris
392
393 struct ClockOrbit co;
394 memset(&co, 0, sizeof(co));
395 co.GPSEpochTime = (int)GPSweeks;
396 co.GLONASSEpochTime = (int)fmod(GPSweeks, 86400.0)
397 + 3 * 3600 - gnumleap(year, month, day);
398 co.ClockDataSupplied = 1;
399 co.OrbitDataSupplied = 1;
400 co.SatRefDatum = DATUM_ITRF;
401
402 struct Bias bias;
403 memset(&bias, 0, sizeof(bias));
404 bias.GPSEpochTime = (int)GPSweeks;
405 bias.GLONASSEpochTime = (int)fmod(GPSweeks, 86400.0)
406 + 3 * 3600 - gnumleap(year, month, day);
407
408 for (int ii = 0; ii < lines.size(); ii++) {
409
410 QString prn;
411 ColumnVector xx(14); xx = 0.0;
412 t_eph* ep = 0;
413
414 if (oldEph == 0 && ic == 0) {
415 QTextStream in(lines[ii].toAscii());
416 in >> prn;
417 prns << prn;
418 if ( _ephList.contains(prn) ) {
419 in >> xx(1) >> xx(2) >> xx(3) >> xx(4) >> xx(5)
420 >> xx(6) >> xx(7) >> xx(8) >> xx(9) >> xx(10)
421 >> xx(11) >> xx(12) >> xx(13) >> xx(14);
422 xx(1) *= 1e3; // x-crd
423 xx(2) *= 1e3; // y-crd
424 xx(3) *= 1e3; // z-crd
425 xx(4) *= 1e-6; // clk
426 xx(5) *= 1e-6; // rel. corr.
427 // xx(6), xx(7), xx(8) ... PhaseCent - CoM
428 // xx(9) .. P1-C1 DCB, xx(10) ... P1-P2 DCB
429 // xx(11) ... dT
430 xx(12) *= 1e3; // x-crd at time + dT
431 xx(13) *= 1e3; // y-crd at time + dT
432 xx(14) *= 1e3; // z-crd at time + dT
433
434 t_ephPair* pair = _ephList[prn];
435 pair->xx = xx;
436 ep = pair->eph;
437 }
438 }
439 else {
440 prn = prns[ii];
441 if ( _ephList.contains(prn) ) {
442 t_ephPair* pair = _ephList[prn];
443 prn = pair->eph->prn();
444 xx = pair->xx;
445 if (oldEph) {
446 ep = pair->oldEph;
447 }
448 else {
449 ep = pair->eph;
450 }
451 }
452 }
453
454 if (ep != 0) {
455 struct ClockOrbit::SatData* sd = 0;
456 if (prn[0] == 'G') {
457 sd = co.Sat + co.NumberOfGPSSat;
458 ++co.NumberOfGPSSat;
459 }
460 else if (prn[0] == 'R') {
461 sd = co.Sat + CLOCKORBIT_NUMGPS + co.NumberOfGLONASSSat;
462 ++co.NumberOfGLONASSSat;
463 }
464 if (sd) {
465 QString outLine;
466 processSatellite(oldEph, ic, _caster.at(ic)->crdTrafo(),
467 _caster.at(ic)->CoM(), ep,
468 GPSweek, GPSweeks, prn, xx, sd, outLine);
469 _caster.at(ic)->printAscii(outLine);
470 }
471
472 struct Bias::BiasSat* biasSat = 0;
473 if (prn[0] == 'G') {
474 biasSat = bias.Sat + bias.NumberOfGPSSat;
475 ++bias.NumberOfGPSSat;
476 }
477// else if (prn[0] == 'R') {
478// biasSat = bias.Sat + CLOCKORBIT_NUMGPS + bias.NumberOfGLONASSSat;
479// ++bias.NumberOfGLONASSSat;
480// }
481
482 // Coefficient of Ionosphere-Free LC
483 // ---------------------------------
484 const static double a_L1_GPS = 2.54572778;
485 const static double a_L2_GPS = -1.54572778;
486 const static double a_L1_Glo = 2.53125000;
487 const static double a_L2_Glo = -1.53125000;
488
489 if (biasSat) {
490 biasSat->ID = prn.mid(1).toInt();
491 biasSat->NumberOfCodeBiases = 3;
492 if (prn[0] == 'G') {
493 biasSat->Biases[0].Type = CODETYPEGPS_L1_Z;
494 biasSat->Biases[0].Bias = 0.0;
495 biasSat->Biases[1].Type = CODETYPEGPS_L1_CA;
496 biasSat->Biases[1].Bias = xx(9) / 0.299792458; // IGS P1C1 DCB
497 biasSat->Biases[2].Type = CODETYPEGPS_L2_Z;
498 biasSat->Biases[2].Bias = xx(10) / 0.299792458; // IGS P1P2 DCB
499 }
500 else if (prn[0] == 'R') {
501 biasSat->Biases[0].Type = CODETYPEGLONASS_L1_P;
502 biasSat->Biases[0].Bias = 0.0;
503 biasSat->Biases[1].Type = CODETYPEGLONASS_L1_CA;
504 biasSat->Biases[1].Bias = xx(9) / 0.299792458; // IGS P1C1 DCB
505 biasSat->Biases[2].Type = CODETYPEGLONASS_L2_P;
506 biasSat->Biases[2].Bias = xx(10) / 0.299792458; // IGS P1P2 DCB
507 }
508 }
509 }
510 }
511
512 if ( _caster.at(ic)->usedSocket() &&
513 (co.NumberOfGPSSat > 0 || co.NumberOfGLONASSSat > 0) ) {
514 char obuffer[CLOCKORBIT_BUFFERSIZE];
515
516 if (_caster.at(ic)->CoM()) {
517 co.SatRefPoint = POINT_CENTER;
518 }
519 else {
520 co.SatRefPoint = POINT_IONOFREE;
521 }
522
523 int len = MakeClockOrbit(&co, COTYPE_AUTO, 0, obuffer, sizeof(obuffer));
524 if (len > 0) {
525 if (_caster.at(ic)->ic() == 1) { emit(newOutBytes1(len));}
526 if (_caster.at(ic)->ic() == 2) { emit(newOutBytes2(len));}
527 if (_caster.at(ic)->ic() == 3) { emit(newOutBytes3(len));}
528 if (_caster.at(ic)->ic() == 4) { emit(newOutBytes4(len));}
529 if (_caster.at(ic)->ic() == 5) { emit(newOutBytes5(len));}
530 if (_caster.at(ic)->ic() == 6) { emit(newOutBytes6(len));}
531 if (_caster.at(ic)->ic() == 7) { emit(newOutBytes7(len));}
532 if (_caster.at(ic)->ic() == 8) { emit(newOutBytes8(len));}
533 if (_caster.at(ic)->ic() == 9) { emit(newOutBytes9(len));}
534 if (_caster.at(ic)->ic() == 10) { emit(newOutBytes10(len));}
535 _caster.at(ic)->write(obuffer, len);
536 }
537 }
538
539 if ( _caster.at(ic)->usedSocket() &&
540 (bias.NumberOfGPSSat > 0 || bias.NumberOfGLONASSSat > 0) ) {
541 char obuffer[CLOCKORBIT_BUFFERSIZE];
542 int len = MakeBias(&bias, BTYPE_AUTO, 0, obuffer, sizeof(obuffer));
543 if (len > 0) {
544 _caster.at(ic)->write(obuffer, len);
545 }
546 }
547 }
548 }
549 }
550}
551
552//
553////////////////////////////////////////////////////////////////////////////
554void t_bns::processSatellite(int oldEph, int iCaster, const QString trafo,
555 bool CoM, t_eph* ep, int GPSweek,
556 double GPSweeks, const QString& prn,
557 const ColumnVector& xx,
558 struct ClockOrbit::SatData* sd,
559 QString& outLine) {
560
561 const double secPerWeek = 7.0 * 86400.0;
562
563 ColumnVector rsw(3);
564 ColumnVector rsw2(3);
565 double dClk;
566
567 for (int ii = 1; ii <= 2; ++ii) {
568
569 int GPSweek12 = GPSweek;
570 double GPSweeks12 = GPSweeks;
571 if (ii == 2) {
572 GPSweeks12 += xx(11);
573 if (GPSweeks12 > secPerWeek) {
574 GPSweek12 += 1;
575 GPSweeks12 -= secPerWeek;
576 }
577 }
578
579 ColumnVector xB(4);
580 ColumnVector vv(3);
581
582 ep->position(GPSweek12, GPSweeks12, xB, vv);
583
584 ColumnVector xyz;
585 if (ii == 1) {
586 xyz = xx.Rows(1,3);
587 }
588 else {
589 xyz = xx.Rows(12,14);
590 }
591
592 // Correction Center of Mass -> Antenna Phase Center
593 // -------------------------------------------------
594 if (! CoM) {
595 xyz(1) += xx(6);
596 xyz(2) += xx(7);
597 xyz(3) += xx(8);
598 }
599
600 if (trafo != "IGS05") {
601 crdTrafo(GPSweek12, xyz, trafo);
602 }
603
604 ColumnVector dx = xB.Rows(1,3) - xyz ;
605
606 if (ii == 1) {
607 XYZ_to_RSW(xB.Rows(1,3), vv, dx, rsw);
608 dClk = (xx(4) + xx(5) - xB(4)) * 299792458.0;
609 }
610 else {
611 XYZ_to_RSW(xB.Rows(1,3), vv, dx, rsw2);
612 }
613 }
614
615 if (sd) {
616 sd->ID = prn.mid(1).toInt();
617 sd->IOD = ep->IOD();
618 sd->Clock.DeltaA0 = dClk;
619 sd->Orbit.DeltaRadial = rsw(1);
620 sd->Orbit.DeltaAlongTrack = rsw(2);
621 sd->Orbit.DeltaCrossTrack = rsw(3);
622 sd->Orbit.DotDeltaRadial = (rsw2(1) - rsw(1)) / xx(11);
623 sd->Orbit.DotDeltaAlongTrack = (rsw2(2) - rsw(2)) / xx(11);
624 sd->Orbit.DotDeltaCrossTrack = (rsw2(3) - rsw(3)) / xx(11);
625 }
626
627 char oldCh = (oldEph ? '!' : ' ');
628 outLine.sprintf("%c %d %.1f %s %3d %10.3f %8.3f %8.3f %8.3f\n",
629 oldCh, GPSweek, GPSweeks, ep->prn().toAscii().data(),
630 ep->IOD(), dClk, rsw(1), rsw(2), rsw(3));
631
632 if (!oldEph && iCaster == 0) {
633 if (_rnx) {
634 _rnx->write(GPSweek, GPSweeks, prn, xx);
635 }
636 if (_sp3) {
637 _sp3->write(GPSweek, GPSweeks, prn, xx);
638 }
639 }
640}
641
642//
643////////////////////////////////////////////////////////////////////////////
644void t_bns::slotMoveSocket(QThread* tt) {
645 _clkSocket->setParent(0);
646 _clkSocket->moveToThread(tt);
647//slotMessage("bns::slotMoveSocket");
648 slotMessage("Clocks & orbits port: Socket moved to thread"); // weber
649}
650
651// Transform Coordinates
652////////////////////////////////////////////////////////////////////////////
653void t_bns::crdTrafo(int GPSWeek, ColumnVector& xyz, const QString trafo) {
654
655 bnsSettings settings;
656
657 if (trafo == "ETRF2000") {
658 _dx = 0.0541;
659 _dy = 0.0502;
660 _dz = -0.0538;
661 _dxr = -0.0002;
662 _dyr = 0.0001;
663 _dzr = -0.0018;
664 _ox = 0.000891;
665 _oy = 0.005390;
666 _oz = -0.008712;
667 _oxr = 0.000081;
668 _oyr = 0.000490;
669 _ozr = -0.000792;
670 _sc = 0.40;
671 _scr = 0.08;
672 _t0 = 2000.0;
673 }
674 else if (trafo == "NAD83") {
675 _dx = 0.9963;
676 _dy = -1.9024;
677 _dz = -0.5210;
678 _dxr = 0.0005;
679 _dyr = -0.0006;
680 _dzr = -0.0013;
681 _ox = 0.025915;
682 _oy = 0.009426;
683 _oz = 0.011599;
684 _oxr = 0.000067;
685 _oyr = -0.000757;
686 _ozr = -0.000051;
687 _sc = 0.78;
688 _scr = -0.10;
689 _t0 = 1997.0;
690 }
691 else if (trafo == "GDA94") {
692 _dx = 0.07167;
693 _dy = 0.00486;
694 _dz = -0.04711;
695 _dxr = -0.00342;
696 _dyr = 0.00055;
697 _dzr = 0.00136;
698 _ox = 0.0091362;
699 _oy = 0.0093086;
700 _oz = 0.0091599;
701 _oxr = 0.0014652;
702 _oyr = 0.0011005;
703 _ozr = 0.0011480;
704 _sc = -8.239;
705 _scr = -0.212;
706 _t0 = 2000.0;
707 }
708 else if (trafo == "SIRGAS2000") {
709 _dx = -0.0051;
710 _dy = -0.0065;
711 _dz = -0.0099;
712 _dxr = 0.0000;
713 _dyr = 0.0000;
714 _dzr = 0.0000;
715 _ox = 0.000150;
716 _oy = 0.000020;
717 _oz = 0.000021;
718 _oxr = 0.000000;
719 _oyr = 0.000000;
720 _ozr = 0.000000;
721 _sc = 0.000;
722 _scr = 0.000;
723 _t0 = 0000.0;
724 }
725 else if (trafo == "SIRGAS95") {
726 _dx = 0.0077;
727 _dy = 0.0058;
728 _dz = -0.0138;
729 _dxr = 0.0000;
730 _dyr = 0.0000;
731 _dzr = 0.0000;
732 _ox = 0.000000;
733 _oy = 0.000000;
734 _oz = -0.000030;
735 _oxr = 0.000000;
736 _oyr = 0.000000;
737 _ozr = 0.000000;
738 _sc = 1.570;
739 _scr = 0.000;
740 _t0 = 0000.0;
741 }
742 else if (trafo == "Custom") {
743 _dx = settings.value("trafo_dx").toDouble();
744 _dy = settings.value("trafo_dy").toDouble();
745 _dz = settings.value("trafo_dz").toDouble();
746 _dxr = settings.value("trafo_dxr").toDouble();
747 _dyr = settings.value("trafo_dyr").toDouble();
748 _dzr = settings.value("trafo_dzr").toDouble();
749 _ox = settings.value("trafo_ox").toDouble();
750 _oy = settings.value("trafo_oy").toDouble();
751 _oz = settings.value("trafo_oz").toDouble();
752 _oxr = settings.value("trafo_oxr").toDouble();
753 _oyr = settings.value("trafo_oyr").toDouble();
754 _ozr = settings.value("trafo_ozr").toDouble();
755 _sc = settings.value("trafo_sc").toDouble();
756 _scr = settings.value("trafo_scr").toDouble();
757 _t0 = settings.value("trafo_t0").toDouble();
758 }
759
760 // Current epoch minus 2000.0 in years
761 // ------------------------------------
762 double dt = (GPSWeek - (1042.0+6.0/7.0)) / 365.2422 * 7.0 + 2000.0 - _t0;
763
764 ColumnVector dx(3);
765
766 dx(1) = _dx + dt * _dxr;
767 dx(2) = _dy + dt * _dyr;
768 dx(3) = _dz + dt * _dzr;
769
770 static const double arcSec = 180.0 * 3600.0 / M_PI;
771
772 double ox = (_ox + dt * _oxr) / arcSec;
773 double oy = (_oy + dt * _oyr) / arcSec;
774 double oz = (_oz + dt * _ozr) / arcSec;
775
776 double sc = 1.0 + _sc * 1e-9 + dt * _scr * 1e-9;
777
778 Matrix rMat(3,3);
779 rMat(1,1) = 1.0;
780 rMat(1,2) = -oz;
781 rMat(1,3) = oy;
782 rMat(2,1) = oz;
783 rMat(2,2) = 1.0;
784 rMat(2,3) = -ox;
785 rMat(3,1) = -oy;
786 rMat(3,2) = ox;
787 rMat(3,3) = 1.0;
788
789 xyz = sc * rMat * xyz + dx;
790}
Note: See TracBrowser for help on using the repository browser.