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

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

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