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

Last change on this file since 8697 was 8697, checked in by stuerze, 6 years ago

minor changes

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