source: ntrip/trunk/BNC/src/ephemeris.cpp@ 8964

Last change on this file since 8964 was 8964, checked in by stuerze, 4 years ago

minor changes
File size: 47.7 KB
Line 
1#include <sstream>
2#include <iostream>
3#include <iomanip>
4#include <cstring>
5
6#include <newmatio.h>
7
8#include "ephemeris.h"
9#include "bncutils.h"
10#include "bnctime.h"
11#include "bnccore.h"
12#include "bncutils.h"
13#include "satObs.h"
14#include "pppInclude.h"
15#include "pppModel.h"
16
17using namespace std;
18
19// Constructor
20////////////////////////////////////////////////////////////////////////////
21t_eph::t_eph() {
22 _checkState = unchecked;
23 _orbCorr = 0;
24 _clkCorr = 0;
25}
26// Destructor
27////////////////////////////////////////////////////////////////////////////
28t_eph::~t_eph() {
29 if (_orbCorr)
30 delete _orbCorr;
31 if (_clkCorr)
32 delete _clkCorr;
33}
34
35//
36////////////////////////////////////////////////////////////////////////////
37void t_eph::setOrbCorr(const t_orbCorr* orbCorr) {
38 if (_orbCorr) {
39 delete _orbCorr;
40 _orbCorr = 0;
41 }
42 _orbCorr = new t_orbCorr(*orbCorr);
43}
44
45//
46////////////////////////////////////////////////////////////////////////////
47void t_eph::setClkCorr(const t_clkCorr* clkCorr) {
48 if (_clkCorr) {
49 delete _clkCorr;
50 _clkCorr = 0;
51 }
52 _clkCorr = new t_clkCorr(*clkCorr);
53}
54
55//
56////////////////////////////////////////////////////////////////////////////
57t_irc t_eph::getCrd(const bncTime& tt, ColumnVector& xc, ColumnVector& vv, bool useCorr) const {
58
59 if (_checkState == bad ||
60 _checkState == unhealthy ||
61 _checkState == outdated) {
62 return failure;
63 }
64
65 xc.ReSize(6);
66 vv.ReSize(3);
67 if (position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data()) != success) {
68 return failure;
69 }
70 if (useCorr) {
71 if (_orbCorr && _clkCorr) {
72 double dtO = tt - _orbCorr->_time;
73 if (_orbCorr->_updateInt) {
74 dtO -= (0.5 * ssrUpdateInt[_orbCorr->_updateInt]);
75 }
76 ColumnVector dx(3);
77 dx[0] = _orbCorr->_xr[0] + _orbCorr->_dotXr[0] * dtO;
78 dx[1] = _orbCorr->_xr[1] + _orbCorr->_dotXr[1] * dtO;
79 dx[2] = _orbCorr->_xr[2] + _orbCorr->_dotXr[2] * dtO;
80
81 RSW_to_XYZ(xc.Rows(1,3), vv.Rows(1,3), dx, dx);
82
83 xc[0] -= dx[0];
84 xc[1] -= dx[1];
85 xc[2] -= dx[2];
86
87 ColumnVector dv(3);
88 RSW_to_XYZ(xc.Rows(1,3), vv.Rows(1,3), _orbCorr->_dotXr, dv);
89
90 vv[0] -= dv[0];
91 vv[1] -= dv[1];
92 vv[2] -= dv[2];
93
94 double dtC = tt - _clkCorr->_time;
95 if (_clkCorr->_updateInt) {
96 dtC -= (0.5 * ssrUpdateInt[_clkCorr->_updateInt]);
97 }
98 xc[3] += _clkCorr->_dClk + _clkCorr->_dotDClk * dtC + _clkCorr->_dotDotDClk * dtC * dtC;
99 }
100 else {
101 return failure;
102 }
103 }
104 return success;
105}
106
107//
108//////////////////////////////////////////////////////////////////////////////
109QString t_eph::rinexDateStr(const bncTime& tt, const t_prn& prn, double version) {
110 QString prnStr(prn.toString().c_str());
111 return rinexDateStr(tt, prnStr, version);
112}
113
114//
115//////////////////////////////////////////////////////////////////////////////
116QString t_eph::rinexDateStr(const bncTime& tt, const QString& prnStr, double version) {
117
118 QString datStr;
119
120 unsigned year, month, day, hour, min;
121 double sec;
122 tt.civil_date(year, month, day);
123 tt.civil_time(hour, min, sec);
124
125 QTextStream out(&datStr);
126
127 if (version < 3.0) {
128 QString prnHlp = prnStr.mid(1,2); if (prnHlp[0] == '0') prnHlp[0] = ' ';
129 out << prnHlp << QString(" %1 %2 %3 %4 %5%6")
130 .arg(year % 100, 2, 10, QChar('0'))
131 .arg(month, 2)
132 .arg(day, 2)
133 .arg(hour, 2)
134 .arg(min, 2)
135 .arg(sec, 5, 'f',1);
136 }
137 else {
138 out << prnStr << QString(" %1 %2 %3 %4 %5 %6")
139 .arg(year, 4)
140 .arg(month, 2, 10, QChar('0'))
141 .arg(day, 2, 10, QChar('0'))
142 .arg(hour, 2, 10, QChar('0'))
143 .arg(min, 2, 10, QChar('0'))
144 .arg(int(sec), 2, 10, QChar('0'));
145 }
146
147 return datStr;
148}
149
150// Constructor
151//////////////////////////////////////////////////////////////////////////////
152t_ephGPS::t_ephGPS(float rnxVersion, const QStringList& lines) {
153
154 const int nLines = 8;
155
156 if (lines.size() != nLines) {
157 _checkState = bad;
158 return;
159 }
160
161 // RINEX Format
162 // ------------
163 int fieldLen = 19;
164
165 int pos[4];
166 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
167 pos[1] = pos[0] + fieldLen;
168 pos[2] = pos[1] + fieldLen;
169 pos[3] = pos[2] + fieldLen;
170
171 // Read eight lines
172 // ----------------
173 for (int iLine = 0; iLine < nLines; iLine++) {
174 QString line = lines[iLine];
175
176 if ( iLine == 0 ) {
177 QTextStream in(line.left(pos[1]).toLatin1());
178 int year, month, day, hour, min;
179 double sec;
180
181 QString prnStr, n;
182 in >> prnStr;
183
184 if (prnStr.size() == 1 &&
185 (prnStr[0] == 'G' ||
186 prnStr[0] == 'J' ||
187 prnStr[0] == 'I')) {
188 in >> n;
189 prnStr.append(n);
190 }
191
192 in >> year >> month >> day >> hour >> min >> sec;
193 if (prnStr.at(0) == 'G') {
194 _prn.set('G', prnStr.mid(1).toInt());
195 }
196 else if (prnStr.at(0) == 'J') {
197 _prn.set('J', prnStr.mid(1).toInt());
198 }
199 else if (prnStr.at(0) == 'I') {
200 _prn.set('I', prnStr.mid(1).toInt());
201 }
202 else {
203 _prn.set('G', prnStr.toInt());
204 }
205
206 if (year < 80) {
207 year += 2000;
208 }
209 else if (year < 100) {
210 year += 1900;
211 }
212
213 _TOC.set(year, month, day, hour, min, sec);
214
215 if ( readDbl(line, pos[1], fieldLen, _clock_bias ) ||
216 readDbl(line, pos[2], fieldLen, _clock_drift ) ||
217 readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
218 _checkState = bad;
219 return;
220 }
221 }
222
223 else if ( iLine == 1 ) {
224 if ( readDbl(line, pos[0], fieldLen, _IODE ) ||
225 readDbl(line, pos[1], fieldLen, _Crs ) ||
226 readDbl(line, pos[2], fieldLen, _Delta_n) ||
227 readDbl(line, pos[3], fieldLen, _M0 ) ) {
228 _checkState = bad;
229 return;
230 }
231 }
232
233 else if ( iLine == 2 ) {
234 if ( readDbl(line, pos[0], fieldLen, _Cuc ) ||
235 readDbl(line, pos[1], fieldLen, _e ) ||
236 readDbl(line, pos[2], fieldLen, _Cus ) ||
237 readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
238 _checkState = bad;
239 return;
240 }
241 }
242
243 else if ( iLine == 3 ) {
244 if ( readDbl(line, pos[0], fieldLen, _TOEsec) ||
245 readDbl(line, pos[1], fieldLen, _Cic ) ||
246 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
247 readDbl(line, pos[3], fieldLen, _Cis ) ) {
248 _checkState = bad;
249 return;
250 }
251 }
252
253 else if ( iLine == 4 ) {
254 if ( readDbl(line, pos[0], fieldLen, _i0 ) ||
255 readDbl(line, pos[1], fieldLen, _Crc ) ||
256 readDbl(line, pos[2], fieldLen, _omega ) ||
257 readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
258 _checkState = bad;
259 return;
260 }
261 }
262
263 else if ( iLine == 5 && type() != t_eph::IRNSS) {
264 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
265 readDbl(line, pos[1], fieldLen, _L2Codes) ||
266 readDbl(line, pos[2], fieldLen, _TOEweek ) ||
267 readDbl(line, pos[3], fieldLen, _L2PFlag) ) {
268 _checkState = bad;
269 return;
270 }
271 }
272 else if ( iLine == 5 && type() == t_eph::IRNSS) {
273 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
274 readDbl(line, pos[2], fieldLen, _TOEweek) ) {
275 _checkState = bad;
276 return;
277 }
278 }
279
280 else if ( iLine == 6 && type() != t_eph::IRNSS) {
281 if ( readDbl(line, pos[0], fieldLen, _ura ) ||
282 readDbl(line, pos[1], fieldLen, _health) ||
283 readDbl(line, pos[2], fieldLen, _TGD ) ||
284 readDbl(line, pos[3], fieldLen, _IODC ) ) {
285 _checkState = bad;
286 return;
287 }
288 }
289 else if ( iLine == 6 && type() == t_eph::IRNSS) {
290 if ( readDbl(line, pos[0], fieldLen, _ura ) ||
291 readDbl(line, pos[1], fieldLen, _health) ||
292 readDbl(line, pos[2], fieldLen, _TGD ) ) {
293 _checkState = bad;
294 return;
295 }
296 }
297 else if ( iLine == 7 ) {
298 if ( readDbl(line, pos[0], fieldLen, _TOT) ) {
299 _checkState = bad;
300 return;
301 }
302 // fitInterval is not valid for IRNSS
303 if (type() != t_eph::IRNSS) {
304 double fitIntervalRnx;
305 readDbl(line, pos[1], fieldLen, fitIntervalRnx);
306 if (type() == t_eph::GPS) { // in RINEX specified allways as time period for GPS
307 _fitInterval = fitIntervalRnx;
308 } else if (type() == t_eph::QZSS) { // specified as flag for QZSS
309 if (rnxVersion == 3.02) {
310 _fitInterval = fitIntervalRnx; // specified as time period
311 }
312 else {
313 _fitInterval = fitIntervalFromFlag(fitIntervalRnx, _IODC, t_eph::QZSS);
314 }
315 }
316 }
317 }
318 }
319}
320
321// Compute GPS Satellite Position (virtual)
322////////////////////////////////////////////////////////////////////////////
323t_irc t_ephGPS::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
324
325 static const double omegaEarth = 7292115.1467e-11;
326 static const double gmGRS = 398.6005e12;
327
328 memset(xc, 0, 6*sizeof(double));
329 memset(vv, 0, 3*sizeof(double));
330
331 double a0 = _sqrt_A * _sqrt_A;
332 if (a0 == 0) {
333 return failure;
334 }
335
336 double n0 = sqrt(gmGRS/(a0*a0*a0));
337
338 bncTime tt(GPSweek, GPSweeks);
339 double tk = tt - bncTime(int(_TOEweek), _TOEsec);
340
341 double n = n0 + _Delta_n;
342 double M = _M0 + n*tk;
343 double E = M;
344 double E_last;
345 int nLoop = 0;
346 do {
347 E_last = E;
348 E = M + _e*sin(E);
349
350 if (++nLoop == 100) {
351 return failure;
352 }
353 } while ( fabs(E-E_last)*a0 > 0.001);
354 double v = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) );
355 double u0 = v + _omega;
356 double sin2u0 = sin(2*u0);
357 double cos2u0 = cos(2*u0);
358 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
359 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
360 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
361 double xp = r*cos(u);
362 double yp = r*sin(u);
363 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
364 omegaEarth*_TOEsec;
365
366 double sinom = sin(OM);
367 double cosom = cos(OM);
368 double sini = sin(i);
369 double cosi = cos(i);
370 xc[0] = xp*cosom - yp*cosi*sinom;
371 xc[1] = xp*sinom + yp*cosi*cosom;
372 xc[2] = yp*sini;
373
374 double tc = tt - _TOC;
375 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
376
377 // Velocity
378 // --------
379 double tanv2 = tan(v/2);
380 double dEdM = 1 / (1 - _e*cos(E));
381 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
382 * dEdM * n;
383 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
384 double dotom = _OMEGADOT - omegaEarth;
385 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
386 double dotr = a0 * _e*sin(E) * dEdM * n
387 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
388 double dotx = dotr*cos(u) - r*sin(u)*dotu;
389 double doty = dotr*sin(u) + r*cos(u)*dotu;
390
391 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
392 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
393 + yp*sini*sinom*doti; // dX / di
394
395 vv[1] = sinom *dotx + cosi*cosom *doty
396 + xp*cosom*dotom - yp*cosi*sinom*dotom
397 - yp*sini*cosom*doti;
398
399 vv[2] = sini *doty + yp*cosi *doti;
400
401 // Relativistic Correction
402 // -----------------------
403 // correspondent to IGS convention and GPS ICD (and SSR standard)
404 xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c;
405
406 xc[4] = _clock_drift + _clock_driftrate*tc;
407 xc[5] = _clock_driftrate;
408
409 return success;
410}
411
412// RINEX Format String
413//////////////////////////////////////////////////////////////////////////////
414QString t_ephGPS::toString(double version) const {
415
416 QString rnxStr = rinexDateStr(_TOC, _prn, version);
417
418 QTextStream out(&rnxStr);
419
420 out << QString("%1%2%3\n")
421 .arg(_clock_bias, 19, 'e', 12)
422 .arg(_clock_drift, 19, 'e', 12)
423 .arg(_clock_driftrate, 19, 'e', 12);
424
425 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
426
427 out << QString(fmt)
428 .arg(_IODE, 19, 'e', 12)
429 .arg(_Crs, 19, 'e', 12)
430 .arg(_Delta_n, 19, 'e', 12)
431 .arg(_M0, 19, 'e', 12);
432
433 out << QString(fmt)
434 .arg(_Cuc, 19, 'e', 12)
435 .arg(_e, 19, 'e', 12)
436 .arg(_Cus, 19, 'e', 12)
437 .arg(_sqrt_A, 19, 'e', 12);
438
439 out << QString(fmt)
440 .arg(_TOEsec, 19, 'e', 12)
441 .arg(_Cic, 19, 'e', 12)
442 .arg(_OMEGA0, 19, 'e', 12)
443 .arg(_Cis, 19, 'e', 12);
444
445 out << QString(fmt)
446 .arg(_i0, 19, 'e', 12)
447 .arg(_Crc, 19, 'e', 12)
448 .arg(_omega, 19, 'e', 12)
449 .arg(_OMEGADOT, 19, 'e', 12);
450
451 if (type() == t_eph::IRNSS) {
452 out << QString(fmt)
453 .arg(_IDOT, 19, 'e', 12)
454 .arg(0.0, 19, 'e', 12)
455 .arg(_TOEweek, 19, 'e', 12)
456 .arg(0.0, 19, 'e', 12);
457 }
458 else {
459 out << QString(fmt)
460 .arg(_IDOT, 19, 'e', 12)
461 .arg(_L2Codes, 19, 'e', 12)
462 .arg(_TOEweek, 19, 'e', 12)
463 .arg(_L2PFlag, 19, 'e', 12);
464 }
465
466 if (type() == t_eph::IRNSS) {
467 out << QString(fmt)
468 .arg(_ura, 19, 'e', 12)
469 .arg(_health, 19, 'e', 12)
470 .arg(_TGD, 19, 'e', 12)
471 .arg(0.0, 19, 'e', 12);
472 }
473 else {
474 out << QString(fmt)
475 .arg(_ura, 19, 'e', 12)
476 .arg(_health, 19, 'e', 12)
477 .arg(_TGD, 19, 'e', 12)
478 .arg(_IODC, 19, 'e', 12);
479 }
480
481 double tot = _TOT;
482 if (tot == 0.9999e9 && version < 3.0) {
483 tot = 0.0;
484 }
485 // fitInterval
486 if (type() == t_eph::IRNSS) {// not valid for IRNSS
487 out << QString(fmt)
488 .arg(tot, 19, 'e', 12)
489 .arg(0.0, 19, 'e', 12)
490 .arg("", 19, QChar(' '))
491 .arg("", 19, QChar(' '));
492 }
493 else {
494 // for GPS and QZSS in version 3.02 specified in hours
495 double fitIntervalRnx = _fitInterval;
496 // otherwise specified as flag
497 if (type() == t_eph::QZSS && version != 3.02) {
498 (_fitInterval == 2.0) ? fitIntervalRnx = 0.0 : fitIntervalRnx = 1.0;
499 }
500 out << QString(fmt)
501 .arg(tot, 19, 'e', 12)
502 .arg(fitIntervalRnx, 19, 'e', 12)
503 .arg("", 19, QChar(' '))
504 .arg("", 19, QChar(' '));
505 }
506 return rnxStr;
507}
508
509// Constructor
510//////////////////////////////////////////////////////////////////////////////
511t_ephGlo::t_ephGlo(float rnxVersion, const QStringList& lines) {
512
513 const int nLines = 4;
514
515 if (lines.size() != nLines) {
516 _checkState = bad;
517 return;
518 }
519
520 // RINEX Format
521 // ------------
522 int fieldLen = 19;
523
524 int pos[4];
525 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
526 pos[1] = pos[0] + fieldLen;
527 pos[2] = pos[1] + fieldLen;
528 pos[3] = pos[2] + fieldLen;
529
530 // Read four lines
531 // ---------------
532 for (int iLine = 0; iLine < nLines; iLine++) {
533 QString line = lines[iLine];
534
535 if ( iLine == 0 ) {
536 QTextStream in(line.left(pos[1]).toLatin1());
537
538 int year, month, day, hour, min;
539 double sec;
540
541 QString prnStr, n;
542 in >> prnStr;
543 if (prnStr.size() == 1 && prnStr[0] == 'R') {
544 in >> n;
545 prnStr.append(n);
546 }
547 in >> year >> month >> day >> hour >> min >> sec;
548 if (prnStr.at(0) == 'R') {
549 _prn.set('R', prnStr.mid(1).toInt());
550 }
551 else {
552 _prn.set('R', prnStr.toInt());
553 }
554
555 if (year < 80) {
556 year += 2000;
557 }
558 else if (year < 100) {
559 year += 1900;
560 }
561
562 _gps_utc = gnumleap(year, month, day);
563
564 _TOC.set(year, month, day, hour, min, sec);
565 _TOC = _TOC + _gps_utc;
566 int nd = int((_TOC.gpssec())) / (24.0*60.0*60.0);
567 if ( readDbl(line, pos[1], fieldLen, _tau ) ||
568 readDbl(line, pos[2], fieldLen, _gamma) ||
569 readDbl(line, pos[3], fieldLen, _tki ) ) {
570 _checkState = bad;
571 return;
572 }
573 _tki -= nd * 86400.0;
574 _tau = -_tau;
575 }
576
577 else if ( iLine == 1 ) {
578 if ( readDbl(line, pos[0], fieldLen, _x_pos ) ||
579 readDbl(line, pos[1], fieldLen, _x_velocity ) ||
580 readDbl(line, pos[2], fieldLen, _x_acceleration) ||
581 readDbl(line, pos[3], fieldLen, _health ) ) {
582 _checkState = bad;
583 return;
584 }
585 }
586
587 else if ( iLine == 2 ) {
588 if ( readDbl(line, pos[0], fieldLen, _y_pos ) ||
589 readDbl(line, pos[1], fieldLen, _y_velocity ) ||
590 readDbl(line, pos[2], fieldLen, _y_acceleration ) ||
591 readDbl(line, pos[3], fieldLen, _frequency_number) ) {
592 _checkState = bad;
593 return;
594 }
595 }
596
597 else if ( iLine == 3 ) {
598 if ( readDbl(line, pos[0], fieldLen, _z_pos ) ||
599 readDbl(line, pos[1], fieldLen, _z_velocity ) ||
600 readDbl(line, pos[2], fieldLen, _z_acceleration) ||
601 readDbl(line, pos[3], fieldLen, _E ) ) {
602 _checkState = bad;
603 return;
604 }
605 }
606 }
607
608 // Initialize status vector
609 // ------------------------
610 _tt = _TOC;
611 _xv.ReSize(6); _xv = 0.0;
612 _xv(1) = _x_pos * 1.e3;
613 _xv(2) = _y_pos * 1.e3;
614 _xv(3) = _z_pos * 1.e3;
615 _xv(4) = _x_velocity * 1.e3;
616 _xv(5) = _y_velocity * 1.e3;
617 _xv(6) = _z_velocity * 1.e3;
618}
619
620// Compute Glonass Satellite Position (virtual)
621////////////////////////////////////////////////////////////////////////////
622t_irc t_ephGlo::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
623
624 static const double nominalStep = 10.0;
625
626 memset(xc, 0, 6*sizeof(double));
627 memset(vv, 0, 3*sizeof(double));
628
629 double dtPos = bncTime(GPSweek, GPSweeks) - _tt;
630
631 if (fabs(dtPos) > 24 * 3600.0) {
632 return failure;
633 }
634
635 int nSteps = int(fabs(dtPos) / nominalStep) + 1;
636 double step = dtPos / nSteps;
637
638 double acc[3];
639 acc[0] = _x_acceleration * 1.e3;
640 acc[1] = _y_acceleration * 1.e3;
641 acc[2] = _z_acceleration * 1.e3;
642
643 for (int ii = 1; ii <= nSteps; ii++) {
644 _xv = rungeKutta4(_tt.gpssec(), _xv, step, acc, glo_deriv);
645 _tt = _tt + step;
646 }
647
648 // Position and Velocity
649 // ---------------------
650 xc[0] = _xv(1);
651 xc[1] = _xv(2);
652 xc[2] = _xv(3);
653
654 vv[0] = _xv(4);
655 vv[1] = _xv(5);
656 vv[2] = _xv(6);
657
658 // Clock Correction
659 // ----------------
660 double dtClk = bncTime(GPSweek, GPSweeks) - _TOC;
661 xc[3] = -_tau + _gamma * dtClk;
662
663 xc[4] = _gamma;
664 xc[5] = 0.0;
665
666 return success;
667}
668
669// RINEX Format String
670//////////////////////////////////////////////////////////////////////////////
671QString t_ephGlo::toString(double version) const {
672
673 QString rnxStr = rinexDateStr(_TOC -_gps_utc, _prn, version);
674 int nd = int((_TOC - _gps_utc).gpssec()) / (24.0*60.0*60.0);
675 QTextStream out(&rnxStr);
676
677 out << QString("%1%2%3\n")
678 .arg(-_tau, 19, 'e', 12)
679 .arg(_gamma, 19, 'e', 12)
680 .arg(_tki+nd*86400.0, 19, 'e', 12);
681
682 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
683
684 out << QString(fmt)
685 .arg(_x_pos, 19, 'e', 12)
686 .arg(_x_velocity, 19, 'e', 12)
687 .arg(_x_acceleration, 19, 'e', 12)
688 .arg(_health, 19, 'e', 12);
689
690 out << QString(fmt)
691 .arg(_y_pos, 19, 'e', 12)
692 .arg(_y_velocity, 19, 'e', 12)
693 .arg(_y_acceleration, 19, 'e', 12)
694 .arg(_frequency_number, 19, 'e', 12);
695
696 out << QString(fmt)
697 .arg(_z_pos, 19, 'e', 12)
698 .arg(_z_velocity, 19, 'e', 12)
699 .arg(_z_acceleration, 19, 'e', 12)
700 .arg(_E, 19, 'e', 12);
701
702 return rnxStr;
703}
704
705// Derivative of the state vector using a simple force model (static)
706////////////////////////////////////////////////////////////////////////////
707ColumnVector t_ephGlo::glo_deriv(double /* tt */, const ColumnVector& xv,
708 double* acc) {
709
710 // State vector components
711 // -----------------------
712 ColumnVector rr = xv.rows(1,3);
713 ColumnVector vv = xv.rows(4,6);
714
715 // Acceleration
716 // ------------
717 static const double gmWGS = 398.60044e12;
718 static const double AE = 6378136.0;
719 static const double OMEGA = 7292115.e-11;
720 static const double C20 = -1082.6257e-6;
721
722 double rho = rr.NormFrobenius();
723 double t1 = -gmWGS/(rho*rho*rho);
724 double t2 = 3.0/2.0 * C20 * (gmWGS*AE*AE) / (rho*rho*rho*rho*rho);
725 double t3 = OMEGA * OMEGA;
726 double t4 = 2.0 * OMEGA;
727 double z2 = rr(3) * rr(3);
728
729 // Vector of derivatives
730 // ---------------------
731 ColumnVector va(6);
732 va(1) = vv(1);
733 va(2) = vv(2);
734 va(3) = vv(3);
735 va(4) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2) + acc[0];
736 va(5) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1) + acc[1];
737 va(6) = (t1 + t2*(3.0-5.0*z2/(rho*rho)) ) * rr(3) + acc[2];
738
739 return va;
740}
741
742// IOD of Glonass Ephemeris (virtual)
743////////////////////////////////////////////////////////////////////////////
744unsigned int t_ephGlo::IOD() const {
745 bncTime tMoscow = _TOC - _gps_utc + 3 * 3600.0;
746 return (unsigned long)tMoscow.daysec() / 900;
747}
748
749// Health status of Glonass Ephemeris (virtual)
750////////////////////////////////////////////////////////////////////////////
751unsigned int t_ephGlo::isUnhealthy() const {
752
753 if (_almanac_health_availablility_indicator) {
754 if ((_health == 0 && _almanac_health == 0) ||
755 (_health == 1 && _almanac_health == 0) ||
756 (_health == 1 && _almanac_health == 1)) {
757 return 1;
758 }
759 }
760 else if (!_almanac_health_availablility_indicator) {
761 if (_health) {
762 return 1;
763 }
764 }
765 return 0; /* (_health == 0 && _almanac_health == 1) or (_health == 0) */
766}
767
768// Constructor
769//////////////////////////////////////////////////////////////////////////////
770t_ephGal::t_ephGal(float rnxVersion, const QStringList& lines) {
771 int year, month, day, hour, min;
772 double sec;
773 QString prnStr;
774 const int nLines = 8;
775 if (lines.size() != nLines) {
776 _checkState = bad;
777 return;
778 }
779
780 // RINEX Format
781 // ------------
782 int fieldLen = 19;
783 double SVhealth = 0.0;
784 double datasource = 0.0;
785
786 int pos[4];
787 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
788 pos[1] = pos[0] + fieldLen;
789 pos[2] = pos[1] + fieldLen;
790 pos[3] = pos[2] + fieldLen;
791
792 // Read eight lines
793 // ----------------
794 for (int iLine = 0; iLine < nLines; iLine++) {
795 QString line = lines[iLine];
796
797 if ( iLine == 0 ) {
798 QTextStream in(line.left(pos[1]).toLatin1());
799 QString n;
800 in >> prnStr;
801 if (prnStr.size() == 1 && prnStr[0] == 'E') {
802 in >> n;
803 prnStr.append(n);
804 }
805 in >> year >> month >> day >> hour >> min >> sec;
806 if (year < 80) {
807 year += 2000;
808 }
809 else if (year < 100) {
810 year += 1900;
811 }
812
813 _TOC.set(year, month, day, hour, min, sec);
814
815 if ( readDbl(line, pos[1], fieldLen, _clock_bias ) ||
816 readDbl(line, pos[2], fieldLen, _clock_drift ) ||
817 readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
818 _checkState = bad;
819 return;
820 }
821 }
822
823 else if ( iLine == 1 ) {
824 if ( readDbl(line, pos[0], fieldLen, _IODnav ) ||
825 readDbl(line, pos[1], fieldLen, _Crs ) ||
826 readDbl(line, pos[2], fieldLen, _Delta_n) ||
827 readDbl(line, pos[3], fieldLen, _M0 ) ) {
828 _checkState = bad;
829 return;
830 }
831 }
832
833 else if ( iLine == 2 ) {
834 if ( readDbl(line, pos[0], fieldLen, _Cuc ) ||
835 readDbl(line, pos[1], fieldLen, _e ) ||
836 readDbl(line, pos[2], fieldLen, _Cus ) ||
837 readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
838 _checkState = bad;
839 return;
840 }
841 }
842
843 else if ( iLine == 3 ) {
844 if ( readDbl(line, pos[0], fieldLen, _TOEsec) ||
845 readDbl(line, pos[1], fieldLen, _Cic ) ||
846 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
847 readDbl(line, pos[3], fieldLen, _Cis ) ) {
848 _checkState = bad;
849 return;
850 }
851 }
852
853 else if ( iLine == 4 ) {
854 if ( readDbl(line, pos[0], fieldLen, _i0 ) ||
855 readDbl(line, pos[1], fieldLen, _Crc ) ||
856 readDbl(line, pos[2], fieldLen, _omega ) ||
857 readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
858 _checkState = bad;
859 return;
860 }
861 }
862
863 else if ( iLine == 5 ) {
864 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
865 readDbl(line, pos[1], fieldLen, datasource) ||
866 readDbl(line, pos[2], fieldLen, _TOEweek ) ) {
867 _checkState = bad;
868 return;
869 } else {
870 if (int(datasource) & (1<<8)) {
871 _fnav = true;
872 _inav = false;
873 } else if (int(datasource) & (1<<9)) {
874 _fnav = false;
875 _inav = true;
876 }
877 _TOEweek -= 1024.0;
878 }
879 }
880
881 else if ( iLine == 6 ) {
882 if ( readDbl(line, pos[0], fieldLen, _SISA ) ||
883 readDbl(line, pos[1], fieldLen, SVhealth) ||
884 readDbl(line, pos[2], fieldLen, _BGD_1_5A) ||
885 readDbl(line, pos[3], fieldLen, _BGD_1_5B) ) {
886 _checkState = bad;
887 return;
888 } else {
889 // Bit 0
890 _e1DataInValid = (int(SVhealth) & (1<<0));
891 // Bit 1-2
892 _E1_bHS = double((int(SVhealth) >> 1) & 0x3);
893 // Bit 3
894 _e5aDataInValid = (int(SVhealth) & (1<<3));
895 // Bit 4-5
896 _E5aHS = double((int(SVhealth) >> 4) & 0x3);
897 // Bit 6
898 _e5bDataInValid = (int(SVhealth) & (1<<6));
899 // Bit 7-8
900 _E5bHS = double((int(SVhealth) >> 7) & 0x3);
901
902 if (prnStr.at(0) == 'E') {
903 _prn.set('E', prnStr.mid(1).toInt(), _inav ? 1 : 0);
904 }
905 }
906 }
907
908 else if ( iLine == 7 ) {
909 if ( readDbl(line, pos[0], fieldLen, _TOT) ) {
910 _checkState = bad;
911 return;
912 }
913 }
914 }
915}
916
917// Compute Galileo Satellite Position (virtual)
918////////////////////////////////////////////////////////////////////////////
919t_irc t_ephGal::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
920
921 static const double omegaEarth = 7292115.1467e-11;
922 static const double gmWGS = 398.6004418e12;
923
924 memset(xc, 0, 6*sizeof(double));
925 memset(vv, 0, 3*sizeof(double));
926
927 double a0 = _sqrt_A * _sqrt_A;
928 if (a0 == 0) {
929 return failure;
930 }
931
932 double n0 = sqrt(gmWGS/(a0*a0*a0));
933
934 bncTime tt(GPSweek, GPSweeks);
935 double tk = tt - bncTime(_TOC.gpsw(), _TOEsec);
936
937 double n = n0 + _Delta_n;
938 double M = _M0 + n*tk;
939 double E = M;
940 double E_last;
941 int nLoop = 0;
942 do {
943 E_last = E;
944 E = M + _e*sin(E);
945
946 if (++nLoop == 100) {
947 return failure;
948 }
949 } while ( fabs(E-E_last)*a0 > 0.001 );
950 double v = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) );
951 double u0 = v + _omega;
952 double sin2u0 = sin(2*u0);
953 double cos2u0 = cos(2*u0);
954 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
955 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
956 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
957 double xp = r*cos(u);
958 double yp = r*sin(u);
959 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
960 omegaEarth*_TOEsec;
961
962 double sinom = sin(OM);
963 double cosom = cos(OM);
964 double sini = sin(i);
965 double cosi = cos(i);
966 xc[0] = xp*cosom - yp*cosi*sinom;
967 xc[1] = xp*sinom + yp*cosi*cosom;
968 xc[2] = yp*sini;
969
970 double tc = tt - _TOC;
971 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
972
973 // Velocity
974 // --------
975 double tanv2 = tan(v/2);
976 double dEdM = 1 / (1 - _e*cos(E));
977 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
978 * dEdM * n;
979 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
980 double dotom = _OMEGADOT - omegaEarth;
981 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
982 double dotr = a0 * _e*sin(E) * dEdM * n
983 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
984 double dotx = dotr*cos(u) - r*sin(u)*dotu;
985 double doty = dotr*sin(u) + r*cos(u)*dotu;
986
987 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
988 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
989 + yp*sini*sinom*doti; // dX / di
990
991 vv[1] = sinom *dotx + cosi*cosom *doty
992 + xp*cosom*dotom - yp*cosi*sinom*dotom
993 - yp*sini*cosom*doti;
994
995 vv[2] = sini *doty + yp*cosi *doti;
996
997 // Relativistic Correction
998 // -----------------------
999 // correspondent to Galileo ICD and to SSR standard
1000 xc[3] -= 4.442807633e-10 * _e * sqrt(a0) *sin(E);
1001 // correspondent to IGS convention
1002 //xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c;
1003
1004 xc[4] = _clock_drift + _clock_driftrate*tc;
1005 xc[5] = _clock_driftrate;
1006
1007 return success;
1008}
1009
1010// Health status of Galileo Ephemeris (virtual)
1011////////////////////////////////////////////////////////////////////////////
1012unsigned int t_ephGal::isUnhealthy() const {
1013 if (_E5aHS && _E5bHS && _E1_bHS) {
1014 return 1;
1015 }
1016 return 0;
1017}
1018
1019// RINEX Format String
1020//////////////////////////////////////////////////////////////////////////////
1021QString t_ephGal::toString(double version) const {
1022
1023 QString rnxStr = rinexDateStr(_TOC, _prn, version);
1024
1025 QTextStream out(&rnxStr);
1026
1027 out << QString("%1%2%3\n")
1028 .arg(_clock_bias, 19, 'e', 12)
1029 .arg(_clock_drift, 19, 'e', 12)
1030 .arg(_clock_driftrate, 19, 'e', 12);
1031
1032 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1033
1034 out << QString(fmt)
1035 .arg(_IODnav, 19, 'e', 12)
1036 .arg(_Crs, 19, 'e', 12)
1037 .arg(_Delta_n, 19, 'e', 12)
1038 .arg(_M0, 19, 'e', 12);
1039
1040 out << QString(fmt)
1041 .arg(_Cuc, 19, 'e', 12)
1042 .arg(_e, 19, 'e', 12)
1043 .arg(_Cus, 19, 'e', 12)
1044 .arg(_sqrt_A, 19, 'e', 12);
1045
1046 out << QString(fmt)
1047 .arg(_TOEsec, 19, 'e', 12)
1048 .arg(_Cic, 19, 'e', 12)
1049 .arg(_OMEGA0, 19, 'e', 12)
1050 .arg(_Cis, 19, 'e', 12);
1051
1052 out << QString(fmt)
1053 .arg(_i0, 19, 'e', 12)
1054 .arg(_Crc, 19, 'e', 12)
1055 .arg(_omega, 19, 'e', 12)
1056 .arg(_OMEGADOT, 19, 'e', 12);
1057
1058 int dataSource = 0;
1059 int SVhealth = 0;
1060 double BGD_1_5A = _BGD_1_5A;
1061 double BGD_1_5B = _BGD_1_5B;
1062 if (_fnav) {
1063 dataSource |= (1<<1);
1064 dataSource |= (1<<8);
1065 BGD_1_5B = 0.0;
1066 // SVhealth
1067 // Bit 3 : E5a DVS
1068 if (_e5aDataInValid) {
1069 SVhealth |= (1<<3);
1070 }
1071 // Bit 4-5: E5a HS
1072 if (_E5aHS == 1.0) {
1073 SVhealth |= (1<<4);
1074 }
1075 else if (_E5aHS == 2.0) {
1076 SVhealth |= (1<<5);
1077 }
1078 else if (_E5aHS == 3.0) {
1079 SVhealth |= (1<<4);
1080 SVhealth |= (1<<5);
1081 }
1082 }
1083 else if(_inav) {
1084 // Bit 2 and 0 are set because from MT1046 the data source cannot be determined
1085 // and RNXv3.03 says both can be set if the navigation messages were merged
1086 dataSource |= (1<<0);
1087 dataSource |= (1<<2);
1088 dataSource |= (1<<9);
1089 // SVhealth
1090 // Bit 0 : E1-B DVS
1091 if (_e1DataInValid) {
1092 SVhealth |= (1<<0);
1093 }
1094 // Bit 1-2: E1-B HS
1095 if (_E1_bHS == 1.0) {
1096 SVhealth |= (1<<1);
1097 }
1098 else if (_E1_bHS == 2.0) {
1099 SVhealth |= (1<<2);
1100 }
1101 else if (_E1_bHS == 3.0) {
1102 SVhealth |= (1<<1);
1103 SVhealth |= (1<<2);
1104 }
1105 // Bit 3 : E5a DVS
1106 if (_e5aDataInValid) {
1107 SVhealth |= (1<<3);
1108 }
1109 // Bit 4-5: E5a HS
1110 if (_E5aHS == 1.0) {
1111 SVhealth |= (1<<4);
1112 }
1113 else if (_E5aHS == 2.0) {
1114 SVhealth |= (1<<5);
1115 }
1116 else if (_E5aHS == 3.0) {
1117 SVhealth |= (1<<4);
1118 SVhealth |= (1<<5);
1119 }
1120 // Bit 6 : E5b DVS
1121 if (_e5bDataInValid) {
1122 SVhealth |= (1<<6);
1123 }
1124 // Bit 7-8: E5b HS
1125 if (_E5bHS == 1.0) {
1126 SVhealth |= (1<<7);
1127 }
1128 else if (_E5bHS == 2.0) {
1129 SVhealth |= (1<<8);
1130 }
1131 else if (_E5bHS == 3.0) {
1132 SVhealth |= (1<<7);
1133 SVhealth |= (1<<8);
1134 }
1135 }
1136
1137 out << QString(fmt)
1138 .arg(_IDOT, 19, 'e', 12)
1139 .arg(double(dataSource), 19, 'e', 12)
1140 .arg(_TOEweek + 1024.0, 19, 'e', 12)
1141 .arg(0.0, 19, 'e', 12);
1142
1143 out << QString(fmt)
1144 .arg(_SISA, 19, 'e', 12)
1145 .arg(double(SVhealth), 19, 'e', 12)
1146 .arg(BGD_1_5A, 19, 'e', 12)
1147 .arg(BGD_1_5B, 19, 'e', 12);
1148
1149 double tot = _TOT;
1150 if (tot == 0.9999e9 && version < 3.0) {
1151 tot = 0.0;
1152 }
1153 out << QString(fmt)
1154 .arg(tot, 19, 'e', 12)
1155 .arg("", 19, QChar(' '))
1156 .arg("", 19, QChar(' '))
1157 .arg("", 19, QChar(' '));
1158
1159 return rnxStr;
1160}
1161
1162// Constructor
1163//////////////////////////////////////////////////////////////////////////////
1164t_ephSBAS::t_ephSBAS(float rnxVersion, const QStringList& lines) {
1165
1166 const int nLines = 4;
1167
1168 if (lines.size() != nLines) {
1169 _checkState = bad;
1170 return;
1171 }
1172
1173 // RINEX Format
1174 // ------------
1175 int fieldLen = 19;
1176
1177 int pos[4];
1178 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
1179 pos[1] = pos[0] + fieldLen;
1180 pos[2] = pos[1] + fieldLen;
1181 pos[3] = pos[2] + fieldLen;
1182
1183 // Read four lines
1184 // ---------------
1185 for (int iLine = 0; iLine < nLines; iLine++) {
1186 QString line = lines[iLine];
1187
1188 if ( iLine == 0 ) {
1189 QTextStream in(line.left(pos[1]).toLatin1());
1190
1191 int year, month, day, hour, min;
1192 double sec;
1193
1194 QString prnStr, n;
1195 in >> prnStr;
1196 if (prnStr.size() == 1 && prnStr[0] == 'S') {
1197 in >> n;
1198 prnStr.append(n);
1199 }
1200 in >> year >> month >> day >> hour >> min >> sec;
1201 if (prnStr.at(0) == 'S') {
1202 _prn.set('S', prnStr.mid(1).toInt());
1203 }
1204 else {
1205 _prn.set('S', prnStr.toInt());
1206 }
1207
1208 if (year < 80) {
1209 year += 2000;
1210 }
1211 else if (year < 100) {
1212 year += 1900;
1213 }
1214
1215 _TOC.set(year, month, day, hour, min, sec);
1216
1217 if ( readDbl(line, pos[1], fieldLen, _agf0 ) ||
1218 readDbl(line, pos[2], fieldLen, _agf1 ) ||
1219 readDbl(line, pos[3], fieldLen, _TOT ) ) {
1220 _checkState = bad;
1221 return;
1222 }
1223 }
1224
1225 else if ( iLine == 1 ) {
1226 if ( readDbl(line, pos[0], fieldLen, _x_pos ) ||
1227 readDbl(line, pos[1], fieldLen, _x_velocity ) ||
1228 readDbl(line, pos[2], fieldLen, _x_acceleration) ||
1229 readDbl(line, pos[3], fieldLen, _health ) ) {
1230 _checkState = bad;
1231 return;
1232 }
1233 }
1234
1235 else if ( iLine == 2 ) {
1236 if ( readDbl(line, pos[0], fieldLen, _y_pos ) ||
1237 readDbl(line, pos[1], fieldLen, _y_velocity ) ||
1238 readDbl(line, pos[2], fieldLen, _y_acceleration ) ||
1239 readDbl(line, pos[3], fieldLen, _ura ) ) {
1240 _checkState = bad;
1241 return;
1242 }
1243 }
1244
1245 else if ( iLine == 3 ) {
1246 double iodn;
1247 if ( readDbl(line, pos[0], fieldLen, _z_pos ) ||
1248 readDbl(line, pos[1], fieldLen, _z_velocity ) ||
1249 readDbl(line, pos[2], fieldLen, _z_acceleration) ||
1250 readDbl(line, pos[3], fieldLen, iodn ) ) {
1251 _checkState = bad;
1252 return;
1253 } else {
1254 _IODN = int(iodn);
1255 }
1256 }
1257 }
1258
1259 _x_pos *= 1.e3;
1260 _y_pos *= 1.e3;
1261 _z_pos *= 1.e3;
1262 _x_velocity *= 1.e3;
1263 _y_velocity *= 1.e3;
1264 _z_velocity *= 1.e3;
1265 _x_acceleration *= 1.e3;
1266 _y_acceleration *= 1.e3;
1267 _z_acceleration *= 1.e3;
1268}
1269
1270// IOD of SBAS Ephemeris (virtual)
1271////////////////////////////////////////////////////////////////////////////
1272
1273unsigned int t_ephSBAS::IOD() const {
1274 unsigned char buffer[80];
1275 int size = 0;
1276 int numbits = 0;
1277 long long bitbuffer = 0;
1278 unsigned char *startbuffer = buffer;
1279
1280 SBASADDBITSFLOAT(30, this->_x_pos, 0.08)
1281 SBASADDBITSFLOAT(30, this->_y_pos, 0.08)
1282 SBASADDBITSFLOAT(25, this->_z_pos, 0.4)
1283 SBASADDBITSFLOAT(17, this->_x_velocity, 0.000625)
1284 SBASADDBITSFLOAT(17, this->_y_velocity, 0.000625)
1285 SBASADDBITSFLOAT(18, this->_z_velocity, 0.004)
1286 SBASADDBITSFLOAT(10, this->_x_acceleration, 0.0000125)
1287 SBASADDBITSFLOAT(10, this->_y_acceleration, 0.0000125)
1288 SBASADDBITSFLOAT(10, this->_z_acceleration, 0.0000625)
1289 SBASADDBITSFLOAT(12, this->_agf0, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
1290 SBASADDBITSFLOAT(8, this->_agf1, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<10))
1291 SBASADDBITS(5,0); // the last byte is filled by 0-bits to obtain a length of an integer multiple of 8
1292
1293 return CRC24(size, startbuffer);
1294}
1295
1296// Compute SBAS Satellite Position (virtual)
1297////////////////////////////////////////////////////////////////////////////
1298t_irc t_ephSBAS::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
1299
1300 bncTime tt(GPSweek, GPSweeks);
1301 double dt = tt - _TOC;
1302
1303 xc[0] = _x_pos + _x_velocity * dt + _x_acceleration * dt * dt / 2.0;
1304 xc[1] = _y_pos + _y_velocity * dt + _y_acceleration * dt * dt / 2.0;
1305 xc[2] = _z_pos + _z_velocity * dt + _z_acceleration * dt * dt / 2.0;
1306
1307 vv[0] = _x_velocity + _x_acceleration * dt;
1308 vv[1] = _y_velocity + _y_acceleration * dt;
1309 vv[2] = _z_velocity + _z_acceleration * dt;
1310
1311 xc[3] = _agf0 + _agf1 * dt;
1312
1313 xc[4] = _agf1;
1314 xc[5] = 0.0;
1315
1316 return success;
1317}
1318
1319// RINEX Format String
1320//////////////////////////////////////////////////////////////////////////////
1321QString t_ephSBAS::toString(double version) const {
1322
1323 QString rnxStr = rinexDateStr(_TOC, _prn, version);
1324
1325 QTextStream out(&rnxStr);
1326
1327 out << QString("%1%2%3\n")
1328 .arg(_agf0, 19, 'e', 12)
1329 .arg(_agf1, 19, 'e', 12)
1330 .arg(_TOT, 19, 'e', 12);
1331
1332 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1333
1334 out << QString(fmt)
1335 .arg(1.e-3*_x_pos, 19, 'e', 12)
1336 .arg(1.e-3*_x_velocity, 19, 'e', 12)
1337 .arg(1.e-3*_x_acceleration, 19, 'e', 12)
1338 .arg(_health, 19, 'e', 12);
1339
1340 out << QString(fmt)
1341 .arg(1.e-3*_y_pos, 19, 'e', 12)
1342 .arg(1.e-3*_y_velocity, 19, 'e', 12)
1343 .arg(1.e-3*_y_acceleration, 19, 'e', 12)
1344 .arg(_ura, 19, 'e', 12);
1345
1346 out << QString(fmt)
1347 .arg(1.e-3*_z_pos, 19, 'e', 12)
1348 .arg(1.e-3*_z_velocity, 19, 'e', 12)
1349 .arg(1.e-3*_z_acceleration, 19, 'e', 12)
1350 .arg(double(_IODN), 19, 'e', 12);
1351
1352 return rnxStr;
1353}
1354
1355// Constructor
1356//////////////////////////////////////////////////////////////////////////////
1357t_ephBDS::t_ephBDS(float rnxVersion, const QStringList& lines) {
1358
1359 const int nLines = 8;
1360
1361 if (lines.size() != nLines) {
1362 _checkState = bad;
1363 return;
1364 }
1365
1366 // RINEX Format
1367 // ------------
1368 int fieldLen = 19;
1369
1370 int pos[4];
1371 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
1372 pos[1] = pos[0] + fieldLen;
1373 pos[2] = pos[1] + fieldLen;
1374 pos[3] = pos[2] + fieldLen;
1375
1376 // Read eight lines
1377 // ----------------
1378 for (int iLine = 0; iLine < nLines; iLine++) {
1379 QString line = lines[iLine];
1380
1381 if ( iLine == 0 ) {
1382 QTextStream in(line.left(pos[1]).toLatin1());
1383
1384 int year, month, day, hour, min;
1385 double sec;
1386
1387 QString prnStr, n;
1388 in >> prnStr;
1389 if (prnStr.size() == 1 && prnStr[0] == 'C') {
1390 in >> n;
1391 prnStr.append(n);
1392 }
1393 in >> year >> month >> day >> hour >> min >> sec;
1394 if (prnStr.at(0) == 'C') {
1395 _prn.set('C', prnStr.mid(1).toInt());
1396 }
1397 else {
1398 _prn.set('C', prnStr.toInt());
1399 }
1400
1401 if (year < 80) {
1402 year += 2000;
1403 }
1404 else if (year < 100) {
1405 year += 1900;
1406 }
1407
1408 _TOC.setBDS(year, month, day, hour, min, sec);
1409
1410 if ( readDbl(line, pos[1], fieldLen, _clock_bias ) ||
1411 readDbl(line, pos[2], fieldLen, _clock_drift ) ||
1412 readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
1413 _checkState = bad;
1414 return;
1415 }
1416 }
1417
1418 else if ( iLine == 1 ) {
1419 double aode;
1420 if ( readDbl(line, pos[0], fieldLen, aode ) ||
1421 readDbl(line, pos[1], fieldLen, _Crs ) ||
1422 readDbl(line, pos[2], fieldLen, _Delta_n) ||
1423 readDbl(line, pos[3], fieldLen, _M0 ) ) {
1424 _checkState = bad;
1425 return;
1426 }
1427 _AODE = int(aode);
1428 }
1429
1430 else if ( iLine == 2 ) {
1431 if ( readDbl(line, pos[0], fieldLen, _Cuc ) ||
1432 readDbl(line, pos[1], fieldLen, _e ) ||
1433 readDbl(line, pos[2], fieldLen, _Cus ) ||
1434 readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
1435 _checkState = bad;
1436 return;
1437 }
1438 }
1439
1440 else if ( iLine == 3 ) {
1441 if ( readDbl(line, pos[0], fieldLen, _TOEsec ) ||
1442 readDbl(line, pos[1], fieldLen, _Cic ) ||
1443 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
1444 readDbl(line, pos[3], fieldLen, _Cis ) ) {
1445 _checkState = bad;
1446 return;
1447 }
1448 }
1449
1450 else if ( iLine == 4 ) {
1451 if ( readDbl(line, pos[0], fieldLen, _i0 ) ||
1452 readDbl(line, pos[1], fieldLen, _Crc ) ||
1453 readDbl(line, pos[2], fieldLen, _omega ) ||
1454 readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
1455 _checkState = bad;
1456 return;
1457 }
1458 }
1459
1460 else if ( iLine == 5 ) {
1461 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
1462 readDbl(line, pos[2], fieldLen, _TOEweek)) {
1463 _checkState = bad;
1464 return;
1465 }
1466 }
1467
1468 else if ( iLine == 6 ) {
1469 double SatH1;
1470 if ( readDbl(line, pos[0], fieldLen, _URA ) ||
1471 readDbl(line, pos[1], fieldLen, SatH1) ||
1472 readDbl(line, pos[2], fieldLen, _TGD1) ||
1473 readDbl(line, pos[3], fieldLen, _TGD2) ) {
1474 _checkState = bad;
1475 return;
1476 }
1477 _SatH1 = int(SatH1);
1478 }
1479
1480 else if ( iLine == 7 ) {
1481 double aodc;
1482 if ( readDbl(line, pos[0], fieldLen, _TOT) ||
1483 readDbl(line, pos[1], fieldLen, aodc) ) {
1484 _checkState = bad;
1485 return;
1486 }
1487 if (_TOT == 0.9999e9) { // 0.9999e9 means not known (RINEX standard)
1488 _TOT = _TOEsec;
1489 }
1490 _AODC = int(aodc);
1491 }
1492 }
1493
1494 _TOE.setBDS(int(_TOEweek), _TOEsec);
1495
1496 // remark: actually should be computed from second_tot
1497 // but it seems to be unreliable in RINEX files
1498 //_TOT = _TOC.bdssec();
1499}
1500
1501// IOD of BDS Ephemeris (virtual)
1502////////////////////////////////////////////////////////////////////////////
1503unsigned int t_ephBDS::IOD() const {
1504 return (int(_TOEsec)/720) % 240;
1505}
1506
1507// Compute BDS Satellite Position (virtual)
1508//////////////////////////////////////////////////////////////////////////////
1509t_irc t_ephBDS::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
1510
1511 static const double gmBDS = 398.6004418e12;
1512 static const double omegaBDS = 7292115.0000e-11;
1513
1514 xc[0] = xc[1] = xc[2] = xc[3] = 0.0;
1515 vv[0] = vv[1] = vv[2] = 0.0;
1516
1517 bncTime tt(GPSweek, GPSweeks);
1518
1519 if (_sqrt_A == 0) {
1520 return failure;
1521 }
1522 double a0 = _sqrt_A * _sqrt_A;
1523
1524 double n0 = sqrt(gmBDS/(a0*a0*a0));
1525 double tk = tt - _TOE;
1526 double n = n0 + _Delta_n;
1527 double M = _M0 + n*tk;
1528 double E = M;
1529 double E_last;
1530 int nLoop = 0;
1531 do {
1532 E_last = E;
1533 E = M + _e*sin(E);
1534
1535 if (++nLoop == 100) {
1536 return failure;
1537 }
1538 } while ( fabs(E-E_last)*a0 > 0.001 );
1539
1540 double v = atan2(sqrt(1-_e*_e) * sin(E), cos(E) - _e);
1541 double u0 = v + _omega;
1542 double sin2u0 = sin(2*u0);
1543 double cos2u0 = cos(2*u0);
1544 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
1545 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
1546 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
1547 double xp = r*cos(u);
1548 double yp = r*sin(u);
1549 double toesec = (_TOE.gpssec() - 14.0);
1550 double sinom = 0;
1551 double cosom = 0;
1552 double sini = 0;
1553 double cosi = 0;
1554
1555 // Velocity
1556 // --------
1557 double tanv2 = tan(v/2);
1558 double dEdM = 1 / (1 - _e*cos(E));
1559 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2)
1560 / (1 + tanv2*tanv2) * dEdM * n;
1561 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
1562 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
1563 double dotr = a0 * _e*sin(E) * dEdM * n
1564 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
1565
1566 double dotx = dotr*cos(u) - r*sin(u)*dotu;
1567 double doty = dotr*sin(u) + r*cos(u)*dotu;
1568
1569 const double iMaxGEO = 10.0 / 180.0 * M_PI;
1570
1571 // MEO/IGSO satellite
1572 // ------------------
1573 if (_i0 > iMaxGEO) {
1574 double OM = _OMEGA0 + (_OMEGADOT - omegaBDS)*tk - omegaBDS*toesec;
1575
1576 sinom = sin(OM);
1577 cosom = cos(OM);
1578 sini = sin(i);
1579 cosi = cos(i);
1580
1581 xc[0] = xp*cosom - yp*cosi*sinom;
1582 xc[1] = xp*sinom + yp*cosi*cosom;
1583 xc[2] = yp*sini;
1584
1585 // Velocity
1586 // --------
1587
1588 double dotom = _OMEGADOT - t_CST::omega;
1589
1590 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
1591 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
1592 + yp*sini*sinom*doti; // dX / di
1593
1594 vv[1] = sinom *dotx + cosi*cosom *doty
1595 + xp*cosom*dotom - yp*cosi*sinom*dotom
1596 - yp*sini*cosom*doti;
1597
1598 vv[2] = sini *doty + yp*cosi *doti;
1599
1600 }
1601
1602 // GEO satellite
1603 // -------------
1604 else {
1605 double OM = _OMEGA0 + _OMEGADOT*tk - omegaBDS*toesec;
1606 double ll = omegaBDS*tk;
1607
1608 sinom = sin(OM);
1609 cosom = cos(OM);
1610 sini = sin(i);
1611 cosi = cos(i);
1612
1613 double xx = xp*cosom - yp*cosi*sinom;
1614 double yy = xp*sinom + yp*cosi*cosom;
1615 double zz = yp*sini;
1616
1617 Matrix RX = BNC_PPP::t_astro::rotX(-5.0 / 180.0 * M_PI);
1618 Matrix RZ = BNC_PPP::t_astro::rotZ(ll);
1619
1620 ColumnVector X1(3); X1 << xx << yy << zz;
1621 ColumnVector X2 = RZ*RX*X1;
1622
1623 xc[0] = X2(1);
1624 xc[1] = X2(2);
1625 xc[2] = X2(3);
1626
1627 double dotom = _OMEGADOT;
1628
1629 double vx = cosom *dotx - cosi*sinom *doty
1630 - xp*sinom*dotom - yp*cosi*cosom*dotom
1631 + yp*sini*sinom*doti;
1632
1633 double vy = sinom *dotx + cosi*cosom *doty
1634 + xp*cosom*dotom - yp*cosi*sinom*dotom
1635 - yp*sini*cosom*doti;
1636
1637 double vz = sini *doty + yp*cosi *doti;
1638
1639 ColumnVector V(3); V << vx << vy << vz;
1640
1641 Matrix RdotZ(3,3);
1642 double C = cos(ll);
1643 double S = sin(ll);
1644 Matrix UU(3,3);
1645 UU[0][0] = -S; UU[0][1] = +C; UU[0][2] = 0.0;
1646 UU[1][0] = -C; UU[1][1] = -S; UU[1][2] = 0.0;
1647 UU[2][0] = 0.0; UU[2][1] = 0.0; UU[2][2] = 0.0;
1648 RdotZ = omegaBDS * UU;
1649
1650 ColumnVector VV(3);
1651 VV = RZ*RX*V + RdotZ*RX*X1;
1652
1653 vv[0] = VV(1);
1654 vv[1] = VV(2);
1655 vv[2] = VV(3);
1656 }
1657
1658 double tc = tt - _TOC;
1659 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
1660
1661 // dotC = _clock_drift + _clock_driftrate*tc
1662 // - 4.442807633e-10*_e*sqrt(a0)*cos(E) * dEdM * n;
1663
1664 // Relativistic Correction
1665 // -----------------------
1666 // correspondent to BDS ICD and to SSR standard
1667 xc[3] -= 4.442807633e-10 * _e * sqrt(a0) *sin(E);
1668 // correspondent to IGS convention
1669 // xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c;
1670
1671 xc[4] = _clock_drift + _clock_driftrate*tc;
1672 xc[5] = _clock_driftrate;
1673 return success;
1674}
1675
1676// RINEX Format String
1677//////////////////////////////////////////////////////////////////////////////
1678QString t_ephBDS::toString(double version) const {
1679
1680 QString rnxStr = rinexDateStr(_TOC-14.0, _prn, version);
1681
1682 QTextStream out(&rnxStr);
1683
1684 out << QString("%1%2%3\n")
1685 .arg(_clock_bias, 19, 'e', 12)
1686 .arg(_clock_drift, 19, 'e', 12)
1687 .arg(_clock_driftrate, 19, 'e', 12);
1688
1689 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1690
1691 out << QString(fmt)
1692 .arg(double(_AODE), 19, 'e', 12)
1693 .arg(_Crs, 19, 'e', 12)
1694 .arg(_Delta_n, 19, 'e', 12)
1695 .arg(_M0, 19, 'e', 12);
1696
1697 out << QString(fmt)
1698 .arg(_Cuc, 19, 'e', 12)
1699 .arg(_e, 19, 'e', 12)
1700 .arg(_Cus, 19, 'e', 12)
1701 .arg(_sqrt_A, 19, 'e', 12);
1702
1703 double toes = 0.0;
1704 if (_TOEweek > -1.0) {// RINEX input
1705 toes = _TOEsec;
1706 }
1707 else {// RTCM stream input
1708 toes = _TOE.bdssec();
1709 }
1710 out << QString(fmt)
1711 .arg(toes, 19, 'e', 12)
1712 .arg(_Cic, 19, 'e', 12)
1713 .arg(_OMEGA0, 19, 'e', 12)
1714 .arg(_Cis, 19, 'e', 12);
1715
1716 out << QString(fmt)
1717 .arg(_i0, 19, 'e', 12)
1718 .arg(_Crc, 19, 'e', 12)
1719 .arg(_omega, 19, 'e', 12)
1720 .arg(_OMEGADOT, 19, 'e', 12);
1721
1722 double toew = 0.0;
1723 if (_TOEweek > -1.0) {// RINEX input
1724 toew = _TOEweek;
1725 }
1726 else {// RTCM stream input
1727 toew = double(_TOE.bdsw());
1728 }
1729 out << QString(fmt)
1730 .arg(_IDOT, 19, 'e', 12)
1731 .arg(0.0, 19, 'e', 12)
1732 .arg(toew, 19, 'e', 12)
1733 .arg(0.0, 19, 'e', 12);
1734
1735 out << QString(fmt)
1736 .arg(_URA, 19, 'e', 12)
1737 .arg(double(_SatH1), 19, 'e', 12)
1738 .arg(_TGD1, 19, 'e', 12)
1739 .arg(_TGD2, 19, 'e', 12);
1740
1741 double tots = 0.0;
1742 if (_TOEweek > -1.0) {// RINEX input
1743 tots = _TOT;
1744 }
1745 else {// RTCM stream input
1746 tots = _TOE.bdssec();
1747 }
1748 out << QString(fmt)
1749 .arg(tots, 19, 'e', 12)
1750 .arg(double(_AODC), 19, 'e', 12)
1751 .arg("", 19, QChar(' '))
1752 .arg("", 19, QChar(' '));
1753 return rnxStr;
1754}
Note: See TracBrowser for help on using the repository browser.