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

Last change on this file since 7056 was 7054, checked in by stuerze, 10 years ago

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