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

Last change on this file since 2546 was 2493, checked in by stoecker, 15 years ago

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