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

Last change on this file since 10573 was 10562, checked in by stuerze, 4 weeks ago

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