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

Last change on this file since 2632 was 2632, checked in by mervart, 13 years ago

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