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

Last change on this file since 8650 was 8650, checked in by stuerze, 5 years ago

try to prevent crasches in case of GLONASS satellite coordinate determination
File size: 48.8 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);
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 || _xv.size() != 6) {
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 _xv = rungeKutta4(_tt.gpssec(), _xv, step, acc, glo_deriv);
628 _tt = _tt + step;
629 }
630
631 // Position and Velocity
632 // ---------------------
633 xc[0] = _xv(1);
634 xc[1] = _xv(2);
635 xc[2] = _xv(3);
636
637 vv[0] = _xv(4);
638 vv[1] = _xv(5);
639 vv[2] = _xv(6);
640
641 // Clock Correction
642 // ----------------
643 double dtClk = bncTime(GPSweek, GPSweeks) - _TOC;
644 xc[3] = -_tau + _gamma * dtClk;
645
646 xc[4] = _gamma;
647 xc[5] = 0.0;
648
649 return success;
650}
651
652// RINEX Format String
653//////////////////////////////////////////////////////////////////////////////
654QString t_ephGlo::toString(double version) const {
655
656 QString rnxStr = rinexDateStr(_TOC-_gps_utc, _prn, version);
657
658 QTextStream out(&rnxStr);
659
660 out << QString("%1%2%3\n")
661 .arg(-_tau, 19, 'e', 12)
662 .arg(_gamma, 19, 'e', 12)
663 .arg(_tki, 19, 'e', 12);
664
665 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
666
667 out << QString(fmt)
668 .arg(_x_pos, 19, 'e', 12)
669 .arg(_x_velocity, 19, 'e', 12)
670 .arg(_x_acceleration, 19, 'e', 12)
671 .arg(_health, 19, 'e', 12);
672
673 out << QString(fmt)
674 .arg(_y_pos, 19, 'e', 12)
675 .arg(_y_velocity, 19, 'e', 12)
676 .arg(_y_acceleration, 19, 'e', 12)
677 .arg(_frequency_number, 19, 'e', 12);
678
679 out << QString(fmt)
680 .arg(_z_pos, 19, 'e', 12)
681 .arg(_z_velocity, 19, 'e', 12)
682 .arg(_z_acceleration, 19, 'e', 12)
683 .arg(_E, 19, 'e', 12);
684
685 return rnxStr;
686}
687
688// Derivative of the state vector using a simple force model (static)
689////////////////////////////////////////////////////////////////////////////
690ColumnVector t_ephGlo::glo_deriv(double /* tt */, const ColumnVector& xv,
691 double* acc) {
692
693 // State vector components
694 // -----------------------
695 ColumnVector rr = xv.rows(1,3);
696 ColumnVector vv = xv.rows(4,6);
697
698 // Acceleration
699 // ------------
700 static const double gmWGS = 398.60044e12;
701 static const double AE = 6378136.0;
702 static const double OMEGA = 7292115.e-11;
703 static const double C20 = -1082.6257e-6;
704
705 double rho = rr.norm_Frobenius();
706 double t1 = -gmWGS/(rho*rho*rho);
707 double t2 = 3.0/2.0 * C20 * (gmWGS*AE*AE) / (rho*rho*rho*rho*rho);
708 double t3 = OMEGA * OMEGA;
709 double t4 = 2.0 * OMEGA;
710 double z2 = rr(3) * rr(3);
711
712 // Vector of derivatives
713 // ---------------------
714 ColumnVector va(6);
715 va(1) = vv(1);
716 va(2) = vv(2);
717 va(3) = vv(3);
718 va(4) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2) + acc[0];
719 va(5) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1) + acc[1];
720 va(6) = (t1 + t2*(3.0-5.0*z2/(rho*rho)) ) * rr(3) + acc[2];
721
722 return va;
723}
724
725// IOD of Glonass Ephemeris (virtual)
726////////////////////////////////////////////////////////////////////////////
727unsigned int t_ephGlo::IOD() const {
728 bncTime tMoscow = _TOC - _gps_utc + 3 * 3600.0;
729 return (unsigned long)tMoscow.daysec() / 900;
730}
731
732// Health status of Glonass Ephemeris (virtual)
733////////////////////////////////////////////////////////////////////////////
734unsigned int t_ephGlo::isUnhealthy() const {
735
736 if (_almanac_health_availablility_indicator) {
737 if ((_health == 0 && _almanac_health == 0) ||
738 (_health == 1 && _almanac_health == 0) ||
739 (_health == 1 && _almanac_health == 1)) {
740 return 1;
741 }
742 }
743 else if (!_almanac_health_availablility_indicator) {
744 if (_health) {
745 return 1;
746 }
747 }
748 return 0; /* (_health == 0 && _almanac_health == 1) or (_health == 0) */
749}
750
751// Constructor
752//////////////////////////////////////////////////////////////////////////////
753t_ephGal::t_ephGal(float rnxVersion, const QStringList& lines) {
754 int year, month, day, hour, min;
755 double sec;
756 QString prnStr;
757 const int nLines = 8;
758 if (lines.size() != nLines) {
759 _checkState = bad;
760 return;
761 }
762
763 // RINEX Format
764 // ------------
765 int fieldLen = 19;
766 double SVhealth = 0.0;
767 double datasource = 0.0;
768
769 int pos[4];
770 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
771 pos[1] = pos[0] + fieldLen;
772 pos[2] = pos[1] + fieldLen;
773 pos[3] = pos[2] + fieldLen;
774
775 // Read eight lines
776 // ----------------
777 for (int iLine = 0; iLine < nLines; iLine++) {
778 QString line = lines[iLine];
779
780 if ( iLine == 0 ) {
781 QTextStream in(line.left(pos[1]).toAscii());
782 QString n;
783 in >> prnStr;
784 if (prnStr.size() == 1 && prnStr[0] == 'E') {
785 in >> n;
786 prnStr.append(n);
787 }
788 in >> year >> month >> day >> hour >> min >> sec;
789 if (year < 80) {
790 year += 2000;
791 }
792 else if (year < 100) {
793 year += 1900;
794 }
795
796 _TOC.set(year, month, day, hour, min, sec);
797
798 if ( readDbl(line, pos[1], fieldLen, _clock_bias ) ||
799 readDbl(line, pos[2], fieldLen, _clock_drift ) ||
800 readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
801 _checkState = bad;
802 return;
803 }
804 }
805
806 else if ( iLine == 1 ) {
807 if ( readDbl(line, pos[0], fieldLen, _IODnav ) ||
808 readDbl(line, pos[1], fieldLen, _Crs ) ||
809 readDbl(line, pos[2], fieldLen, _Delta_n) ||
810 readDbl(line, pos[3], fieldLen, _M0 ) ) {
811 _checkState = bad;
812 return;
813 }
814 }
815
816 else if ( iLine == 2 ) {
817 if ( readDbl(line, pos[0], fieldLen, _Cuc ) ||
818 readDbl(line, pos[1], fieldLen, _e ) ||
819 readDbl(line, pos[2], fieldLen, _Cus ) ||
820 readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
821 _checkState = bad;
822 return;
823 }
824 }
825
826 else if ( iLine == 3 ) {
827 if ( readDbl(line, pos[0], fieldLen, _TOEsec) ||
828 readDbl(line, pos[1], fieldLen, _Cic ) ||
829 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
830 readDbl(line, pos[3], fieldLen, _Cis ) ) {
831 _checkState = bad;
832 return;
833 }
834 }
835
836 else if ( iLine == 4 ) {
837 if ( readDbl(line, pos[0], fieldLen, _i0 ) ||
838 readDbl(line, pos[1], fieldLen, _Crc ) ||
839 readDbl(line, pos[2], fieldLen, _omega ) ||
840 readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
841 _checkState = bad;
842 return;
843 }
844 }
845
846 else if ( iLine == 5 ) {
847 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
848 readDbl(line, pos[1], fieldLen, datasource) ||
849 readDbl(line, pos[2], fieldLen, _TOEweek ) ) {
850 _checkState = bad;
851 return;
852 } else {
853 if (int(datasource) & (1<<8)) {
854 _fnav = true;
855 _inav = false;
856 } else if (int(datasource) & (1<<9)) {
857 _fnav = false;
858 _inav = true;
859 }
860 _TOEweek -= 1024.0;
861 }
862 }
863
864 else if ( iLine == 6 ) {
865 if ( readDbl(line, pos[0], fieldLen, _SISA ) ||
866 readDbl(line, pos[1], fieldLen, SVhealth) ||
867 readDbl(line, pos[2], fieldLen, _BGD_1_5A) ||
868 readDbl(line, pos[3], fieldLen, _BGD_1_5B) ) {
869 _checkState = bad;
870 return;
871 } else {
872 // Bit 0
873 _e1DataInValid = (int(SVhealth) & (1<<0));
874 // Bit 1-2
875 _E1_bHS = double((int(SVhealth) >> 1) & 0x3);
876 // Bit 3
877 _e5aDataInValid = (int(SVhealth) & (1<<3));
878 // Bit 4-5
879 _E5aHS = double((int(SVhealth) >> 4) & 0x3);
880 // Bit 6
881 _e5bDataInValid = (int(SVhealth) & (1<<6));
882 // Bit 7-8
883 _E5bHS = double((int(SVhealth) >> 7) & 0x3);
884
885 if (prnStr.at(0) == 'E') {
886 _prn.set('E', prnStr.mid(1).toInt(), _inav ? 1 : 0);
887 }
888 }
889 }
890
891 else if ( iLine == 7 ) {
892 if ( readDbl(line, pos[0], fieldLen, _TOT) ) {
893 _checkState = bad;
894 return;
895 }
896 }
897 }
898}
899
900// Compute Galileo Satellite Position (virtual)
901////////////////////////////////////////////////////////////////////////////
902t_irc t_ephGal::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
903
904 static const double omegaEarth = 7292115.1467e-11;
905 static const double gmWGS = 398.6004418e12;
906
907 memset(xc, 0, 6*sizeof(double));
908 memset(vv, 0, 3*sizeof(double));
909
910 double a0 = _sqrt_A * _sqrt_A;
911 if (a0 == 0) {
912 return failure;
913 }
914
915 double n0 = sqrt(gmWGS/(a0*a0*a0));
916
917 bncTime tt(GPSweek, GPSweeks);
918 double tk = tt - bncTime(_TOC.gpsw(), _TOEsec);
919
920 double n = n0 + _Delta_n;
921 double M = _M0 + n*tk;
922 double E = M;
923 double E_last;
924 int nLoop = 0;
925 do {
926 E_last = E;
927 E = M + _e*sin(E);
928
929 if (++nLoop == 100) {
930 return failure;
931 }
932 } while ( fabs(E-E_last)*a0 > 0.001 );
933 double v = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) );
934 double u0 = v + _omega;
935 double sin2u0 = sin(2*u0);
936 double cos2u0 = cos(2*u0);
937 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
938 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
939 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
940 double xp = r*cos(u);
941 double yp = r*sin(u);
942 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
943 omegaEarth*_TOEsec;
944
945 double sinom = sin(OM);
946 double cosom = cos(OM);
947 double sini = sin(i);
948 double cosi = cos(i);
949 xc[0] = xp*cosom - yp*cosi*sinom;
950 xc[1] = xp*sinom + yp*cosi*cosom;
951 xc[2] = yp*sini;
952
953 double tc = tt - _TOC;
954 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
955
956 // Velocity
957 // --------
958 double tanv2 = tan(v/2);
959 double dEdM = 1 / (1 - _e*cos(E));
960 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
961 * dEdM * n;
962 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
963 double dotom = _OMEGADOT - omegaEarth;
964 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
965 double dotr = a0 * _e*sin(E) * dEdM * n
966 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
967 double dotx = dotr*cos(u) - r*sin(u)*dotu;
968 double doty = dotr*sin(u) + r*cos(u)*dotu;
969
970 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
971 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
972 + yp*sini*sinom*doti; // dX / di
973
974 vv[1] = sinom *dotx + cosi*cosom *doty
975 + xp*cosom*dotom - yp*cosi*sinom*dotom
976 - yp*sini*cosom*doti;
977
978 vv[2] = sini *doty + yp*cosi *doti;
979
980 // Relativistic Correction
981 // -----------------------
982 // correspondent to Galileo ICD and to SSR standard
983 xc[3] -= 4.442807633e-10 * _e * sqrt(a0) *sin(E);
984 // correspondent to IGS convention
985 //xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c;
986
987 xc[4] = _clock_drift + _clock_driftrate*tc;
988 xc[5] = _clock_driftrate;
989
990 return success;
991}
992
993// Health status of Galileo Ephemeris (virtual)
994////////////////////////////////////////////////////////////////////////////
995unsigned int t_ephGal::isUnhealthy() const {
996 if (_E5aHS && _E5bHS && _E1_bHS) {
997 return 1;
998 }
999 return 0;
1000}
1001
1002// RINEX Format String
1003//////////////////////////////////////////////////////////////////////////////
1004QString t_ephGal::toString(double version) const {
1005
1006 QString rnxStr = rinexDateStr(_TOC, _prn, version);
1007
1008 QTextStream out(&rnxStr);
1009
1010 out << QString("%1%2%3\n")
1011 .arg(_clock_bias, 19, 'e', 12)
1012 .arg(_clock_drift, 19, 'e', 12)
1013 .arg(_clock_driftrate, 19, 'e', 12);
1014
1015 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1016
1017 out << QString(fmt)
1018 .arg(_IODnav, 19, 'e', 12)
1019 .arg(_Crs, 19, 'e', 12)
1020 .arg(_Delta_n, 19, 'e', 12)
1021 .arg(_M0, 19, 'e', 12);
1022
1023 out << QString(fmt)
1024 .arg(_Cuc, 19, 'e', 12)
1025 .arg(_e, 19, 'e', 12)
1026 .arg(_Cus, 19, 'e', 12)
1027 .arg(_sqrt_A, 19, 'e', 12);
1028
1029 out << QString(fmt)
1030 .arg(_TOEsec, 19, 'e', 12)
1031 .arg(_Cic, 19, 'e', 12)
1032 .arg(_OMEGA0, 19, 'e', 12)
1033 .arg(_Cis, 19, 'e', 12);
1034
1035 out << QString(fmt)
1036 .arg(_i0, 19, 'e', 12)
1037 .arg(_Crc, 19, 'e', 12)
1038 .arg(_omega, 19, 'e', 12)
1039 .arg(_OMEGADOT, 19, 'e', 12);
1040
1041 int dataSource = 0;
1042 int SVhealth = 0;
1043 double BGD_1_5A = _BGD_1_5A;
1044 double BGD_1_5B = _BGD_1_5B;
1045 if (_fnav) {
1046 dataSource |= (1<<1);
1047 dataSource |= (1<<8);
1048 BGD_1_5B = 0.0;
1049 // SVhealth
1050 // Bit 3 : E5a DVS
1051 if (_e5aDataInValid) {
1052 SVhealth |= (1<<3);
1053 }
1054 // Bit 4-5: E5a HS
1055 if (_E5aHS == 1.0) {
1056 SVhealth |= (1<<4);
1057 }
1058 else if (_E5aHS == 2.0) {
1059 SVhealth |= (1<<5);
1060 }
1061 else if (_E5aHS == 3.0) {
1062 SVhealth |= (1<<4);
1063 SVhealth |= (1<<5);
1064 }
1065 }
1066 else if(_inav) {
1067 // Bit 2 and 0 are set because from MT1046 the data source cannot be determined
1068 // and RNXv3.03 says both can be set if the navigation messages were merged
1069 dataSource |= (1<<0);
1070 dataSource |= (1<<2);
1071 dataSource |= (1<<9);
1072 // SVhealth
1073 // Bit 0 : E1-B DVS
1074 if (_e1DataInValid) {
1075 SVhealth |= (1<<0);
1076 }
1077 // Bit 1-2: E1-B HS
1078 if (_E1_bHS == 1.0) {
1079 SVhealth |= (1<<1);
1080 }
1081 else if (_E1_bHS == 2.0) {
1082 SVhealth |= (1<<2);
1083 }
1084 else if (_E1_bHS == 3.0) {
1085 SVhealth |= (1<<1);
1086 SVhealth |= (1<<2);
1087 }
1088 // Bit 3 : E5a DVS
1089 if (_e5aDataInValid) {
1090 SVhealth |= (1<<3);
1091 }
1092 // Bit 4-5: E5a HS
1093 if (_E5aHS == 1.0) {
1094 SVhealth |= (1<<4);
1095 }
1096 else if (_E5aHS == 2.0) {
1097 SVhealth |= (1<<5);
1098 }
1099 else if (_E5aHS == 3.0) {
1100 SVhealth |= (1<<4);
1101 SVhealth |= (1<<5);
1102 }
1103 // Bit 6 : E5b DVS
1104 if (_e5bDataInValid) {
1105 SVhealth |= (1<<6);
1106 }
1107 // Bit 7-8: E5b HS
1108 if (_E5bHS == 1.0) {
1109 SVhealth |= (1<<7);
1110 }
1111 else if (_E5bHS == 2.0) {
1112 SVhealth |= (1<<8);
1113 }
1114 else if (_E5bHS == 3.0) {
1115 SVhealth |= (1<<7);
1116 SVhealth |= (1<<8);
1117 }
1118 }
1119
1120 out << QString(fmt)
1121 .arg(_IDOT, 19, 'e', 12)
1122 .arg(double(dataSource), 19, 'e', 12)
1123 .arg(_TOEweek + 1024.0, 19, 'e', 12)
1124 .arg(0.0, 19, 'e', 12);
1125
1126 out << QString(fmt)
1127 .arg(_SISA, 19, 'e', 12)
1128 .arg(double(SVhealth), 19, 'e', 12)
1129 .arg(BGD_1_5A, 19, 'e', 12)
1130 .arg(BGD_1_5B, 19, 'e', 12);
1131
1132 double tot = _TOT;
1133 if (tot == 0.9999e9 && version < 3.0) {
1134 tot = 0.0;
1135 }
1136 out << QString(fmt)
1137 .arg(tot, 19, 'e', 12)
1138 .arg("", 19, QChar(' '))
1139 .arg("", 19, QChar(' '))
1140 .arg("", 19, QChar(' '));
1141
1142 return rnxStr;
1143}
1144
1145// Constructor
1146//////////////////////////////////////////////////////////////////////////////
1147t_ephSBAS::t_ephSBAS(float rnxVersion, const QStringList& lines) {
1148
1149 const int nLines = 4;
1150
1151 if (lines.size() != nLines) {
1152 _checkState = bad;
1153 return;
1154 }
1155
1156 // RINEX Format
1157 // ------------
1158 int fieldLen = 19;
1159
1160 int pos[4];
1161 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
1162 pos[1] = pos[0] + fieldLen;
1163 pos[2] = pos[1] + fieldLen;
1164 pos[3] = pos[2] + fieldLen;
1165
1166 // Read four lines
1167 // ---------------
1168 for (int iLine = 0; iLine < nLines; iLine++) {
1169 QString line = lines[iLine];
1170
1171 if ( iLine == 0 ) {
1172 QTextStream in(line.left(pos[1]).toAscii());
1173
1174 int year, month, day, hour, min;
1175 double sec;
1176
1177 QString prnStr, n;
1178 in >> prnStr;
1179 if (prnStr.size() == 1 && prnStr[0] == 'S') {
1180 in >> n;
1181 prnStr.append(n);
1182 }
1183 in >> year >> month >> day >> hour >> min >> sec;
1184 if (prnStr.at(0) == 'S') {
1185 _prn.set('S', prnStr.mid(1).toInt());
1186 }
1187 else {
1188 _prn.set('S', prnStr.toInt());
1189 }
1190
1191 if (year < 80) {
1192 year += 2000;
1193 }
1194 else if (year < 100) {
1195 year += 1900;
1196 }
1197
1198 _TOC.set(year, month, day, hour, min, sec);
1199
1200 if ( readDbl(line, pos[1], fieldLen, _agf0 ) ||
1201 readDbl(line, pos[2], fieldLen, _agf1 ) ||
1202 readDbl(line, pos[3], fieldLen, _TOT ) ) {
1203 _checkState = bad;
1204 return;
1205 }
1206 }
1207
1208 else if ( iLine == 1 ) {
1209 if ( readDbl(line, pos[0], fieldLen, _x_pos ) ||
1210 readDbl(line, pos[1], fieldLen, _x_velocity ) ||
1211 readDbl(line, pos[2], fieldLen, _x_acceleration) ||
1212 readDbl(line, pos[3], fieldLen, _health ) ) {
1213 _checkState = bad;
1214 return;
1215 }
1216 }
1217
1218 else if ( iLine == 2 ) {
1219 if ( readDbl(line, pos[0], fieldLen, _y_pos ) ||
1220 readDbl(line, pos[1], fieldLen, _y_velocity ) ||
1221 readDbl(line, pos[2], fieldLen, _y_acceleration ) ||
1222 readDbl(line, pos[3], fieldLen, _ura ) ) {
1223 _checkState = bad;
1224 return;
1225 }
1226 }
1227
1228 else if ( iLine == 3 ) {
1229 double iodn;
1230 if ( readDbl(line, pos[0], fieldLen, _z_pos ) ||
1231 readDbl(line, pos[1], fieldLen, _z_velocity ) ||
1232 readDbl(line, pos[2], fieldLen, _z_acceleration) ||
1233 readDbl(line, pos[3], fieldLen, iodn ) ) {
1234 _checkState = bad;
1235 return;
1236 } else {
1237 _IODN = int(iodn);
1238 }
1239 }
1240 }
1241
1242 _x_pos *= 1.e3;
1243 _y_pos *= 1.e3;
1244 _z_pos *= 1.e3;
1245 _x_velocity *= 1.e3;
1246 _y_velocity *= 1.e3;
1247 _z_velocity *= 1.e3;
1248 _x_acceleration *= 1.e3;
1249 _y_acceleration *= 1.e3;
1250 _z_acceleration *= 1.e3;
1251}
1252
1253// IOD of SBAS Ephemeris (virtual)
1254////////////////////////////////////////////////////////////////////////////
1255
1256unsigned int t_ephSBAS::IOD() const {
1257 unsigned char buffer[80];
1258 int size = 0;
1259 int numbits = 0;
1260 long long bitbuffer = 0;
1261 unsigned char *startbuffer = buffer;
1262
1263 SBASADDBITSFLOAT(30, this->_x_pos, 0.08)
1264 SBASADDBITSFLOAT(30, this->_y_pos, 0.08)
1265 SBASADDBITSFLOAT(25, this->_z_pos, 0.4)
1266 SBASADDBITSFLOAT(17, this->_x_velocity, 0.000625)
1267 SBASADDBITSFLOAT(17, this->_y_velocity, 0.000625)
1268 SBASADDBITSFLOAT(18, this->_z_velocity, 0.004)
1269 SBASADDBITSFLOAT(10, this->_x_acceleration, 0.0000125)
1270 SBASADDBITSFLOAT(10, this->_y_acceleration, 0.0000125)
1271 SBASADDBITSFLOAT(10, this->_z_acceleration, 0.0000625)
1272 SBASADDBITSFLOAT(12, this->_agf0, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
1273 SBASADDBITSFLOAT(8, this->_agf1, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<10))
1274 SBASADDBITS(5,0); // the last byte is filled by 0-bits to obtain a length of an integer multiple of 8
1275
1276 return CRC24(size, startbuffer);
1277}
1278
1279// Compute SBAS Satellite Position (virtual)
1280////////////////////////////////////////////////////////////////////////////
1281t_irc t_ephSBAS::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
1282
1283 bncTime tt(GPSweek, GPSweeks);
1284 double dt = tt - _TOC;
1285
1286 xc[0] = _x_pos + _x_velocity * dt + _x_acceleration * dt * dt / 2.0;
1287 xc[1] = _y_pos + _y_velocity * dt + _y_acceleration * dt * dt / 2.0;
1288 xc[2] = _z_pos + _z_velocity * dt + _z_acceleration * dt * dt / 2.0;
1289
1290 vv[0] = _x_velocity + _x_acceleration * dt;
1291 vv[1] = _y_velocity + _y_acceleration * dt;
1292 vv[2] = _z_velocity + _z_acceleration * dt;
1293
1294 xc[3] = _agf0 + _agf1 * dt;
1295
1296 xc[4] = _agf1;
1297 xc[5] = 0.0;
1298
1299 return success;
1300}
1301
1302// RINEX Format String
1303//////////////////////////////////////////////////////////////////////////////
1304QString t_ephSBAS::toString(double version) const {
1305
1306 QString rnxStr = rinexDateStr(_TOC, _prn, version);
1307
1308 QTextStream out(&rnxStr);
1309
1310 out << QString("%1%2%3\n")
1311 .arg(_agf0, 19, 'e', 12)
1312 .arg(_agf1, 19, 'e', 12)
1313 .arg(_TOT, 19, 'e', 12);
1314
1315 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1316
1317 out << QString(fmt)
1318 .arg(1.e-3*_x_pos, 19, 'e', 12)
1319 .arg(1.e-3*_x_velocity, 19, 'e', 12)
1320 .arg(1.e-3*_x_acceleration, 19, 'e', 12)
1321 .arg(_health, 19, 'e', 12);
1322
1323 out << QString(fmt)
1324 .arg(1.e-3*_y_pos, 19, 'e', 12)
1325 .arg(1.e-3*_y_velocity, 19, 'e', 12)
1326 .arg(1.e-3*_y_acceleration, 19, 'e', 12)
1327 .arg(_ura, 19, 'e', 12);
1328
1329 out << QString(fmt)
1330 .arg(1.e-3*_z_pos, 19, 'e', 12)
1331 .arg(1.e-3*_z_velocity, 19, 'e', 12)
1332 .arg(1.e-3*_z_acceleration, 19, 'e', 12)
1333 .arg(double(_IODN), 19, 'e', 12);
1334
1335 return rnxStr;
1336}
1337
1338// Constructor
1339//////////////////////////////////////////////////////////////////////////////
1340t_ephBDS::t_ephBDS(float rnxVersion, const QStringList& lines) {
1341
1342 const int nLines = 8;
1343
1344 if (lines.size() != nLines) {
1345 _checkState = bad;
1346 return;
1347 }
1348
1349 // RINEX Format
1350 // ------------
1351 int fieldLen = 19;
1352
1353 int pos[4];
1354 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
1355 pos[1] = pos[0] + fieldLen;
1356 pos[2] = pos[1] + fieldLen;
1357 pos[3] = pos[2] + fieldLen;
1358
1359 // Read eight lines
1360 // ----------------
1361 for (int iLine = 0; iLine < nLines; iLine++) {
1362 QString line = lines[iLine];
1363
1364 if ( iLine == 0 ) {
1365 QTextStream in(line.left(pos[1]).toAscii());
1366
1367 int year, month, day, hour, min;
1368 double sec;
1369
1370 QString prnStr, n;
1371 in >> prnStr;
1372 if (prnStr.size() == 1 && prnStr[0] == 'C') {
1373 in >> n;
1374 prnStr.append(n);
1375 }
1376 in >> year >> month >> day >> hour >> min >> sec;
1377 if (prnStr.at(0) == 'C') {
1378 _prn.set('C', prnStr.mid(1).toInt());
1379 }
1380 else {
1381 _prn.set('C', prnStr.toInt());
1382 }
1383
1384 if (year < 80) {
1385 year += 2000;
1386 }
1387 else if (year < 100) {
1388 year += 1900;
1389 }
1390
1391 _TOC.setBDS(year, month, day, hour, min, sec);
1392
1393 if ( readDbl(line, pos[1], fieldLen, _clock_bias ) ||
1394 readDbl(line, pos[2], fieldLen, _clock_drift ) ||
1395 readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
1396 _checkState = bad;
1397 return;
1398 }
1399 }
1400
1401 else if ( iLine == 1 ) {
1402 double aode;
1403 if ( readDbl(line, pos[0], fieldLen, aode ) ||
1404 readDbl(line, pos[1], fieldLen, _Crs ) ||
1405 readDbl(line, pos[2], fieldLen, _Delta_n) ||
1406 readDbl(line, pos[3], fieldLen, _M0 ) ) {
1407 _checkState = bad;
1408 return;
1409 }
1410 _AODE = int(aode);
1411 }
1412
1413 else if ( iLine == 2 ) {
1414 if ( readDbl(line, pos[0], fieldLen, _Cuc ) ||
1415 readDbl(line, pos[1], fieldLen, _e ) ||
1416 readDbl(line, pos[2], fieldLen, _Cus ) ||
1417 readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
1418 _checkState = bad;
1419 return;
1420 }
1421 }
1422
1423 else if ( iLine == 3 ) {
1424 if ( readDbl(line, pos[0], fieldLen, _TOEsec ) ||
1425 readDbl(line, pos[1], fieldLen, _Cic ) ||
1426 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
1427 readDbl(line, pos[3], fieldLen, _Cis ) ) {
1428 _checkState = bad;
1429 return;
1430 }
1431 }
1432
1433 else if ( iLine == 4 ) {
1434 if ( readDbl(line, pos[0], fieldLen, _i0 ) ||
1435 readDbl(line, pos[1], fieldLen, _Crc ) ||
1436 readDbl(line, pos[2], fieldLen, _omega ) ||
1437 readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
1438 _checkState = bad;
1439 return;
1440 }
1441 }
1442
1443 else if ( iLine == 5 ) {
1444 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
1445 readDbl(line, pos[2], fieldLen, _TOEweek)) {
1446 _checkState = bad;
1447 return;
1448 }
1449 }
1450
1451 else if ( iLine == 6 ) {
1452 double SatH1;
1453 if ( readDbl(line, pos[0], fieldLen, _URA ) ||
1454 readDbl(line, pos[1], fieldLen, SatH1) ||
1455 readDbl(line, pos[2], fieldLen, _TGD1) ||
1456 readDbl(line, pos[3], fieldLen, _TGD2) ) {
1457 _checkState = bad;
1458 return;
1459 }
1460 _SatH1 = int(SatH1);
1461 }
1462
1463 else if ( iLine == 7 ) {
1464 double aodc;
1465 if ( readDbl(line, pos[0], fieldLen, _TOT) ||
1466 readDbl(line, pos[1], fieldLen, aodc) ) {
1467 _checkState = bad;
1468 return;
1469 }
1470 if (_TOT == 0.9999e9) { // 0.9999e9 means not known (RINEX standard)
1471 _TOT = _TOEsec;
1472 }
1473 _AODC = int(aodc);
1474 }
1475 }
1476
1477 _TOE.setBDS(int(_TOEweek), _TOEsec);
1478
1479 // remark: actually should be computed from second_tot
1480 // but it seems to be unreliable in RINEX files
1481 //_TOT = _TOC.bdssec();
1482}
1483
1484// IOD of BDS Ephemeris (virtual)
1485////////////////////////////////////////////////////////////////////////////
1486unsigned int t_ephBDS::IOD() const {
1487 unsigned char buffer[80];
1488 int size = 0;
1489 int numbits = 0;
1490 long long bitbuffer = 0;
1491 unsigned char *startbuffer = buffer;
1492
1493 BDSADDBITSFLOAT(14, this->_IDOT, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
1494 BDSADDBITSFLOAT(11, this->_clock_driftrate, 1.0/static_cast<double>(1<<30)
1495 /static_cast<double>(1<<30)/static_cast<double>(1<<6))
1496 BDSADDBITSFLOAT(22, this->_clock_drift, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<20))
1497 BDSADDBITSFLOAT(24, this->_clock_bias, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
1498 BDSADDBITSFLOAT(18, this->_Crs, 1.0/static_cast<double>(1<<6))
1499 BDSADDBITSFLOAT(16, this->_Delta_n, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
1500 BDSADDBITSFLOAT(32, this->_M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
1501 BDSADDBITSFLOAT(18, this->_Cuc, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
1502 BDSADDBITSFLOAT(32, this->_e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
1503 BDSADDBITSFLOAT(18, this->_Cus, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
1504 BDSADDBITSFLOAT(32, this->_sqrt_A, 1.0/static_cast<double>(1<<19))
1505 BDSADDBITSFLOAT(18, this->_Cic, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
1506 BDSADDBITSFLOAT(32, this->_OMEGA0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
1507 BDSADDBITSFLOAT(18, this->_Cis, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
1508 BDSADDBITSFLOAT(32, this->_i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
1509 BDSADDBITSFLOAT(18, this->_Crc, 1.0/static_cast<double>(1<<6))
1510 BDSADDBITSFLOAT(32, this->_omega, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
1511 BDSADDBITSFLOAT(24, this->_OMEGADOT, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
1512 BDSADDBITS(5, 0) // the last byte is filled by 0-bits to obtain a length of an integer multiple of 8
1513
1514 return CRC24(size, startbuffer);
1515}
1516
1517// Compute BDS Satellite Position (virtual)
1518//////////////////////////////////////////////////////////////////////////////
1519t_irc t_ephBDS::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
1520
1521 static const double gmBDS = 398.6004418e12;
1522 static const double omegaBDS = 7292115.0000e-11;
1523
1524 xc[0] = xc[1] = xc[2] = xc[3] = 0.0;
1525 vv[0] = vv[1] = vv[2] = 0.0;
1526
1527 bncTime tt(GPSweek, GPSweeks);
1528
1529 if (_sqrt_A == 0) {
1530 return failure;
1531 }
1532 double a0 = _sqrt_A * _sqrt_A;
1533
1534 double n0 = sqrt(gmBDS/(a0*a0*a0));
1535 double tk = tt - _TOE;
1536 double n = n0 + _Delta_n;
1537 double M = _M0 + n*tk;
1538 double E = M;
1539 double E_last;
1540 int nLoop = 0;
1541 do {
1542 E_last = E;
1543 E = M + _e*sin(E);
1544
1545 if (++nLoop == 100) {
1546 return failure;
1547 }
1548 } while ( fabs(E-E_last)*a0 > 0.001 );
1549
1550 double v = atan2(sqrt(1-_e*_e) * sin(E), cos(E) - _e);
1551 double u0 = v + _omega;
1552 double sin2u0 = sin(2*u0);
1553 double cos2u0 = cos(2*u0);
1554 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
1555 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
1556 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
1557 double xp = r*cos(u);
1558 double yp = r*sin(u);
1559 double toesec = (_TOE.gpssec() - 14.0);
1560 double sinom = 0;
1561 double cosom = 0;
1562 double sini = 0;
1563 double cosi = 0;
1564
1565 // Velocity
1566 // --------
1567 double tanv2 = tan(v/2);
1568 double dEdM = 1 / (1 - _e*cos(E));
1569 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2)
1570 / (1 + tanv2*tanv2) * dEdM * n;
1571 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
1572 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
1573 double dotr = a0 * _e*sin(E) * dEdM * n
1574 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
1575
1576 double dotx = dotr*cos(u) - r*sin(u)*dotu;
1577 double doty = dotr*sin(u) + r*cos(u)*dotu;
1578
1579 const double iMaxGEO = 10.0 / 180.0 * M_PI;
1580
1581 // MEO/IGSO satellite
1582 // ------------------
1583 if (_i0 > iMaxGEO) {
1584 double OM = _OMEGA0 + (_OMEGADOT - omegaBDS)*tk - omegaBDS*toesec;
1585
1586 sinom = sin(OM);
1587 cosom = cos(OM);
1588 sini = sin(i);
1589 cosi = cos(i);
1590
1591 xc[0] = xp*cosom - yp*cosi*sinom;
1592 xc[1] = xp*sinom + yp*cosi*cosom;
1593 xc[2] = yp*sini;
1594
1595 // Velocity
1596 // --------
1597
1598 double dotom = _OMEGADOT - t_CST::omega;
1599
1600 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
1601 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
1602 + yp*sini*sinom*doti; // dX / di
1603
1604 vv[1] = sinom *dotx + cosi*cosom *doty
1605 + xp*cosom*dotom - yp*cosi*sinom*dotom
1606 - yp*sini*cosom*doti;
1607
1608 vv[2] = sini *doty + yp*cosi *doti;
1609
1610 }
1611
1612 // GEO satellite
1613 // -------------
1614 else {
1615 double OM = _OMEGA0 + _OMEGADOT*tk - omegaBDS*toesec;
1616 double ll = omegaBDS*tk;
1617
1618 sinom = sin(OM);
1619 cosom = cos(OM);
1620 sini = sin(i);
1621 cosi = cos(i);
1622
1623 double xx = xp*cosom - yp*cosi*sinom;
1624 double yy = xp*sinom + yp*cosi*cosom;
1625 double zz = yp*sini;
1626
1627 Matrix RX = BNC_PPP::t_astro::rotX(-5.0 / 180.0 * M_PI);
1628 Matrix RZ = BNC_PPP::t_astro::rotZ(ll);
1629
1630 ColumnVector X1(3); X1 << xx << yy << zz;
1631 ColumnVector X2 = RZ*RX*X1;
1632
1633 xc[0] = X2(1);
1634 xc[1] = X2(2);
1635 xc[2] = X2(3);
1636
1637 double dotom = _OMEGADOT;
1638
1639 double vx = cosom *dotx - cosi*sinom *doty
1640 - xp*sinom*dotom - yp*cosi*cosom*dotom
1641 + yp*sini*sinom*doti;
1642
1643 double vy = sinom *dotx + cosi*cosom *doty
1644 + xp*cosom*dotom - yp*cosi*sinom*dotom
1645 - yp*sini*cosom*doti;
1646
1647 double vz = sini *doty + yp*cosi *doti;
1648
1649 ColumnVector V(3); V << vx << vy << vz;
1650
1651 Matrix RdotZ(3,3);
1652 double C = cos(ll);
1653 double S = sin(ll);
1654 Matrix UU(3,3);
1655 UU[0][0] = -S; UU[0][1] = +C; UU[0][2] = 0.0;
1656 UU[1][0] = -C; UU[1][1] = -S; UU[1][2] = 0.0;
1657 UU[2][0] = 0.0; UU[2][1] = 0.0; UU[2][2] = 0.0;
1658 RdotZ = omegaBDS * UU;
1659
1660 ColumnVector VV(3);
1661 VV = RZ*RX*V + RdotZ*RX*X1;
1662
1663 vv[0] = VV(1);
1664 vv[1] = VV(2);
1665 vv[2] = VV(3);
1666 }
1667
1668 double tc = tt - _TOC;
1669 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
1670
1671 // dotC = _clock_drift + _clock_driftrate*tc
1672 // - 4.442807633e-10*_e*sqrt(a0)*cos(E) * dEdM * n;
1673
1674 // Relativistic Correction
1675 // -----------------------
1676 // correspondent to BDS ICD and to SSR standard
1677 xc[3] -= 4.442807633e-10 * _e * sqrt(a0) *sin(E);
1678 // correspondent to IGS convention
1679 // xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c;
1680
1681 xc[4] = _clock_drift + _clock_driftrate*tc;
1682 xc[5] = _clock_driftrate;
1683
1684 return success;
1685}
1686
1687// RINEX Format String
1688//////////////////////////////////////////////////////////////////////////////
1689QString t_ephBDS::toString(double version) const {
1690
1691 QString rnxStr = rinexDateStr(_TOC-14.0, _prn, version);
1692
1693 QTextStream out(&rnxStr);
1694
1695 out << QString("%1%2%3\n")
1696 .arg(_clock_bias, 19, 'e', 12)
1697 .arg(_clock_drift, 19, 'e', 12)
1698 .arg(_clock_driftrate, 19, 'e', 12);
1699
1700 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1701
1702 out << QString(fmt)
1703 .arg(double(_AODE), 19, 'e', 12)
1704 .arg(_Crs, 19, 'e', 12)
1705 .arg(_Delta_n, 19, 'e', 12)
1706 .arg(_M0, 19, 'e', 12);
1707
1708 out << QString(fmt)
1709 .arg(_Cuc, 19, 'e', 12)
1710 .arg(_e, 19, 'e', 12)
1711 .arg(_Cus, 19, 'e', 12)
1712 .arg(_sqrt_A, 19, 'e', 12);
1713
1714 double toes = 0.0;
1715 if (_TOEweek > -1.0) {// RINEX input
1716 toes = _TOEsec;
1717 }
1718 else {// RTCM stream input
1719 toes = _TOE.bdssec();
1720 }
1721 out << QString(fmt)
1722 .arg(toes, 19, 'e', 12)
1723 .arg(_Cic, 19, 'e', 12)
1724 .arg(_OMEGA0, 19, 'e', 12)
1725 .arg(_Cis, 19, 'e', 12);
1726
1727 out << QString(fmt)
1728 .arg(_i0, 19, 'e', 12)
1729 .arg(_Crc, 19, 'e', 12)
1730 .arg(_omega, 19, 'e', 12)
1731 .arg(_OMEGADOT, 19, 'e', 12);
1732
1733 double toew = 0.0;
1734 if (_TOEweek > -1.0) {// RINEX input
1735 toew = _TOEweek;
1736 }
1737 else {// RTCM stream input
1738 toew = double(_TOE.bdsw());
1739 }
1740 out << QString(fmt)
1741 .arg(_IDOT, 19, 'e', 12)
1742 .arg(0.0, 19, 'e', 12)
1743 .arg(toew, 19, 'e', 12)
1744 .arg(0.0, 19, 'e', 12);
1745
1746 out << QString(fmt)
1747 .arg(_URA, 19, 'e', 12)
1748 .arg(double(_SatH1), 19, 'e', 12)
1749 .arg(_TGD1, 19, 'e', 12)
1750 .arg(_TGD2, 19, 'e', 12);
1751
1752 double tots = 0.0;
1753 if (_TOEweek > -1.0) {// RINEX input
1754 tots = _TOT;
1755 }
1756 else {// RTCM stream input
1757 tots = _TOE.bdssec();
1758 }
1759 out << QString(fmt)
1760 .arg(tots, 19, 'e', 12)
1761 .arg(double(_AODC), 19, 'e', 12)
1762 .arg("", 19, QChar(' '))
1763 .arg("", 19, QChar(' '));
1764 return rnxStr;
1765}
Note: See TracBrowser for help on using the repository browser.