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

Last change on this file since 7481 was 7481, checked in by stuerze, 9 years ago

distinction of GEO/MEO satellites during BDS velocity determination

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