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

Last change on this file since 10409 was 10317, checked in by stuerze, 10 months ago

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