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

Last change on this file since 10575 was 10575, checked in by stuerze, 13 months ago

minor changes
File size: 71.2 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#include "RTCM3/bits.h"
17
18using namespace std;
19
20// Constructor
21////////////////////////////////////////////////////////////////////////////
22t_eph::t_eph() {
23 _checkState = unchecked;
24 _navType = undefined;
25 _orbCorr = 0;
26 _clkCorr = 0;
27}
28// Destructor
29////////////////////////////////////////////////////////////////////////////
30t_eph::~t_eph() {
31 if (_orbCorr)
32 delete _orbCorr;
33 if (_clkCorr)
34 delete _clkCorr;
35}
36
37//
38////////////////////////////////////////////////////////////////////////////
39void t_eph::setOrbCorr(const t_orbCorr* orbCorr) {
40 if (_orbCorr) {
41 delete _orbCorr;
42 _orbCorr = 0;
43 }
44 _orbCorr = new t_orbCorr(*orbCorr);
45}
46
47//
48////////////////////////////////////////////////////////////////////////////
49void t_eph::setClkCorr(const t_clkCorr* clkCorr) {
50 if (_clkCorr) {
51 delete _clkCorr;
52 _clkCorr = 0;
53 }
54 _clkCorr = new t_clkCorr(*clkCorr);
55}
56
57//
58////////////////////////////////////////////////////////////////////////////
59t_irc t_eph::getCrd(const bncTime& tt, ColumnVector& xc, ColumnVector& vv, bool useCorr) const {
60
61 if (_checkState == bad ||
62 _checkState == unhealthy ||
63 _checkState == outdated) {
64 return failure;
65 }
66
67 xc.ReSize(6);
68 vv.ReSize(3);
69 if (position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data()) != success) {
70 return failure;
71 }
72 if (useCorr) {
73 if (_orbCorr && _clkCorr) {
74 double dtO = tt - _orbCorr->_time;
75 if (_orbCorr->_updateInt) {
76 dtO -= (0.5 * ssrUpdateInt[_orbCorr->_updateInt]);
77 }
78 ColumnVector dx(3);
79 dx[0] = _orbCorr->_xr[0] + _orbCorr->_dotXr[0] * dtO;
80 dx[1] = _orbCorr->_xr[1] + _orbCorr->_dotXr[1] * dtO;
81 dx[2] = _orbCorr->_xr[2] + _orbCorr->_dotXr[2] * dtO;
82
83 RSW_to_XYZ(xc.Rows(1,3), vv.Rows(1,3), dx, dx);
84
85 xc[0] -= dx[0];
86 xc[1] -= dx[1];
87 xc[2] -= dx[2];
88
89 ColumnVector dv(3);
90 RSW_to_XYZ(xc.Rows(1,3), vv.Rows(1,3), _orbCorr->_dotXr, dv);
91
92 vv[0] -= dv[0];
93 vv[1] -= dv[1];
94 vv[2] -= dv[2];
95
96 double dtC = tt - _clkCorr->_time;
97 if (_clkCorr->_updateInt) {
98 dtC -= (0.5 * ssrUpdateInt[_clkCorr->_updateInt]);
99 }
100 xc[3] += _clkCorr->_dClk + _clkCorr->_dotDClk * dtC + _clkCorr->_dotDotDClk * dtC * dtC;
101 }
102 else {
103 return failure;
104 }
105 }
106 return success;
107}
108
109
110//
111//////////////////////////////////////////////////////////////////////////////
112t_irc t_eph::setNavType(QString navTypeStr) {
113
114 if (navTypeStr == "LNAV") {
115 _navType = t_eph::LNAV;
116 }
117 else if (navTypeStr == "FDMA") {
118 _navType = t_eph::FDMA;
119 }
120 else if (navTypeStr == "FNAV") {
121 _navType = t_eph::FNAV;
122 }
123 else if (navTypeStr == "INAV") {
124 _navType = t_eph::INAF;
125 }
126 else if (navTypeStr == "D1") {
127 _navType = t_eph::D1;
128 }
129 else if (navTypeStr == "D2") {
130 _navType = t_eph::D2;
131 }
132 else if (navTypeStr == "SBAS") {
133 _navType = t_eph::SBASL1;
134 }
135 else if (navTypeStr == "CNAV") {
136 _navType = t_eph::CNAV;
137 }
138 else if (navTypeStr == "CNV1") {
139 _navType = t_eph::CNV1;
140 }
141 else if (navTypeStr == "CNV2") {
142 _navType = t_eph::CNV2;
143 }
144 else if (navTypeStr == "CNV3") {
145 _navType = t_eph::CNV3;
146 }
147 else {
148 _navType = t_eph::undefined;
149 return failure;
150 }
151
152 return success;
153}
154
155//
156//////////////////////////////////////////////////////////////////////////////
157QString t_eph::navTypeString(e_navType navType, const t_prn& prn, double version) {
158 QString navTypeString = "";
159 QString epochStart;
160 QString eolStr;
161
162 if (version < 4.0) {
163 return navTypeString;
164 }
165
166 if (version == 99.0) {
167 epochStart = "";
168 eolStr = "";
169 }
170 else {
171 epochStart = "> ";
172 eolStr = "\n";
173 }
174
175 QString ephStr = QString("EPH %1 ").arg(prn.toString().c_str());
176 switch (navType) {
177 case undefined:
178 navTypeString = epochStart + ephStr + "unknown" + eolStr;
179 break;
180 case LNAV:
181 navTypeString = epochStart + ephStr + "LNAV" + eolStr;
182 break;
183 case FDMA:
184 navTypeString = epochStart + ephStr + "FDMA" + eolStr;
185 break;
186 case FNAV:
187 navTypeString = epochStart + ephStr + "FNAV" + eolStr;
188 break;
189 case INAF:
190 navTypeString = epochStart + ephStr + "INAV" + eolStr;
191 break;
192 case D1:
193 navTypeString = epochStart + ephStr + "D1 " + eolStr;
194 break;
195 case D2:
196 navTypeString = epochStart + ephStr + "D2 " + eolStr;
197 break;
198 case SBASL1:
199 navTypeString = epochStart + ephStr + "SBAS" + eolStr;
200 break;
201 case CNAV:
202 navTypeString = epochStart + ephStr + "CNAV" + eolStr;
203 break;
204 case CNV1:
205 navTypeString = epochStart + ephStr + "CNV1" + eolStr;
206 break;
207 case CNV2:
208 navTypeString = epochStart + ephStr + "CNV2" + eolStr;
209 break;
210 case CNV3:
211 navTypeString = epochStart + ephStr + "CNV3" + eolStr;
212 break;
213 }
214 return navTypeString;
215 }
216
217//
218//////////////////////////////////////////////////////////////////////////////
219QString t_eph::rinexDateStr(const bncTime& tt, const t_prn& prn, double version) {
220 QString prnStr(prn.toString().c_str());
221 return rinexDateStr(tt, prnStr, version);
222}
223
224//
225//////////////////////////////////////////////////////////////////////////////
226QString t_eph::rinexDateStr(const bncTime& tt, const QString& prnStr, double version) {
227
228 QString datStr;
229
230 unsigned year, month, day, hour, min;
231 double sec;
232 tt.civil_date(year, month, day);
233 tt.civil_time(hour, min, sec);
234
235 QTextStream out(&datStr);
236
237 if (version < 3.0) {
238 QString prnHlp = prnStr.mid(1,2); if (prnHlp[0] == '0') prnHlp[0] = ' ';
239 out << prnHlp << QString(" %1 %2 %3 %4 %5%6")
240 .arg(year % 100, 2, 10, QChar('0'))
241 .arg(month, 2)
242 .arg(day, 2)
243 .arg(hour, 2)
244 .arg(min, 2)
245 .arg(sec, 5, 'f',1);
246 }
247 else if (version == 99) {
248 out << QString(" %1 %2 %3 %4 %5 %6")
249 .arg(year, 4)
250 .arg(month, 2, 10, QChar('0'))
251 .arg(day, 2, 10, QChar('0'))
252 .arg(hour, 2, 10, QChar('0'))
253 .arg(min, 2, 10, QChar('0'))
254 .arg(int(sec), 2, 10, QChar('0'));
255 }
256 else {
257 out << prnStr << QString(" %1 %2 %3 %4 %5 %6")
258 .arg(year, 4)
259 .arg(month, 2, 10, QChar('0'))
260 .arg(day, 2, 10, QChar('0'))
261 .arg(hour, 2, 10, QChar('0'))
262 .arg(min, 2, 10, QChar('0'))
263 .arg(int(sec), 2, 10, QChar('0'));
264 }
265
266 return datStr;
267}
268
269// Constructor
270//////////////////////////////////////////////////////////////////////////////
271t_ephGPS::t_ephGPS(double rnxVersion, const QStringList& lines) {
272
273 int nLines = 8;
274
275 if (navType() == t_eph::CNAV) {
276 nLines += 1;
277 }
278 if (navType() == t_eph::CNV2) {
279 nLines += 2;
280 }
281
282 if (lines.size() != nLines) {
283 _checkState = bad;
284 return;
285 }
286
287 // RINEX Format
288 // ------------
289 int fieldLen = 19;
290
291 int pos[4];
292 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
293 pos[1] = pos[0] + fieldLen;
294 pos[2] = pos[1] + fieldLen;
295 pos[3] = pos[2] + fieldLen;
296
297 // Read eight lines
298 // ----------------
299 for (int iLine = 0; iLine < nLines; iLine++) {
300 QString line = lines[iLine];
301
302 if ( iLine == 0 ) {
303 QTextStream in(line.left(pos[1]).toLatin1());
304 int year, month, day, hour, min;
305 double sec;
306
307 QString prnStr, n;
308 in >> prnStr;
309
310 if (prnStr.size() == 1 &&
311 (prnStr[0] == 'G' ||
312 prnStr[0] == 'J' ||
313 prnStr[0] == 'I')) {
314 in >> n;
315 prnStr.append(n);
316 }
317
318 in >> year >> month >> day >> hour >> min >> sec;
319 if (prnStr.at(0) == 'G') {
320 _prn.set('G', prnStr.mid(1).toInt());
321 }
322 else if (prnStr.at(0) == 'J') {
323 _prn.set('J', prnStr.mid(1).toInt());
324 }
325 else if (prnStr.at(0) == 'I') {
326 _prn.set('I', prnStr.mid(1).toInt());
327 }
328 else {
329 _prn.set('G', prnStr.toInt());
330 }
331
332 if (year < 80) {
333 year += 2000;
334 }
335 else if (year < 100) {
336 year += 1900;
337 }
338
339 _TOC.set(year, month, day, hour, min, sec);
340
341 if ( readDbl(line, pos[1], fieldLen, _clock_bias ) ||
342 readDbl(line, pos[2], fieldLen, _clock_drift ) ||
343 readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
344 _checkState = bad;
345 return;
346 }
347 }
348 // =====================
349 // BROADCAST ORBIT - 1
350 // =====================
351 else if ( iLine == 1) {
352
353 if (navType() == t_eph::CNAV ||
354 navType() == t_eph::CNV2) {
355 if ( readDbl(line, pos[0], fieldLen, _ADOT ) ||
356 readDbl(line, pos[1], fieldLen, _Crs ) ||
357 readDbl(line, pos[2], fieldLen, _Delta_n) ||
358 readDbl(line, pos[3], fieldLen, _M0 ) ) {
359 _checkState = bad;
360 return;
361 }
362 }
363 else { // LNAV, undefined
364 if ( readDbl(line, pos[0], fieldLen, _IODE ) ||
365 readDbl(line, pos[1], fieldLen, _Crs ) ||
366 readDbl(line, pos[2], fieldLen, _Delta_n) ||
367 readDbl(line, pos[3], fieldLen, _M0 ) ) {
368 _checkState = bad;
369 return;
370 }
371 }
372 }
373 // =====================
374 // BROADCAST ORBIT - 2
375 // =====================
376 else if ( iLine == 2 ) {
377 if ( readDbl(line, pos[0], fieldLen, _Cuc ) ||
378 readDbl(line, pos[1], fieldLen, _e ) ||
379 readDbl(line, pos[2], fieldLen, _Cus ) ||
380 readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
381 _checkState = bad;
382 return;
383 }
384 }
385 // =====================
386 // BROADCAST ORBIT - 3
387 // =====================
388 else if ( iLine == 3 ) {
389
390 if (navType() == t_eph::CNAV ||
391 navType() == t_eph::CNV2) {
392 if ( readDbl(line, pos[0], fieldLen, _top) ||
393 readDbl(line, pos[1], fieldLen, _Cic ) ||
394 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
395 readDbl(line, pos[3], fieldLen, _Cis ) ) {
396 _checkState = bad;
397 return;
398 }
399 }
400 else { // LNAV, undefined
401 if ( readDbl(line, pos[0], fieldLen, _TOEsec) ||
402 readDbl(line, pos[1], fieldLen, _Cic ) ||
403 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
404 readDbl(line, pos[3], fieldLen, _Cis ) ) {
405 _checkState = bad;
406 return;
407 }
408 }
409 }
410 // =====================
411 // BROADCAST ORBIT - 4
412 // =====================
413 else if ( iLine == 4 ) {
414 if ( readDbl(line, pos[0], fieldLen, _i0 ) ||
415 readDbl(line, pos[1], fieldLen, _Crc ) ||
416 readDbl(line, pos[2], fieldLen, _omega ) ||
417 readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
418 _checkState = bad;
419 return;
420 }
421 }
422 // =====================
423 // BROADCAST ORBIT - 5
424 // =====================
425 else if ( iLine == 5 && type() != t_eph::IRNSS) {
426
427 if (navType() == t_eph::CNAV ||
428 navType() == t_eph::CNV2) {
429 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
430 readDbl(line, pos[1], fieldLen, _Delta_n_dot) ||
431 readDbl(line, pos[2], fieldLen, _URAI_NED0 ) ||
432 readDbl(line, pos[3], fieldLen, _URAI_NED1) ) {
433 _checkState = bad;
434 return;
435 }
436 }
437 else { // LNAV, undefined
438 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
439 readDbl(line, pos[1], fieldLen, _L2Codes) ||
440 readDbl(line, pos[2], fieldLen, _TOEweek) ||
441 readDbl(line, pos[3], fieldLen, _L2PFlag) ) {
442 _checkState = bad;
443 return;
444 }
445 }
446 }
447 else if ( iLine == 5 && type() == t_eph::IRNSS) {
448 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
449 readDbl(line, pos[2], fieldLen, _TOEweek) ) {
450 _checkState = bad;
451 return;
452 }
453 }
454 // =====================
455 // BROADCAST ORBIT - 6
456 // =====================
457 else if ( iLine == 6 && type() != t_eph::IRNSS) {
458
459 if (navType() == t_eph::CNAV ||
460 navType() == t_eph::CNV2 ) {
461 if ( readDbl(line, pos[0], fieldLen, _URAI_ED) ||
462 readDbl(line, pos[1], fieldLen, _health ) ||
463 readDbl(line, pos[2], fieldLen, _TGD ) ||
464 readDbl(line, pos[3], fieldLen, _URAI_NED2) ) {
465 _checkState = bad;
466 return;
467 }
468 }
469 else { // LNAV, undefined
470 if ( readDbl(line, pos[0], fieldLen, _ura ) ||
471 readDbl(line, pos[1], fieldLen, _health) ||
472 readDbl(line, pos[2], fieldLen, _TGD ) ||
473 readDbl(line, pos[3], fieldLen, _IODC ) ) {
474 _checkState = bad;
475 return;
476 }
477 }
478 }
479 else if ( iLine == 6 && type() == t_eph::IRNSS) {
480 if ( readDbl(line, pos[0], fieldLen, _ura ) ||
481 readDbl(line, pos[1], fieldLen, _health) ||
482 readDbl(line, pos[2], fieldLen, _TGD ) ) {
483 _checkState = bad;
484 return;
485 }
486 }
487 // =====================
488 // BROADCAST ORBIT - 7
489 // =====================
490 else if ( iLine == 7 ) {
491 if (navType() == t_eph::LNAV ||
492 navType() == t_eph::undefined) {
493
494 if ( readDbl(line, pos[0], fieldLen, _TOT) ) {
495 _checkState = bad;
496 return;
497 }
498
499 if (type() != t_eph::IRNSS) {
500 double fitIntervalRnx;
501 if ( readDbl(line, pos[1], fieldLen, fitIntervalRnx) ) {
502 _checkState = bad;
503 return;
504 }
505 if (type() == t_eph::GPS) { // in RINEX specified always as time period for GPS
506 _fitInterval = fitIntervalRnx;
507 } else if (type() == t_eph::QZSS) { // specified as flag for QZSS
508 if (rnxVersion == 3.02) {
509 _fitInterval = fitIntervalRnx; // specified as time period
510 }
511 else {
512 _fitInterval = fitIntervalFromFlag(fitIntervalRnx, _IODC, t_eph::QZSS);
513 }
514 }
515 }
516 }
517
518 else if (navType() == t_eph::CNAV ||
519 navType() == t_eph::CNV2) {
520 if ( readDbl(line, pos[0], fieldLen, _ISC_L1CA) ||
521 readDbl(line, pos[1], fieldLen, _ISC_L2C ) ||
522 readDbl(line, pos[2], fieldLen, _ISC_L5I5) ||
523 readDbl(line, pos[3], fieldLen, _ISC_L5Q5) ) {
524 _checkState = bad;
525 return;
526 }
527 }
528
529 }
530 // =====================
531 // BROADCAST ORBIT - 8
532 // =====================
533 else if ( iLine == 8 ) {
534 if (navType() == t_eph::CNAV) {
535 if ( readDbl(line, pos[0], fieldLen, _TOT) ||
536 readDbl(line, pos[1], fieldLen, _wnop) ) {
537 _checkState = bad;
538 return;
539 }
540 }
541 else if (navType() == t_eph::CNV2) {
542 if ( readDbl(line, pos[0], fieldLen, _ISC_L1Cd) ||
543 readDbl(line, pos[1], fieldLen, _ISC_L1Cp)) {
544 _checkState = bad;
545 return;
546 }
547 }
548 }
549 // =====================
550 // BROADCAST ORBIT - 9
551 // =====================
552 else if ( iLine == 9 ) {
553 if (navType() == t_eph::CNV2) {
554 if ( readDbl(line, pos[0], fieldLen, _TOT) ||
555 readDbl(line, pos[1], fieldLen, _wnop) ) {
556 _checkState = bad;
557 return;
558 }
559 }
560 }
561 }
562}
563
564// Compute GPS Satellite Position (virtual)
565////////////////////////////////////////////////////////////////////////////
566t_irc t_ephGPS::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
567
568 static const double omegaEarth = 7292115.1467e-11;
569 static const double gmGRS = 398.6005e12;
570
571 memset(xc, 0, 6*sizeof(double));
572 memset(vv, 0, 3*sizeof(double));
573
574 double a0 = _sqrt_A * _sqrt_A;
575 if (a0 == 0) {
576 return failure;
577 }
578
579 double n0 = sqrt(gmGRS/(a0*a0*a0));
580
581 bncTime tt(GPSweek, GPSweeks);
582 double tk = tt - bncTime(int(_TOEweek), _TOEsec);
583
584 double n = n0 + _Delta_n;
585 double M = _M0 + n*tk;
586 double E = M;
587 double E_last;
588 int nLoop = 0;
589 do {
590 E_last = E;
591 E = M + _e*sin(E);
592
593 if (++nLoop == 100) {
594 return failure;
595 }
596 } while ( fabs(E-E_last)*a0 > 0.001);
597 double v = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) );
598 double u0 = v + _omega;
599 double sin2u0 = sin(2*u0);
600 double cos2u0 = cos(2*u0);
601 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
602 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
603 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
604 double xp = r*cos(u);
605 double yp = r*sin(u);
606 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
607 omegaEarth*_TOEsec;
608
609 double sinom = sin(OM);
610 double cosom = cos(OM);
611 double sini = sin(i);
612 double cosi = cos(i);
613 xc[0] = xp*cosom - yp*cosi*sinom;
614 xc[1] = xp*sinom + yp*cosi*cosom;
615 xc[2] = yp*sini;
616
617 double tc = tt - _TOC;
618 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
619
620 // Velocity
621 // --------
622 double tanv2 = tan(v/2);
623 double dEdM = 1 / (1 - _e*cos(E));
624 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
625 * dEdM * n;
626 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
627 double dotom = _OMEGADOT - omegaEarth;
628 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
629 double dotr = a0 * _e*sin(E) * dEdM * n
630 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
631 double dotx = dotr*cos(u) - r*sin(u)*dotu;
632 double doty = dotr*sin(u) + r*cos(u)*dotu;
633
634 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
635 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
636 + yp*sini*sinom*doti; // dX / di
637
638 vv[1] = sinom *dotx + cosi*cosom *doty
639 + xp*cosom*dotom - yp*cosi*sinom*dotom
640 - yp*sini*cosom*doti;
641
642 vv[2] = sini *doty + yp*cosi *doti;
643
644 // Relativistic Correction
645 // -----------------------
646 xc[3] -= 4.442807633e-10 * _e * sqrt(a0) *sin(E);
647
648 xc[4] = _clock_drift + _clock_driftrate*tc;
649 xc[5] = _clock_driftrate;
650
651 return success;
652}
653
654// RINEX Format String
655//////////////////////////////////////////////////////////////////////////////
656QString t_ephGPS::toString(double version) const {
657
658 QString navStr = navTypeString(_navType, _prn, version);
659 QString rnxStr = navStr + rinexDateStr(_TOC, _prn, version);
660
661 QTextStream out(&rnxStr);
662
663 out << QString("%1%2%3\n")
664 .arg(_clock_bias, 19, 'e', 12)
665 .arg(_clock_drift, 19, 'e', 12)
666 .arg(_clock_driftrate, 19, 'e', 12);
667
668 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
669
670
671 // =====================
672 // BROADCAST ORBIT - 1
673 // =====================
674 if (navType() == t_eph::CNAV ||
675 navType() == t_eph::CNV2) {
676 out << QString(fmt)
677 .arg(_ADOT, 19, 'e', 12)
678 .arg(_Crs, 19, 'e', 12)
679 .arg(_Delta_n, 19, 'e', 12)
680 .arg(_M0, 19, 'e', 12);
681 }
682 else { // LNAV, undefinded
683 out << QString(fmt)
684 .arg(_IODE, 19, 'e', 12)
685 .arg(_Crs, 19, 'e', 12)
686 .arg(_Delta_n, 19, 'e', 12)
687 .arg(_M0, 19, 'e', 12);
688 }
689 // =====================
690 // BROADCAST ORBIT - 2
691 // =====================
692 out << QString(fmt)
693 .arg(_Cuc, 19, 'e', 12)
694 .arg(_e, 19, 'e', 12)
695 .arg(_Cus, 19, 'e', 12)
696 .arg(_sqrt_A, 19, 'e', 12);
697 // =====================
698 // BROADCAST ORBIT - 3
699 // =====================
700 if (navType() == t_eph::CNAV ||
701 navType() == t_eph::CNV2) {
702 out << QString(fmt)
703 .arg(_top, 19, 'e', 12)
704 .arg(_Cic, 19, 'e', 12)
705 .arg(_OMEGA0, 19, 'e', 12)
706 .arg(_Cis, 19, 'e', 12);
707 }
708 else {
709 out << QString(fmt)
710 .arg(_TOEsec, 19, 'e', 12)
711 .arg(_Cic, 19, 'e', 12)
712 .arg(_OMEGA0, 19, 'e', 12)
713 .arg(_Cis, 19, 'e', 12);
714 }
715 // =====================
716 // BROADCAST ORBIT - 4
717 // =====================
718 out << QString(fmt)
719 .arg(_i0, 19, 'e', 12)
720 .arg(_Crc, 19, 'e', 12)
721 .arg(_omega, 19, 'e', 12)
722 .arg(_OMEGADOT, 19, 'e', 12);
723 // =====================
724 // BROADCAST ORBIT - 5
725 // =====================
726 if (type() == t_eph::IRNSS) {
727 out << QString(fmt)
728 .arg(_IDOT, 19, 'e', 12)
729 .arg(0.0, 19, 'e', 12)
730 .arg(_TOEweek, 19, 'e', 12)
731 .arg(0.0, 19, 'e', 12);
732 }
733 else {
734 if (navType() == t_eph::CNAV ||
735 navType() == t_eph::CNV2) {
736 out << QString(fmt)
737 .arg(_IDOT, 19, 'e', 12)
738 .arg(_Delta_n_dot, 19, 'e', 12)
739 .arg(_URAI_NED0, 19, 'e', 12)
740 .arg(_URAI_NED1, 19, 'e', 12);
741
742 }
743 else { // LNAV, undefined
744 out << QString(fmt)
745 .arg(_IDOT, 19, 'e', 12)
746 .arg(_L2Codes, 19, 'e', 12)
747 .arg(_TOEweek, 19, 'e', 12)
748 .arg(_L2PFlag, 19, 'e', 12);
749
750 }
751 }
752 // =====================
753 // BROADCAST ORBIT - 6
754 // =====================
755 if (type() == t_eph::IRNSS) {
756 out << QString(fmt)
757 .arg(_ura, 19, 'e', 12)
758 .arg(_health, 19, 'e', 12)
759 .arg(_TGD, 19, 'e', 12)
760 .arg(0.0, 19, 'e', 12);
761 }
762 else {
763 if (navType() == t_eph::CNAV ||
764 navType() == t_eph::CNV2) {
765 out << QString(fmt)
766 .arg(_URAI_ED, 19, 'e', 12)
767 .arg(_health, 19, 'e', 12)
768 .arg(_TGD, 19, 'e', 12)
769 .arg(_URAI_NED2,19, 'e', 12);
770
771 }
772 else { // LNAV, undefined
773 out << QString(fmt)
774 .arg(_ura, 19, 'e', 12)
775 .arg(_health, 19, 'e', 12)
776 .arg(_TGD, 19, 'e', 12)
777 .arg(_IODC, 19, 'e', 12);
778 }
779 }
780 // =====================
781 // BROADCAST ORBIT - 7
782 // =====================
783 if (navType() == t_eph::LNAV ||
784 navType() == t_eph::undefined) {
785
786 double tot = _TOT;
787 if (tot == 0.9999e9 && version < 3.0) {
788 tot = 0.0;
789 }
790 // fitInterval
791 if (type() == t_eph::IRNSS) {// not valid for IRNSS
792 out << QString(fmt)
793 .arg(tot, 19, 'e', 12)
794 .arg(0.0, 19, 'e', 12)
795 .arg("", 19, QChar(' '))
796 .arg("", 19, QChar(' '));
797 }
798 else {
799 // for GPS and QZSS in version 3.02 specified in hours
800 double fitIntervalRnx = _fitInterval;
801 // otherwise specified as flag
802 if (type() == t_eph::QZSS && version != 3.02) {
803 (_fitInterval == 2.0) ? fitIntervalRnx = 0.0 : fitIntervalRnx = 1.0;
804 }
805 out << QString(fmt)
806 .arg(tot, 19, 'e', 12)
807 .arg(fitIntervalRnx, 19, 'e', 12)
808 .arg("", 19, QChar(' '))
809 .arg("", 19, QChar(' '));
810 }
811 }
812 else if (navType() == t_eph::CNAV ||
813 navType() == t_eph::CNV2) {
814 out << QString(fmt)
815 .arg(_ISC_L1CA, 19, 'e', 12)
816 .arg(_ISC_L2C, 19, 'e', 12)
817 .arg(_ISC_L5I5, 19, 'e', 12)
818 .arg(_ISC_L5Q5, 19, 'e', 12);
819 }
820 // =====================
821 // BROADCAST ORBIT - 8
822 // =====================
823 if (navType() == t_eph::CNAV) {
824 out << QString(fmt)
825 .arg(_TOT, 19, 'e', 12)
826 .arg(_wnop, 19, 'e', 12)
827 .arg("", 19, QChar(' '))
828 .arg("", 19, QChar(' '));
829 }
830 else if (navType() == t_eph::CNV2) {
831 out << QString(fmt)
832 .arg(_ISC_L1Cd, 19, 'e', 12)
833 .arg(_ISC_L1Cp, 19, 'e', 12)
834 .arg("", 19, QChar(' '))
835 .arg("", 19, QChar(' '));
836 }
837
838 // =====================
839 // BROADCAST ORBIT - 9
840 // =====================
841 if (navType() == t_eph::CNV2) {
842 out << QString(fmt)
843 .arg(_TOT, 19, 'e', 12)
844 .arg(_wnop, 19, 'e', 12)
845 .arg("", 19, QChar(' '))
846 .arg("", 19, QChar(' '));
847 }
848
849 return rnxStr;
850}
851
852// Constructor
853//////////////////////////////////////////////////////////////////////////////
854t_ephGlo::t_ephGlo(double rnxVersion, const QStringList& lines) {
855
856 int nLines = 4;
857 if (rnxVersion >= 3.05) {
858 nLines += 1;
859 }
860 else {
861 _M_delta_tau = 0.9999e9; // unknown
862 _M_FT = 1.5e1; // unknown
863 }
864
865 if (lines.size() != nLines) {
866 _checkState = bad;
867 return;
868 }
869
870 // RINEX Format
871 // ------------
872 int fieldLen = 19;
873 double statusflags = 0.0;
874 double healthflags = 0.0;
875
876 int pos[4];
877 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
878 pos[1] = pos[0] + fieldLen;
879 pos[2] = pos[1] + fieldLen;
880 pos[3] = pos[2] + fieldLen;
881
882 // Read four lines
883 // ---------------
884 for (int iLine = 0; iLine < nLines; iLine++) {
885 QString line = lines[iLine];
886
887 if ( iLine == 0 ) {
888 QTextStream in(line.left(pos[1]).toLatin1());
889
890 int year, month, day, hour, min;
891 double sec;
892
893 QString prnStr, n;
894 in >> prnStr;
895 if (prnStr.size() == 1 && prnStr[0] == 'R') {
896 in >> n;
897 prnStr.append(n);
898 }
899 in >> year >> month >> day >> hour >> min >> sec;
900 if (prnStr.at(0) == 'R') {
901 _prn.set('R', prnStr.mid(1).toInt());
902 }
903 else {
904 _prn.set('R', prnStr.toInt());
905 }
906
907 if (year < 80) {
908 year += 2000;
909 }
910 else if (year < 100) {
911 year += 1900;
912 }
913
914 _gps_utc = gnumleap(year, month, day);
915
916 _TOC.set(year, month, day, hour, min, sec);
917 _TOC = _TOC + _gps_utc;
918 int nd = int((_TOC.gpssec())) / (24.0*60.0*60.0);
919 if ( readDbl(line, pos[1], fieldLen, _tau ) ||
920 readDbl(line, pos[2], fieldLen, _gamma) ||
921 readDbl(line, pos[3], fieldLen, _tki ) ) {
922 _checkState = bad;
923 return;
924 }
925 _tki -= nd * 86400.0;
926 _tau = -_tau;
927 }
928 // =====================
929 // BROADCAST ORBIT - 1
930 // =====================
931 else if ( iLine == 1 ) {
932 if ( readDbl(line, pos[0], fieldLen, _x_pos ) ||
933 readDbl(line, pos[1], fieldLen, _x_velocity ) ||
934 readDbl(line, pos[2], fieldLen, _x_acceleration) ||
935 readDbl(line, pos[3], fieldLen, _health ) ) {
936 _checkState = bad;
937 return;
938 }
939 }
940 // =====================
941 // BROADCAST ORBIT - 2
942 // =====================
943 else if ( iLine == 2 ) {
944 if ( readDbl(line, pos[0], fieldLen, _y_pos ) ||
945 readDbl(line, pos[1], fieldLen, _y_velocity ) ||
946 readDbl(line, pos[2], fieldLen, _y_acceleration ) ||
947 readDbl(line, pos[3], fieldLen, _frequency_number) ) {
948 _checkState = bad;
949 return;
950 }
951 }
952 // =====================
953 // BROADCAST ORBIT - 3
954 // =====================
955 else if ( iLine == 3 ) {
956 if ( readDbl(line, pos[0], fieldLen, _z_pos ) ||
957 readDbl(line, pos[1], fieldLen, _z_velocity ) ||
958 readDbl(line, pos[2], fieldLen, _z_acceleration) ||
959 readDbl(line, pos[3], fieldLen, _E ) ) {
960 _checkState = bad;
961 return;
962 }
963 }
964 // =====================
965 // BROADCAST ORBIT - 4
966 // =====================
967 else if ( iLine == 4 ) {
968 if (readDbl(line, pos[0], fieldLen, statusflags ) ) {
969 //statusflags BLK, do nothing
970 _flags_unknown = true;
971 }
972 else {
973 _flags_unknown = false;
974 // status flags
975 // ============
976 // bit 0-1
977 _M_P = double(bitExtracted(statusflags, 2, 0));
978 // bit 2-3
979 _P1 = double(bitExtracted(statusflags, 2, 2));
980 // bit 4
981 _P2 = double(bitExtracted(statusflags, 1, 4));
982 // bit 5
983 _P3 = double(bitExtracted(statusflags, 1, 5));
984 // bit 6
985 _M_P4 = double(bitExtracted(statusflags, 1, 6));
986 // bit 7-8
987 _M_M = double(bitExtracted(statusflags, 2, 7));
988 /// GLO M/K exclusive flags/values only valid if flag M is set to '01'
989 if (!_M_M) {
990 _M_P4 = 0.0;
991 _M_P = 0.0;
992 }
993 }
994 if ( readDbl(line, pos[1], fieldLen, _M_delta_tau ) ||
995 readDbl(line, pos[2], fieldLen, _M_FT ) ) {
996 _checkState = bad;
997 return;
998 }
999 if (readDbl(line, pos[3], fieldLen, healthflags ) ) {
1000 // healthflags BLK
1001 _flags_unknown = true;
1002 }
1003 else {
1004 _flags_unknown = false;
1005 // health flags
1006 // ============
1007 // bit 0 (is to be ignored, if bit 1 is zero)
1008 _almanac_health = double(bitExtracted(healthflags, 1, 0));
1009 // bit 1
1010 _almanac_health_availablility_indicator = double(bitExtracted(healthflags, 1, 1));
1011 // bit 2
1012 _M_l3 = double(bitExtracted(healthflags, 1, 2));
1013 }
1014 }
1015 }
1016
1017 // Initialize status vector
1018 // ------------------------
1019 _tt = _TOC;
1020 _xv.ReSize(6); _xv = 0.0;
1021 _xv(1) = _x_pos * 1.e3;
1022 _xv(2) = _y_pos * 1.e3;
1023 _xv(3) = _z_pos * 1.e3;
1024 _xv(4) = _x_velocity * 1.e3;
1025 _xv(5) = _y_velocity * 1.e3;
1026 _xv(6) = _z_velocity * 1.e3;
1027}
1028
1029// Compute Glonass Satellite Position (virtual)
1030////////////////////////////////////////////////////////////////////////////
1031t_irc t_ephGlo::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
1032
1033 static const double nominalStep = 10.0;
1034
1035 memset(xc, 0, 6*sizeof(double));
1036 memset(vv, 0, 3*sizeof(double));
1037
1038 double dtPos = bncTime(GPSweek, GPSweeks) - _tt;
1039
1040 if (fabs(dtPos) > 24 * 3600.0) {
1041 return failure;
1042 }
1043
1044 int nSteps = int(fabs(dtPos) / nominalStep) + 1;
1045 double step = dtPos / nSteps;
1046
1047 double acc[3];
1048 acc[0] = _x_acceleration * 1.e3;
1049 acc[1] = _y_acceleration * 1.e3;
1050 acc[2] = _z_acceleration * 1.e3;
1051
1052 for (int ii = 1; ii <= nSteps; ii++) {
1053 _xv = rungeKutta4(_tt.gpssec(), _xv, step, acc, glo_deriv);
1054 _tt = _tt + step;
1055 }
1056
1057 // Position and Velocity
1058 // ---------------------
1059 xc[0] = _xv(1);
1060 xc[1] = _xv(2);
1061 xc[2] = _xv(3);
1062
1063 vv[0] = _xv(4);
1064 vv[1] = _xv(5);
1065 vv[2] = _xv(6);
1066
1067 // Clock Correction
1068 // ----------------
1069 double dtClk = bncTime(GPSweek, GPSweeks) - _TOC;
1070 xc[3] = -_tau + _gamma * dtClk;
1071
1072 xc[4] = _gamma;
1073 xc[5] = 0.0;
1074
1075 return success;
1076}
1077
1078// RINEX Format String
1079//////////////////////////////////////////////////////////////////////////////
1080QString t_ephGlo::toString(double version) const {
1081
1082 QString navStr = navTypeString(_navType, _prn, version);
1083 QString rnxStr = navStr + rinexDateStr(_TOC -_gps_utc, _prn, version);
1084 int nd = int((_TOC - _gps_utc).gpssec()) / (24.0*60.0*60.0);
1085 QTextStream out(&rnxStr);
1086
1087 out << QString("%1%2%3\n")
1088 .arg(-_tau, 19, 'e', 12)
1089 .arg(_gamma, 19, 'e', 12)
1090 .arg(_tki+nd*86400.0, 19, 'e', 12);
1091
1092 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1093 // =====================
1094 // BROADCAST ORBIT - 1
1095 // =====================
1096 out << QString(fmt)
1097 .arg(_x_pos, 19, 'e', 12)
1098 .arg(_x_velocity, 19, 'e', 12)
1099 .arg(_x_acceleration, 19, 'e', 12)
1100 .arg(_health, 19, 'e', 12);
1101 // =====================
1102 // BROADCAST ORBIT - 2
1103 // =====================
1104 out << QString(fmt)
1105 .arg(_y_pos, 19, 'e', 12)
1106 .arg(_y_velocity, 19, 'e', 12)
1107 .arg(_y_acceleration, 19, 'e', 12)
1108 .arg(_frequency_number, 19, 'e', 12);
1109 // =====================
1110 // BROADCAST ORBIT - 3
1111 // =====================
1112 out << QString(fmt)
1113 .arg(_z_pos, 19, 'e', 12)
1114 .arg(_z_velocity, 19, 'e', 12)
1115 .arg(_z_acceleration, 19, 'e', 12)
1116 .arg(_E, 19, 'e', 12);
1117 // =====================
1118 // BROADCAST ORBIT - 4
1119 // =====================
1120 if (version >= 3.05) {
1121 // unknown (RINEX version < 3.05)
1122 if (_flags_unknown) {
1123 out << QString(fmt)
1124 .arg("", 19, QChar(' ')) // statusflags blank if unknown
1125 .arg(_M_delta_tau, 19, 'e', 12)
1126 .arg(_M_FT, 19, 'e', 12)
1127 .arg("", 19, QChar(' ')); // healthflags blank if unknown
1128 }
1129 else {
1130 int statusflags = 0;
1131 // bit 7-8
1132 if (_M_M == 2.0) {
1133 statusflags |= (1<<7);
1134 }
1135 // bit 6
1136 if (_M_P4) {
1137 statusflags |= (1<<6);
1138 }
1139 // bit 5
1140 if (_P3) {
1141 statusflags |= (1<<5);
1142 }
1143 // bit 4
1144 if (_P2) {
1145 statusflags |= (1<<4);
1146 }
1147 // bit 2-3
1148 if (_P1 == 2.0) {
1149 statusflags |= (1<<2);
1150 }
1151 else if (_P1 == 1.0) {
1152 statusflags |= (1<<3);
1153 }
1154 else if (_P1 == 3.0) {
1155 statusflags |= (1<<2);
1156 statusflags |= (1<<3);
1157 }
1158 // bit 0-1
1159 if (_M_P == 2.0) {
1160 statusflags |= (1<<0);
1161 }
1162 else if (_M_P == 1.0) {
1163 statusflags |= (1<<1);
1164 }
1165 else if (_M_P == 3.0) {
1166 statusflags |= (1<<0);
1167 statusflags |= (1<<1);
1168 }
1169 // health flags
1170 // ============
1171 int healthflags = 0;
1172 // bit 0 (is to be ignored, if bit 1 is zero)
1173 if (_almanac_health) {
1174 healthflags |= (1<<0);
1175 }
1176 // bit 1
1177 if (_almanac_health_availablility_indicator) {
1178 healthflags |= (1<<1);
1179 }
1180 // bit 2
1181 if (_M_l3) {
1182 healthflags |= (1<<2);
1183 }
1184 out << QString(fmt)
1185 .arg(double(statusflags), 19, 'e', 12)
1186 .arg(_M_delta_tau, 19, 'e', 12)
1187 .arg(_M_FT, 19, 'e', 12)
1188 .arg(double(healthflags), 19, 'e', 12);
1189 }
1190 }
1191
1192 return rnxStr;
1193}
1194
1195// Derivative of the state vector using a simple force model (static)
1196////////////////////////////////////////////////////////////////////////////
1197ColumnVector t_ephGlo::glo_deriv(double /* tt */, const ColumnVector& xv,
1198 double* acc) {
1199
1200 // State vector components
1201 // -----------------------
1202 ColumnVector rr = xv.rows(1,3);
1203 ColumnVector vv = xv.rows(4,6);
1204
1205 // Acceleration
1206 // ------------
1207 static const double gmWGS = 398.60044e12;
1208 static const double AE = 6378136.0;
1209 static const double OMEGA = 7292115.e-11;
1210 static const double C20 = -1082.6257e-6;
1211
1212 double rho = rr.NormFrobenius();
1213 double t1 = -gmWGS/(rho*rho*rho);
1214 double t2 = 3.0/2.0 * C20 * (gmWGS*AE*AE) / (rho*rho*rho*rho*rho);
1215 double t3 = OMEGA * OMEGA;
1216 double t4 = 2.0 * OMEGA;
1217 double z2 = rr(3) * rr(3);
1218
1219 // Vector of derivatives
1220 // ---------------------
1221 ColumnVector va(6);
1222 va(1) = vv(1);
1223 va(2) = vv(2);
1224 va(3) = vv(3);
1225 va(4) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2) + acc[0];
1226 va(5) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1) + acc[1];
1227 va(6) = (t1 + t2*(3.0-5.0*z2/(rho*rho)) ) * rr(3) + acc[2];
1228
1229 return va;
1230}
1231
1232// IOD of Glonass Ephemeris (virtual)
1233////////////////////////////////////////////////////////////////////////////
1234unsigned int t_ephGlo::IOD() const {
1235 bncTime tMoscow = _TOC - _gps_utc + 3 * 3600.0;
1236 return (unsigned long)tMoscow.daysec() / 900;
1237}
1238
1239// Health status of Glonass Ephemeris (virtual)
1240////////////////////////////////////////////////////////////////////////////
1241unsigned int t_ephGlo::isUnhealthy() const {
1242
1243 if (_almanac_health_availablility_indicator) {
1244 if ((_health == 0 && _almanac_health == 0) ||
1245 (_health == 1 && _almanac_health == 0) ||
1246 (_health == 1 && _almanac_health == 1)) {
1247 return 1;
1248 }
1249 }
1250 else if (!_almanac_health_availablility_indicator) {
1251 if (_health) {
1252 return 1;
1253 }
1254 }
1255 return 0; /* (_health == 0 && _almanac_health == 1) or (_health == 0) */
1256}
1257
1258// Constructor
1259//////////////////////////////////////////////////////////////////////////////
1260t_ephGal::t_ephGal(double rnxVersion, const QStringList& lines) {
1261 int year, month, day, hour, min;
1262 double sec;
1263 QString prnStr;
1264 const int nLines = 8;
1265 if (lines.size() != nLines) {
1266 _checkState = bad;
1267 return;
1268 }
1269
1270 // RINEX Format
1271 // ------------
1272 int fieldLen = 19;
1273 double SVhealth = 0.0;
1274 double datasource = 0.0;
1275
1276 int pos[4];
1277 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
1278 pos[1] = pos[0] + fieldLen;
1279 pos[2] = pos[1] + fieldLen;
1280 pos[3] = pos[2] + fieldLen;
1281
1282 // Read eight lines
1283 // ----------------
1284 for (int iLine = 0; iLine < nLines; iLine++) {
1285 QString line = lines[iLine];
1286
1287 if ( iLine == 0 ) {
1288 QTextStream in(line.left(pos[1]).toLatin1());
1289 QString n;
1290 in >> prnStr;
1291 if (prnStr.size() == 1 && prnStr[0] == 'E') {
1292 in >> n;
1293 prnStr.append(n);
1294 }
1295 in >> year >> month >> day >> hour >> min >> sec;
1296 if (year < 80) {
1297 year += 2000;
1298 }
1299 else if (year < 100) {
1300 year += 1900;
1301 }
1302
1303 _TOC.set(year, month, day, hour, min, sec);
1304
1305 if ( readDbl(line, pos[1], fieldLen, _clock_bias ) ||
1306 readDbl(line, pos[2], fieldLen, _clock_drift ) ||
1307 readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
1308 _checkState = bad;
1309 return;
1310 }
1311 }
1312 // =====================
1313 // BROADCAST ORBIT - 1
1314 // =====================
1315 else if ( iLine == 1 ) {
1316 if ( readDbl(line, pos[0], fieldLen, _IODnav ) ||
1317 readDbl(line, pos[1], fieldLen, _Crs ) ||
1318 readDbl(line, pos[2], fieldLen, _Delta_n) ||
1319 readDbl(line, pos[3], fieldLen, _M0 ) ) {
1320 _checkState = bad;
1321 return;
1322 }
1323 }
1324 // =====================
1325 // BROADCAST ORBIT - 2
1326 // =====================
1327 else if ( iLine == 2 ) {
1328 if ( readDbl(line, pos[0], fieldLen, _Cuc ) ||
1329 readDbl(line, pos[1], fieldLen, _e ) ||
1330 readDbl(line, pos[2], fieldLen, _Cus ) ||
1331 readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
1332 _checkState = bad;
1333 return;
1334 }
1335 }
1336 // =====================
1337 // BROADCAST ORBIT - 3
1338 // =====================
1339 else if ( iLine == 3 ) {
1340 if ( readDbl(line, pos[0], fieldLen, _TOEsec) ||
1341 readDbl(line, pos[1], fieldLen, _Cic ) ||
1342 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
1343 readDbl(line, pos[3], fieldLen, _Cis ) ) {
1344 _checkState = bad;
1345 return;
1346 }
1347 }
1348 // =====================
1349 // BROADCAST ORBIT - 4
1350 // =====================
1351 else if ( iLine == 4 ) {
1352 if ( readDbl(line, pos[0], fieldLen, _i0 ) ||
1353 readDbl(line, pos[1], fieldLen, _Crc ) ||
1354 readDbl(line, pos[2], fieldLen, _omega ) ||
1355 readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
1356 _checkState = bad;
1357 return;
1358 }
1359 }
1360 // =====================
1361 // BROADCAST ORBIT - 5
1362 // =====================
1363 else if ( iLine == 5 ) {
1364 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
1365 readDbl(line, pos[1], fieldLen, datasource) ||
1366 readDbl(line, pos[2], fieldLen, _TOEweek ) ) {
1367 _checkState = bad;
1368 return;
1369 } else {
1370 if (int(datasource) & (1<<8)) {
1371 _fnav = true;
1372 _inav = false;
1373 } else if (int(datasource) & (1<<9)) {
1374 _fnav = false;
1375 _inav = true;
1376 }
1377 _TOEweek -= 1024.0;
1378 }
1379 }
1380 // =====================
1381 // BROADCAST ORBIT - 6
1382 // =====================
1383 else if ( iLine == 6 ) {
1384 if ( readDbl(line, pos[0], fieldLen, _SISA ) ||
1385 readDbl(line, pos[1], fieldLen, SVhealth) ||
1386 readDbl(line, pos[2], fieldLen, _BGD_1_5A) ||
1387 readDbl(line, pos[3], fieldLen, _BGD_1_5B) ) {
1388 _checkState = bad;
1389 return;
1390 } else {
1391 // Bit 0
1392 _e1DataInValid = (int(SVhealth) & (1<<0));
1393 // Bit 1-2
1394 _E1_bHS = double((int(SVhealth) >> 1) & 0x3);
1395 // Bit 3
1396 _e5aDataInValid = (int(SVhealth) & (1<<3));
1397 // Bit 4-5
1398 _E5aHS = double((int(SVhealth) >> 4) & 0x3);
1399 // Bit 6
1400 _e5bDataInValid = (int(SVhealth) & (1<<6));
1401 // Bit 7-8
1402 _E5bHS = double((int(SVhealth) >> 7) & 0x3);
1403
1404 if (prnStr.at(0) == 'E') {
1405 _prn.set('E', prnStr.mid(1).toInt(), _inav ? 1 : 0);
1406 }
1407 }
1408 }
1409 // =====================
1410 // BROADCAST ORBIT - 7
1411 // =====================
1412 else if ( iLine == 7 ) {
1413 if ( readDbl(line, pos[0], fieldLen, _TOT) ) {
1414 _checkState = bad;
1415 return;
1416 }
1417 }
1418 }
1419}
1420
1421// Compute Galileo Satellite Position (virtual)
1422////////////////////////////////////////////////////////////////////////////
1423t_irc t_ephGal::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
1424
1425 static const double omegaEarth = 7292115.1467e-11;
1426 static const double gmWGS = 398.6004418e12;
1427
1428 memset(xc, 0, 6*sizeof(double));
1429 memset(vv, 0, 3*sizeof(double));
1430
1431 double a0 = _sqrt_A * _sqrt_A;
1432 if (a0 == 0) {
1433 return failure;
1434 }
1435
1436 double n0 = sqrt(gmWGS/(a0*a0*a0));
1437
1438 bncTime tt(GPSweek, GPSweeks);
1439 double tk = tt - bncTime(_TOC.gpsw(), _TOEsec);
1440
1441 double n = n0 + _Delta_n;
1442 double M = _M0 + n*tk;
1443 double E = M;
1444 double E_last;
1445 int nLoop = 0;
1446 do {
1447 E_last = E;
1448 E = M + _e*sin(E);
1449
1450 if (++nLoop == 100) {
1451 return failure;
1452 }
1453 } while ( fabs(E-E_last)*a0 > 0.001 );
1454 double v = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) );
1455 double u0 = v + _omega;
1456 double sin2u0 = sin(2*u0);
1457 double cos2u0 = cos(2*u0);
1458 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
1459 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
1460 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
1461 double xp = r*cos(u);
1462 double yp = r*sin(u);
1463 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
1464 omegaEarth*_TOEsec;
1465
1466 double sinom = sin(OM);
1467 double cosom = cos(OM);
1468 double sini = sin(i);
1469 double cosi = cos(i);
1470 xc[0] = xp*cosom - yp*cosi*sinom;
1471 xc[1] = xp*sinom + yp*cosi*cosom;
1472 xc[2] = yp*sini;
1473
1474 double tc = tt - _TOC;
1475 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
1476
1477 // Velocity
1478 // --------
1479 double tanv2 = tan(v/2);
1480 double dEdM = 1 / (1 - _e*cos(E));
1481 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
1482 * dEdM * n;
1483 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
1484 double dotom = _OMEGADOT - omegaEarth;
1485 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
1486 double dotr = a0 * _e*sin(E) * dEdM * n
1487 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
1488 double dotx = dotr*cos(u) - r*sin(u)*dotu;
1489 double doty = dotr*sin(u) + r*cos(u)*dotu;
1490
1491 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
1492 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
1493 + yp*sini*sinom*doti; // dX / di
1494
1495 vv[1] = sinom *dotx + cosi*cosom *doty
1496 + xp*cosom*dotom - yp*cosi*sinom*dotom
1497 - yp*sini*cosom*doti;
1498
1499 vv[2] = sini *doty + yp*cosi *doti;
1500
1501 // Relativistic Correction
1502 // -----------------------
1503 xc[3] -= 4.442807309e-10 * _e * sqrt(a0) *sin(E);
1504
1505 xc[4] = _clock_drift + _clock_driftrate*tc;
1506 xc[5] = _clock_driftrate;
1507
1508 return success;
1509}
1510
1511// Health status of Galileo Ephemeris (virtual)
1512////////////////////////////////////////////////////////////////////////////
1513unsigned int t_ephGal::isUnhealthy() const {
1514 if (_E5aHS == 1 || _E5aHS == 3 ||
1515 _E5bHS == 1 || _E5bHS == 3 ||
1516 _E1_bHS == 1 || _E1_bHS == 3 ) {
1517 return 1;
1518 }
1519 if (_e5aDataInValid ||
1520 _e5bDataInValid ||
1521 _e1DataInValid) {
1522 return 1;
1523 }
1524 if (_SISA == 255.0) {
1525 return 1;
1526 }
1527 /*
1528 * SDD v1.3: SHS=2 leads to a newly-defined "EOM" status.
1529 * It also means that the satellite signal may be used for PNT.
1530 if (_E5aHS == 2 ||
1531 _E5bHS == 2 ||
1532 _E1_bHS == 2 ) {
1533 return 1;
1534 }
1535 */
1536 return 0;
1537}
1538
1539// RINEX Format String
1540//////////////////////////////////////////////////////////////////////////////
1541QString t_ephGal::toString(double version) const {
1542
1543 QString navStr = navTypeString(_navType, _prn, version);
1544 QString rnxStr = navStr + rinexDateStr(_TOC, _prn, version);
1545
1546 QTextStream out(&rnxStr);
1547
1548 out << QString("%1%2%3\n")
1549 .arg(_clock_bias, 19, 'e', 12)
1550 .arg(_clock_drift, 19, 'e', 12)
1551 .arg(_clock_driftrate, 19, 'e', 12);
1552
1553 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1554 // =====================
1555 // BROADCAST ORBIT - 1
1556 // =====================
1557 out << QString(fmt)
1558 .arg(_IODnav, 19, 'e', 12)
1559 .arg(_Crs, 19, 'e', 12)
1560 .arg(_Delta_n, 19, 'e', 12)
1561 .arg(_M0, 19, 'e', 12);
1562 // =====================
1563 // BROADCAST ORBIT - 2
1564 // =====================
1565 out << QString(fmt)
1566 .arg(_Cuc, 19, 'e', 12)
1567 .arg(_e, 19, 'e', 12)
1568 .arg(_Cus, 19, 'e', 12)
1569 .arg(_sqrt_A, 19, 'e', 12);
1570 // =====================
1571 // BROADCAST ORBIT - 3
1572 // =====================
1573 out << QString(fmt)
1574 .arg(_TOEsec, 19, 'e', 12)
1575 .arg(_Cic, 19, 'e', 12)
1576 .arg(_OMEGA0, 19, 'e', 12)
1577 .arg(_Cis, 19, 'e', 12);
1578 // =====================
1579 // BROADCAST ORBIT - 4
1580 // =====================
1581 out << QString(fmt)
1582 .arg(_i0, 19, 'e', 12)
1583 .arg(_Crc, 19, 'e', 12)
1584 .arg(_omega, 19, 'e', 12)
1585 .arg(_OMEGADOT, 19, 'e', 12);
1586 // =====================
1587 // BROADCAST ORBIT - 5
1588 // =====================
1589 int dataSource = 0;
1590 int SVhealth = 0;
1591 double BGD_1_5A = _BGD_1_5A;
1592 double BGD_1_5B = _BGD_1_5B;
1593 if (_fnav) {
1594 dataSource |= (1<<1);
1595 dataSource |= (1<<8);
1596 BGD_1_5B = 0.0;
1597 // SVhealth
1598 // Bit 3 : E5a DVS
1599 if (_e5aDataInValid) {
1600 SVhealth |= (1<<3);
1601 }
1602 // Bit 4-5: E5a HS
1603 if (_E5aHS == 1.0) {
1604 SVhealth |= (1<<4);
1605 }
1606 else if (_E5aHS == 2.0) {
1607 SVhealth |= (1<<5);
1608 }
1609 else if (_E5aHS == 3.0) {
1610 SVhealth |= (1<<4);
1611 SVhealth |= (1<<5);
1612 }
1613 }
1614 else if(_inav) {
1615 // Bit 2 and 0 are set because from MT1046 the data source cannot be determined
1616 // and RNXv3.03 says both can be set if the navigation messages were merged
1617 dataSource |= (1<<0);
1618 dataSource |= (1<<2);
1619 dataSource |= (1<<9);
1620 // SVhealth
1621 // Bit 0 : E1-B DVS
1622 if (_e1DataInValid) {
1623 SVhealth |= (1<<0);
1624 }
1625 // Bit 1-2: E1-B HS
1626 if (_E1_bHS == 1.0) {
1627 SVhealth |= (1<<1);
1628 }
1629 else if (_E1_bHS == 2.0) {
1630 SVhealth |= (1<<2);
1631 }
1632 else if (_E1_bHS == 3.0) {
1633 SVhealth |= (1<<1);
1634 SVhealth |= (1<<2);
1635 }
1636 // Bit 3 : E5a DVS
1637 if (_e5aDataInValid) {
1638 SVhealth |= (1<<3);
1639 }
1640 // Bit 4-5: E5a HS
1641 if (_E5aHS == 1.0) {
1642 SVhealth |= (1<<4);
1643 }
1644 else if (_E5aHS == 2.0) {
1645 SVhealth |= (1<<5);
1646 }
1647 else if (_E5aHS == 3.0) {
1648 SVhealth |= (1<<4);
1649 SVhealth |= (1<<5);
1650 }
1651 // Bit 6 : E5b DVS
1652 if (_e5bDataInValid) {
1653 SVhealth |= (1<<6);
1654 }
1655 // Bit 7-8: E5b HS
1656 if (_E5bHS == 1.0) {
1657 SVhealth |= (1<<7);
1658 }
1659 else if (_E5bHS == 2.0) {
1660 SVhealth |= (1<<8);
1661 }
1662 else if (_E5bHS == 3.0) {
1663 SVhealth |= (1<<7);
1664 SVhealth |= (1<<8);
1665 }
1666 }
1667
1668 out << QString(fmt)
1669 .arg(_IDOT, 19, 'e', 12)
1670 .arg(double(dataSource), 19, 'e', 12)
1671 .arg(_TOEweek + 1024.0, 19, 'e', 12)
1672 .arg(0.0, 19, 'e', 12);
1673 // =====================
1674 // BROADCAST ORBIT - 6
1675 // =====================
1676 out << QString(fmt)
1677 .arg(_SISA, 19, 'e', 12)
1678 .arg(double(SVhealth), 19, 'e', 12)
1679 .arg(BGD_1_5A, 19, 'e', 12)
1680 .arg(BGD_1_5B, 19, 'e', 12);
1681 // =====================
1682 // BROADCAST ORBIT - 7
1683 // =====================
1684 double tot = _TOT;
1685 if (tot == 0.9999e9 && version < 3.0) {
1686 tot = 0.0;
1687 }
1688 out << QString(fmt)
1689 .arg(tot, 19, 'e', 12)
1690 .arg("", 19, QChar(' '))
1691 .arg("", 19, QChar(' '))
1692 .arg("", 19, QChar(' '));
1693
1694 return rnxStr;
1695}
1696
1697// Constructor
1698//////////////////////////////////////////////////////////////////////////////
1699t_ephSBAS::t_ephSBAS(double rnxVersion, const QStringList& lines) {
1700
1701 const int nLines = 4;
1702
1703 if (lines.size() != nLines) {
1704 _checkState = bad;
1705 return;
1706 }
1707
1708 // RINEX Format
1709 // ------------
1710 int fieldLen = 19;
1711
1712 int pos[4];
1713 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
1714 pos[1] = pos[0] + fieldLen;
1715 pos[2] = pos[1] + fieldLen;
1716 pos[3] = pos[2] + fieldLen;
1717
1718 // Read four lines
1719 // ---------------
1720 for (int iLine = 0; iLine < nLines; iLine++) {
1721 QString line = lines[iLine];
1722
1723 if ( iLine == 0 ) {
1724 QTextStream in(line.left(pos[1]).toLatin1());
1725
1726 int year, month, day, hour, min;
1727 double sec;
1728
1729 QString prnStr, n;
1730 in >> prnStr;
1731 if (prnStr.size() == 1 && prnStr[0] == 'S') {
1732 in >> n;
1733 prnStr.append(n);
1734 }
1735 in >> year >> month >> day >> hour >> min >> sec;
1736 if (prnStr.at(0) == 'S') {
1737 _prn.set('S', prnStr.mid(1).toInt());
1738 }
1739 else {
1740 _prn.set('S', prnStr.toInt());
1741 }
1742
1743 if (year < 80) {
1744 year += 2000;
1745 }
1746 else if (year < 100) {
1747 year += 1900;
1748 }
1749
1750 _TOC.set(year, month, day, hour, min, sec);
1751
1752 if ( readDbl(line, pos[1], fieldLen, _agf0 ) ||
1753 readDbl(line, pos[2], fieldLen, _agf1 ) ||
1754 readDbl(line, pos[3], fieldLen, _TOT ) ) {
1755 _checkState = bad;
1756 return;
1757 }
1758 }
1759 // =====================
1760 // BROADCAST ORBIT - 1
1761 // =====================
1762 else if ( iLine == 1 ) {
1763 if ( readDbl(line, pos[0], fieldLen, _x_pos ) ||
1764 readDbl(line, pos[1], fieldLen, _x_velocity ) ||
1765 readDbl(line, pos[2], fieldLen, _x_acceleration) ||
1766 readDbl(line, pos[3], fieldLen, _health ) ) {
1767 _checkState = bad;
1768 return;
1769 }
1770 }
1771 // =====================
1772 // BROADCAST ORBIT - 2
1773 // =====================
1774 else if ( iLine == 2 ) {
1775 if ( readDbl(line, pos[0], fieldLen, _y_pos ) ||
1776 readDbl(line, pos[1], fieldLen, _y_velocity ) ||
1777 readDbl(line, pos[2], fieldLen, _y_acceleration ) ||
1778 readDbl(line, pos[3], fieldLen, _ura ) ) {
1779 _checkState = bad;
1780 return;
1781 }
1782 }
1783 // =====================
1784 // BROADCAST ORBIT - 3
1785 // =====================
1786 else if ( iLine == 3 ) {
1787 double iodn;
1788 if ( readDbl(line, pos[0], fieldLen, _z_pos ) ||
1789 readDbl(line, pos[1], fieldLen, _z_velocity ) ||
1790 readDbl(line, pos[2], fieldLen, _z_acceleration) ||
1791 readDbl(line, pos[3], fieldLen, iodn ) ) {
1792 _checkState = bad;
1793 return;
1794 } else {
1795 _IODN = int(iodn);
1796 }
1797 }
1798 }
1799
1800 _x_pos *= 1.e3;
1801 _y_pos *= 1.e3;
1802 _z_pos *= 1.e3;
1803 _x_velocity *= 1.e3;
1804 _y_velocity *= 1.e3;
1805 _z_velocity *= 1.e3;
1806 _x_acceleration *= 1.e3;
1807 _y_acceleration *= 1.e3;
1808 _z_acceleration *= 1.e3;
1809}
1810
1811// IOD of SBAS Ephemeris (virtual)
1812////////////////////////////////////////////////////////////////////////////
1813
1814unsigned int t_ephSBAS::IOD() const {
1815 unsigned char buffer[80];
1816 int size = 0;
1817 int numbits = 0;
1818 long long bitbuffer = 0;
1819 unsigned char *startbuffer = buffer;
1820
1821 SBASADDBITSFLOAT(30, this->_x_pos, 0.08)
1822 SBASADDBITSFLOAT(30, this->_y_pos, 0.08)
1823 SBASADDBITSFLOAT(25, this->_z_pos, 0.4)
1824 SBASADDBITSFLOAT(17, this->_x_velocity, 0.000625)
1825 SBASADDBITSFLOAT(17, this->_y_velocity, 0.000625)
1826 SBASADDBITSFLOAT(18, this->_z_velocity, 0.004)
1827 SBASADDBITSFLOAT(10, this->_x_acceleration, 0.0000125)
1828 SBASADDBITSFLOAT(10, this->_y_acceleration, 0.0000125)
1829 SBASADDBITSFLOAT(10, this->_z_acceleration, 0.0000625)
1830 SBASADDBITSFLOAT(12, this->_agf0, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
1831 SBASADDBITSFLOAT(8, this->_agf1, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<10))
1832 SBASADDBITS(5,0); // the last byte is filled by 0-bits to obtain a length of an integer multiple of 8
1833
1834 return CRC24(size, startbuffer);
1835}
1836
1837// Compute SBAS Satellite Position (virtual)
1838////////////////////////////////////////////////////////////////////////////
1839t_irc t_ephSBAS::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
1840
1841 bncTime tt(GPSweek, GPSweeks);
1842 double dt = tt - _TOC;
1843
1844 xc[0] = _x_pos + _x_velocity * dt + _x_acceleration * dt * dt / 2.0;
1845 xc[1] = _y_pos + _y_velocity * dt + _y_acceleration * dt * dt / 2.0;
1846 xc[2] = _z_pos + _z_velocity * dt + _z_acceleration * dt * dt / 2.0;
1847
1848 vv[0] = _x_velocity + _x_acceleration * dt;
1849 vv[1] = _y_velocity + _y_acceleration * dt;
1850 vv[2] = _z_velocity + _z_acceleration * dt;
1851
1852 xc[3] = _agf0 + _agf1 * dt;
1853
1854 xc[4] = _agf1;
1855 xc[5] = 0.0;
1856
1857 return success;
1858}
1859
1860// Health status of SBAS Ephemeris (virtual)
1861////////////////////////////////////////////////////////////////////////////
1862unsigned int t_ephSBAS::isUnhealthy() const {
1863
1864 // Bit 5
1865 bool URAindexIs15 = (int(_health) & (1<<5));
1866 if (URAindexIs15) {
1867 // in this case it is recommended
1868 // to set the bits 0,1,2,3 to 1 (MT17health = 15)
1869 return 1;
1870 }
1871
1872 // Bit 0-3
1873 int MT17health = (int(_health)) & (0x0f);
1874 if (MT17health) {
1875 return 1;
1876 }
1877
1878 // Bit 4
1879 // bool MT17HealthIsUnavailable = (int(_health) & (1<<4));
1880 // is not used because the MT17health bits can be independently set if URA index is 15
1881
1882 return 0;
1883}
1884
1885
1886// RINEX Format String
1887//////////////////////////////////////////////////////////////////////////////
1888QString t_ephSBAS::toString(double version) const {
1889
1890 QString navStr = navTypeString(_navType, _prn, version);
1891 QString rnxStr = navStr + rinexDateStr(_TOC, _prn, version);
1892
1893 QTextStream out(&rnxStr);
1894
1895 out << QString("%1%2%3\n")
1896 .arg(_agf0, 19, 'e', 12)
1897 .arg(_agf1, 19, 'e', 12)
1898 .arg(_TOT, 19, 'e', 12);
1899
1900 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1901 // =====================
1902 // BROADCAST ORBIT - 1
1903 // =====================
1904 out << QString(fmt)
1905 .arg(1.e-3*_x_pos, 19, 'e', 12)
1906 .arg(1.e-3*_x_velocity, 19, 'e', 12)
1907 .arg(1.e-3*_x_acceleration, 19, 'e', 12)
1908 .arg(_health, 19, 'e', 12);
1909 // =====================
1910 // BROADCAST ORBIT - 2
1911 // =====================
1912 out << QString(fmt)
1913 .arg(1.e-3*_y_pos, 19, 'e', 12)
1914 .arg(1.e-3*_y_velocity, 19, 'e', 12)
1915 .arg(1.e-3*_y_acceleration, 19, 'e', 12)
1916 .arg(_ura, 19, 'e', 12);
1917 // =====================
1918 // BROADCAST ORBIT - 3
1919 // =====================
1920 out << QString(fmt)
1921 .arg(1.e-3*_z_pos, 19, 'e', 12)
1922 .arg(1.e-3*_z_velocity, 19, 'e', 12)
1923 .arg(1.e-3*_z_acceleration, 19, 'e', 12)
1924 .arg(double(_IODN), 19, 'e', 12);
1925
1926 return rnxStr;
1927}
1928
1929// Constructor
1930//////////////////////////////////////////////////////////////////////////////
1931t_ephBDS::t_ephBDS(double rnxVersion, const QStringList& lines) {
1932
1933 int nLines = 8;
1934
1935 if (navType() == t_eph::CNV1 ||
1936 navType() == t_eph::CNV2) {
1937 nLines += 2;
1938 }
1939 if (navType() == t_eph::CNV3) {
1940 nLines += 1;
1941 }
1942
1943 if (lines.size() != nLines) {
1944 _checkState = bad;
1945 return;
1946 }
1947
1948 // RINEX Format
1949 // ------------
1950 int fieldLen = 19;
1951
1952 int pos[4];
1953 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
1954 pos[1] = pos[0] + fieldLen;
1955 pos[2] = pos[1] + fieldLen;
1956 pos[3] = pos[2] + fieldLen;
1957
1958 // Read eight lines
1959 // ----------------
1960 for (int iLine = 0; iLine < nLines; iLine++) {
1961 QString line = lines[iLine];
1962
1963 if ( iLine == 0 ) {
1964 QTextStream in(line.left(pos[1]).toLatin1());
1965
1966 int year, month, day, hour, min;
1967 double sec;
1968
1969 QString prnStr, n;
1970 in >> prnStr;
1971 if (prnStr.size() == 1 && prnStr[0] == 'C') {
1972 in >> n;
1973 prnStr.append(n);
1974 }
1975 in >> year >> month >> day >> hour >> min >> sec;
1976 if (prnStr.at(0) == 'C') {
1977 _prn.set('C', prnStr.mid(1).toInt());
1978 }
1979 else {
1980 _prn.set('C', prnStr.toInt());
1981 }
1982
1983 if (year < 80) {
1984 year += 2000;
1985 }
1986 else if (year < 100) {
1987 year += 1900;
1988 }
1989
1990 _TOC.setBDS(year, month, day, hour, min, sec);
1991
1992 if ( readDbl(line, pos[1], fieldLen, _clock_bias ) ||
1993 readDbl(line, pos[2], fieldLen, _clock_drift ) ||
1994 readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
1995 _checkState = bad;
1996 return;
1997 }
1998 }
1999 // =====================
2000 // BROADCAST ORBIT - 1
2001 // =====================
2002 else if ( iLine == 1 ) {
2003 double aode;
2004 if ( readDbl(line, pos[0], fieldLen, aode ) ||
2005 readDbl(line, pos[1], fieldLen, _Crs ) ||
2006 readDbl(line, pos[2], fieldLen, _Delta_n) ||
2007 readDbl(line, pos[3], fieldLen, _M0 ) ) {
2008 _checkState = bad;
2009 return;
2010 }
2011 _AODE = int(aode);
2012 }
2013 // =====================
2014 // BROADCAST ORBIT - 2
2015 // =====================
2016 else if ( iLine == 2 ) {
2017 if ( readDbl(line, pos[0], fieldLen, _Cuc ) ||
2018 readDbl(line, pos[1], fieldLen, _e ) ||
2019 readDbl(line, pos[2], fieldLen, _Cus ) ||
2020 readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
2021 _checkState = bad;
2022 return;
2023 }
2024 }
2025 // =====================
2026 // BROADCAST ORBIT - 3
2027 // =====================
2028 else if ( iLine == 3 ) {
2029 if ( readDbl(line, pos[0], fieldLen, _TOEsec ) ||
2030 readDbl(line, pos[1], fieldLen, _Cic ) ||
2031 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
2032 readDbl(line, pos[3], fieldLen, _Cis ) ) {
2033 _checkState = bad;
2034 return;
2035 }
2036 }
2037 // =====================
2038 // BROADCAST ORBIT - 4
2039 // =====================
2040 else if ( iLine == 4 ) {
2041 if ( readDbl(line, pos[0], fieldLen, _i0 ) ||
2042 readDbl(line, pos[1], fieldLen, _Crc ) ||
2043 readDbl(line, pos[2], fieldLen, _omega ) ||
2044 readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
2045 _checkState = bad;
2046 return;
2047 }
2048 }
2049 // =====================
2050 // BROADCAST ORBIT - 5
2051 // =====================
2052 else if ( iLine == 5 ) {
2053
2054 if (navType() == t_eph::CNV1 ||
2055 navType() == t_eph::CNV2 ||
2056 navType() == t_eph::CNV3 ) {
2057 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
2058 readDbl(line, pos[1], fieldLen, _Delta_n_dot) ||
2059 readDbl(line, pos[2], fieldLen, _satType ) ||
2060 readDbl(line, pos[3], fieldLen, _top ) ) {
2061 _checkState = bad;
2062 return;
2063 }
2064 }
2065 else { // D1, D2, undefined
2066 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
2067 readDbl(line, pos[2], fieldLen, _BDTweek)) {
2068 _checkState = bad;
2069 return;
2070 }
2071 }
2072 }
2073 // =====================
2074 // BROADCAST ORBIT - 6
2075 // =====================
2076 else if ( iLine == 6 ) {
2077 if (navType() == t_eph::CNV1 ||
2078 navType() == t_eph::CNV2 ||
2079 navType() == t_eph::CNV3 ) {
2080 if ( readDbl(line, pos[0], fieldLen, _SISAI_oe ) ||
2081 readDbl(line, pos[1], fieldLen, _SISAI_ocb ) ||
2082 readDbl(line, pos[2], fieldLen, _SISAI_oc1 ) ||
2083 readDbl(line, pos[3], fieldLen, _SISAI_oc2 ) ) {
2084 _checkState = bad;
2085 return;
2086 }
2087 }
2088 else { // D1, D2, undefined
2089 double SatH1;
2090 if ( readDbl(line, pos[0], fieldLen, _URA ) ||
2091 readDbl(line, pos[1], fieldLen, SatH1) ||
2092 readDbl(line, pos[2], fieldLen, _TGD1) ||
2093 readDbl(line, pos[3], fieldLen, _TGD2) ) {
2094 _checkState = bad;
2095 return;
2096 }
2097 _SatH1 = int(SatH1);
2098 }
2099 }
2100 // =====================
2101 // BROADCAST ORBIT - 7
2102 // =====================
2103 else if ( iLine == 7 ) {
2104 if (navType() == t_eph::CNV1) {
2105 if ( readDbl(line, pos[0], fieldLen, _ISC_B1Cd) ||
2106 readDbl(line, pos[2], fieldLen, _TGD_B1Cp) ||
2107 readDbl(line, pos[3], fieldLen, _TGD_B2ap) ) {
2108 _checkState = bad;
2109 return;
2110 }
2111 }
2112 else if (navType() == t_eph::CNV2) {
2113 if ( readDbl(line, pos[1], fieldLen, _ISC_B2ad) ||
2114 readDbl(line, pos[2], fieldLen, _TGD_B1Cp) ||
2115 readDbl(line, pos[3], fieldLen, _TGD_B2ap) ) {
2116 _checkState = bad;
2117 return;
2118 }
2119 }
2120 else if (navType() == t_eph::CNV3) {
2121 double health;
2122 if ( readDbl(line, pos[0], fieldLen, _SISMAI ) ||
2123 readDbl(line, pos[1], fieldLen, health ) ||
2124 readDbl(line, pos[2], fieldLen, _INTEGRITYF_B2b) ||
2125 readDbl(line, pos[3], fieldLen, _TGD_B2bI) ) {
2126 _checkState = bad;
2127 return;
2128 }
2129 _health = int(health);
2130 }
2131 else { // D1, D2 or undefined
2132 double aodc;
2133 if ( readDbl(line, pos[0], fieldLen, _TOT) ||
2134 readDbl(line, pos[1], fieldLen, aodc) ) {
2135 _checkState = bad;
2136 return;
2137 }
2138 if (_TOT == 0.9999e9) { // 0.9999e9 means not known (RINEX standard)
2139 _TOT = _TOEsec;
2140 }
2141 _AODC = int(aodc);
2142 }
2143 }
2144 // =====================
2145 // BROADCAST ORBIT - 8
2146 // =====================
2147 else if ( iLine == 8 ) {
2148 double health;
2149 if (navType() == t_eph::CNV1) {
2150 if ( readDbl(line, pos[0], fieldLen, _SISMAI ) ||
2151 readDbl(line, pos[1], fieldLen, health ) ||
2152 readDbl(line, pos[2], fieldLen, _INTEGRITYF_B1C) ||
2153 readDbl(line, pos[3], fieldLen, _IODC) ) {
2154 _checkState = bad;
2155 return;
2156 }
2157 _health = int(health);
2158 }
2159 else if (navType() == t_eph::CNV2) {
2160 if ( readDbl(line, pos[0], fieldLen, _SISMAI ) ||
2161 readDbl(line, pos[1], fieldLen, health ) ||
2162 readDbl(line, pos[2], fieldLen, _INTEGRITYF_B2aB1C) ||
2163 readDbl(line, pos[3], fieldLen, _IODC) ) {
2164 _checkState = bad;
2165 return;
2166 }
2167 _health = int(health);
2168 }
2169 else if (navType() == t_eph::CNV3) {
2170 if ( readDbl(line, pos[0], fieldLen, _TOT)) {
2171 _checkState = bad;
2172 return;
2173 }
2174 }
2175
2176 }
2177 // =====================
2178 // BROADCAST ORBIT - 9
2179 // =====================
2180 else if ( iLine == 9 ) {
2181
2182 if (navType() == t_eph::CNV1 ||
2183 navType() == t_eph::CNV2) {
2184 if ( readDbl(line, pos[0], fieldLen, _TOT) ||
2185 readDbl(line, pos[3], fieldLen, _IODE) ) {
2186 _checkState = bad;
2187 return;
2188 }
2189 }
2190
2191 }
2192 }
2193
2194 _TOE.setBDS(int(_BDTweek), _TOEsec);
2195 // remark: actually should be computed from second_tot
2196 // but it seems to be unreliable in RINEX files
2197 //_TOT = _TOC.bdssec();
2198}
2199
2200// IOD of BDS Ephemeris (virtual)
2201////////////////////////////////////////////////////////////////////////////
2202unsigned int t_ephBDS::IOD() const {
2203 return (int(_TOC.gpssec())/720) % 240; //return (int(_TOEsec)/720) % 240;
2204}
2205
2206// Compute BDS Satellite Position (virtual)
2207//////////////////////////////////////////////////////////////////////////////
2208t_irc t_ephBDS::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
2209
2210 static const double gmBDS = 398.6004418e12;
2211 static const double omegaBDS = 7292115.0000e-11;
2212
2213 xc[0] = xc[1] = xc[2] = xc[3] = 0.0;
2214 vv[0] = vv[1] = vv[2] = 0.0;
2215
2216 bncTime tt(GPSweek, GPSweeks);
2217
2218 if (_sqrt_A == 0) {
2219 return failure;
2220 }
2221 double a0 = _sqrt_A * _sqrt_A;
2222
2223 double n0 = sqrt(gmBDS/(a0*a0*a0));
2224 double tk = tt - _TOE;
2225 double n = n0 + _Delta_n;
2226 double M = _M0 + n*tk;
2227 double E = M;
2228 double E_last;
2229 int nLoop = 0;
2230 do {
2231 E_last = E;
2232 E = M + _e*sin(E);
2233
2234 if (++nLoop == 100) {
2235 return failure;
2236 }
2237 } while ( fabs(E-E_last)*a0 > 0.001 );
2238
2239 double v = atan2(sqrt(1-_e*_e) * sin(E), cos(E) - _e);
2240 double u0 = v + _omega;
2241 double sin2u0 = sin(2*u0);
2242 double cos2u0 = cos(2*u0);
2243 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
2244 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
2245 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
2246 double xp = r*cos(u);
2247 double yp = r*sin(u);
2248 double toesec = (_TOE.gpssec() - 14.0);
2249 double sinom = 0;
2250 double cosom = 0;
2251 double sini = 0;
2252 double cosi = 0;
2253
2254 // Velocity
2255 // --------
2256 double tanv2 = tan(v/2);
2257 double dEdM = 1 / (1 - _e*cos(E));
2258 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2)
2259 / (1 + tanv2*tanv2) * dEdM * n;
2260 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
2261 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
2262 double dotr = a0 * _e*sin(E) * dEdM * n
2263 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
2264
2265 double dotx = dotr*cos(u) - r*sin(u)*dotu;
2266 double doty = dotr*sin(u) + r*cos(u)*dotu;
2267
2268 const double iMaxGEO = 10.0 / 180.0 * M_PI;
2269
2270 // MEO/IGSO satellite
2271 // ------------------
2272 if (_i0 > iMaxGEO) {
2273 double OM = _OMEGA0 + (_OMEGADOT - omegaBDS)*tk - omegaBDS*toesec;
2274
2275 sinom = sin(OM);
2276 cosom = cos(OM);
2277 sini = sin(i);
2278 cosi = cos(i);
2279
2280 xc[0] = xp*cosom - yp*cosi*sinom;
2281 xc[1] = xp*sinom + yp*cosi*cosom;
2282 xc[2] = yp*sini;
2283
2284 // Velocity
2285 // --------
2286
2287 double dotom = _OMEGADOT - t_CST::omega;
2288
2289 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
2290 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
2291 + yp*sini*sinom*doti; // dX / di
2292
2293 vv[1] = sinom *dotx + cosi*cosom *doty
2294 + xp*cosom*dotom - yp*cosi*sinom*dotom
2295 - yp*sini*cosom*doti;
2296
2297 vv[2] = sini *doty + yp*cosi *doti;
2298
2299 }
2300
2301 // GEO satellite
2302 // -------------
2303 else {
2304 double OM = _OMEGA0 + _OMEGADOT*tk - omegaBDS*toesec;
2305 double ll = omegaBDS*tk;
2306
2307 sinom = sin(OM);
2308 cosom = cos(OM);
2309 sini = sin(i);
2310 cosi = cos(i);
2311
2312 double xx = xp*cosom - yp*cosi*sinom;
2313 double yy = xp*sinom + yp*cosi*cosom;
2314 double zz = yp*sini;
2315
2316 Matrix RX = BNC_PPP::t_astro::rotX(-5.0 / 180.0 * M_PI);
2317 Matrix RZ = BNC_PPP::t_astro::rotZ(ll);
2318
2319 ColumnVector X1(3); X1 << xx << yy << zz;
2320 ColumnVector X2 = RZ*RX*X1;
2321
2322 xc[0] = X2(1);
2323 xc[1] = X2(2);
2324 xc[2] = X2(3);
2325
2326 double dotom = _OMEGADOT;
2327
2328 double vx = cosom *dotx - cosi*sinom *doty
2329 - xp*sinom*dotom - yp*cosi*cosom*dotom
2330 + yp*sini*sinom*doti;
2331
2332 double vy = sinom *dotx + cosi*cosom *doty
2333 + xp*cosom*dotom - yp*cosi*sinom*dotom
2334 - yp*sini*cosom*doti;
2335
2336 double vz = sini *doty + yp*cosi *doti;
2337
2338 ColumnVector V(3); V << vx << vy << vz;
2339
2340 Matrix RdotZ(3,3);
2341 double C = cos(ll);
2342 double S = sin(ll);
2343 Matrix UU(3,3);
2344 UU[0][0] = -S; UU[0][1] = +C; UU[0][2] = 0.0;
2345 UU[1][0] = -C; UU[1][1] = -S; UU[1][2] = 0.0;
2346 UU[2][0] = 0.0; UU[2][1] = 0.0; UU[2][2] = 0.0;
2347 RdotZ = omegaBDS * UU;
2348
2349 ColumnVector VV(3);
2350 VV = RZ*RX*V + RdotZ*RX*X1;
2351
2352 vv[0] = VV(1);
2353 vv[1] = VV(2);
2354 vv[2] = VV(3);
2355 }
2356
2357 double tc = tt - _TOC;
2358 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
2359
2360// dotC = _clock_drift + _clock_driftrate*tc
2361// - 4.442807309e-10*_e * sqrt(a0) * cos(E) * dEdM * n;
2362
2363 // Relativistic Correction
2364 // -----------------------
2365 xc[3] -= 4.442807309e-10 * _e * sqrt(a0) *sin(E);
2366
2367 xc[4] = _clock_drift + _clock_driftrate*tc;
2368 xc[5] = _clock_driftrate;
2369
2370 return success;
2371}
2372
2373
2374// Health status of SBAS Ephemeris (virtual)
2375////////////////////////////////////////////////////////////////////////////
2376unsigned int t_ephBDS::isUnhealthy() const {
2377
2378 if (navType() == t_eph::CNV1 ||
2379 navType() == t_eph::CNV2 ||
2380 navType() == t_eph::CNV3) {
2381 return static_cast<unsigned int>(_health);
2382 }
2383
2384 return static_cast<unsigned int>(_SatH1);
2385
2386}
2387
2388
2389// RINEX Format String
2390//////////////////////////////////////////////////////////////////////////////
2391QString t_ephBDS::toString(double version) const {
2392
2393 QString navStr = navTypeString(_navType, _prn, version);
2394 QString rnxStr = navStr + rinexDateStr(_TOC-14.0, _prn, version);
2395
2396 QTextStream out(&rnxStr);
2397
2398 out << QString("%1%2%3\n")
2399 .arg(_clock_bias, 19, 'e', 12)
2400 .arg(_clock_drift, 19, 'e', 12)
2401 .arg(_clock_driftrate, 19, 'e', 12);
2402
2403 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
2404 // =====================
2405 // BROADCAST ORBIT - 1
2406 // =====================
2407 out << QString(fmt)
2408 .arg(double(_AODE), 19, 'e', 12)
2409 .arg(_Crs, 19, 'e', 12)
2410 .arg(_Delta_n, 19, 'e', 12)
2411 .arg(_M0, 19, 'e', 12);
2412 // =====================
2413 // BROADCAST ORBIT - 2
2414 // =====================
2415 out << QString(fmt)
2416 .arg(_Cuc, 19, 'e', 12)
2417 .arg(_e, 19, 'e', 12)
2418 .arg(_Cus, 19, 'e', 12)
2419 .arg(_sqrt_A, 19, 'e', 12);
2420 // =====================
2421 // BROADCAST ORBIT - 3
2422 // =====================
2423 out << QString(fmt)
2424 .arg(_TOEsec, 19, 'e', 12)
2425 .arg(_Cic, 19, 'e', 12)
2426 .arg(_OMEGA0, 19, 'e', 12)
2427 .arg(_Cis, 19, 'e', 12);
2428 // =====================
2429 // BROADCAST ORBIT - 4
2430 // =====================
2431 out << QString(fmt)
2432 .arg(_i0, 19, 'e', 12)
2433 .arg(_Crc, 19, 'e', 12)
2434 .arg(_omega, 19, 'e', 12)
2435 .arg(_OMEGADOT, 19, 'e', 12);
2436 // =====================
2437 // BROADCAST ORBIT - 5
2438 // =====================
2439 if (navType() == t_eph::CNV1 ||
2440 navType() == t_eph::CNV2 ||
2441 navType() == t_eph::CNV3 ) {
2442 out << QString(fmt)
2443 .arg(_IDOT, 19, 'e', 12)
2444 .arg(_Delta_n_dot, 19, 'e', 12)
2445 .arg(_satType, 19, 'e', 12)
2446 .arg(_top, 19, 'e', 12);
2447 }
2448 else { // D1, D2, undefined
2449 out << QString(fmt)
2450 .arg(_IDOT, 19, 'e', 12)
2451 .arg("", 19, QChar(' '))
2452 .arg(_BDTweek, 19, 'e', 12)
2453 .arg("", 19, QChar(' '));
2454 }
2455 // =====================
2456 // BROADCAST ORBIT - 6
2457 // =====================
2458 if (navType() == t_eph::CNV1 ||
2459 navType() == t_eph::CNV2 ||
2460 navType() == t_eph::CNV3 ) {
2461 out << QString(fmt)
2462 .arg(_SISAI_oe, 19, 'e', 12)
2463 .arg(_SISAI_ocb, 19, 'e', 12)
2464 .arg(_SISAI_oc1, 19, 'e', 12)
2465 .arg(_SISAI_oc2, 19, 'e', 12);
2466 }
2467 else { // D1, D2, undefined
2468 out << QString(fmt)
2469 .arg(_URA, 19, 'e', 12)
2470 .arg(double(_SatH1), 19, 'e', 12)
2471 .arg(_TGD1, 19, 'e', 12)
2472 .arg(_TGD2, 19, 'e', 12);
2473 }
2474 // =====================
2475 // BROADCAST ORBIT - 7
2476 // =====================
2477 if (navType() == t_eph::CNV1) {
2478 out << QString(fmt)
2479 .arg(_ISC_B1Cd, 19, 'e', 12)
2480 .arg("", 19, QChar(' '))
2481 .arg(_TGD_B1Cp, 19, 'e', 12)
2482 .arg(_TGD_B2ap, 19, 'e', 12);
2483 }
2484 else if (navType() == t_eph::CNV2) {
2485 out << QString(fmt)
2486 .arg("", 19, QChar(' '))
2487 .arg(_ISC_B2ad, 19, 'e', 12)
2488 .arg(_TGD_B1Cp, 19, 'e', 12)
2489 .arg(_TGD_B2ap, 19, 'e', 12);
2490 }
2491 else if (navType() == t_eph::CNV3) {
2492 out << QString(fmt)
2493 .arg(_SISMAI, 19, 'e', 12)
2494 .arg(double(_health), 19, 'e', 12)
2495 .arg(_INTEGRITYF_B2b, 19, 'e', 12)
2496 .arg(_TGD_B2bI, 19, 'e', 12);
2497 }
2498 else { // D1, D2, undefined
2499 double tots = 0.0;
2500 if (_receptDateTime.isValid()) {// RTCM stream input
2501 tots = _TOE.bdssec();
2502 }
2503 else { // RINEX input
2504 tots = _TOT;
2505 }
2506 out << QString(fmt)
2507 .arg(tots, 19, 'e', 12)
2508 .arg(double(_AODC), 19, 'e', 12)
2509 .arg("", 19, QChar(' '))
2510 .arg("", 19, QChar(' '));
2511 }
2512
2513 // =====================
2514 // BROADCAST ORBIT - 8
2515 // =====================
2516 if (navType() == t_eph::CNV1) {
2517 out << QString(fmt)
2518 .arg(_SISMAI, 19, 'e', 12)
2519 .arg(double(_health), 19, 'e', 12)
2520 .arg(_INTEGRITYF_B1C, 19, 'e', 12)
2521 .arg(_IODC, 19, 'e', 12);
2522 }
2523 else if (navType() == t_eph::CNV2) {
2524 out << QString(fmt)
2525 .arg(_SISMAI, 19, 'e', 12)
2526 .arg(double(_health), 19, 'e', 12)
2527 .arg(_INTEGRITYF_B2aB1C, 19, 'e', 12)
2528 .arg(_IODC, 19, 'e', 12);
2529 }
2530 else if (navType() == t_eph::CNV3) {
2531 out << QString(fmt)
2532 .arg(_TOT, 19, 'e', 12)
2533 .arg("", 19, QChar(' '))
2534 .arg("", 19, QChar(' '))
2535 .arg("", 19, QChar(' '));
2536 }
2537
2538 // =====================
2539 // BROADCAST ORBIT - 9
2540 // =====================
2541 if (navType() == t_eph::CNV1 ||
2542 navType() == t_eph::CNV2) {
2543 out << QString(fmt)
2544 .arg(_TOT, 19, 'e', 12)
2545 .arg("", 19, QChar(' '))
2546 .arg("", 19, QChar(' '))
2547 .arg(_IODE, 19, 'e', 12);
2548 }
2549
2550
2551 return rnxStr;
2552}
Note: See TracBrowser for help on using the repository browser.