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

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