source: ntrip/branches/BNC_2.12/src/ephemeris.cpp@ 8016

Last change on this file since 8016 was 8006, checked in by stuerze, 8 years ago

minor changes

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