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

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