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

Last change on this file since 10541 was 10533, checked in by stuerze, 2 months ago

Service and RTCM CRS encoding and decoding as well as Helmert parameter decoding added + some re-organisation
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#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 || _E5bHS || _E1_bHS) {
1514 return 1;
1515 }
1516 if (_SISA == 255.0) {
1517 return 1;
1518 }
1519 if (_e5aDataInValid || _e5bDataInValid || _e1DataInValid) {
1520 return 1;
1521 }
1522 return 0;
1523}
1524
1525// RINEX Format String
1526//////////////////////////////////////////////////////////////////////////////
1527QString t_ephGal::toString(double version) const {
1528
1529 QString navStr = navTypeString(_navType, _prn, version);
1530 QString rnxStr = navStr + rinexDateStr(_TOC, _prn, version);
1531
1532 QTextStream out(&rnxStr);
1533
1534 out << QString("%1%2%3\n")
1535 .arg(_clock_bias, 19, 'e', 12)
1536 .arg(_clock_drift, 19, 'e', 12)
1537 .arg(_clock_driftrate, 19, 'e', 12);
1538
1539 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1540 // =====================
1541 // BROADCAST ORBIT - 1
1542 // =====================
1543 out << QString(fmt)
1544 .arg(_IODnav, 19, 'e', 12)
1545 .arg(_Crs, 19, 'e', 12)
1546 .arg(_Delta_n, 19, 'e', 12)
1547 .arg(_M0, 19, 'e', 12);
1548 // =====================
1549 // BROADCAST ORBIT - 2
1550 // =====================
1551 out << QString(fmt)
1552 .arg(_Cuc, 19, 'e', 12)
1553 .arg(_e, 19, 'e', 12)
1554 .arg(_Cus, 19, 'e', 12)
1555 .arg(_sqrt_A, 19, 'e', 12);
1556 // =====================
1557 // BROADCAST ORBIT - 3
1558 // =====================
1559 out << QString(fmt)
1560 .arg(_TOEsec, 19, 'e', 12)
1561 .arg(_Cic, 19, 'e', 12)
1562 .arg(_OMEGA0, 19, 'e', 12)
1563 .arg(_Cis, 19, 'e', 12);
1564 // =====================
1565 // BROADCAST ORBIT - 4
1566 // =====================
1567 out << QString(fmt)
1568 .arg(_i0, 19, 'e', 12)
1569 .arg(_Crc, 19, 'e', 12)
1570 .arg(_omega, 19, 'e', 12)
1571 .arg(_OMEGADOT, 19, 'e', 12);
1572 // =====================
1573 // BROADCAST ORBIT - 5
1574 // =====================
1575 int dataSource = 0;
1576 int SVhealth = 0;
1577 double BGD_1_5A = _BGD_1_5A;
1578 double BGD_1_5B = _BGD_1_5B;
1579 if (_fnav) {
1580 dataSource |= (1<<1);
1581 dataSource |= (1<<8);
1582 BGD_1_5B = 0.0;
1583 // SVhealth
1584 // Bit 3 : E5a DVS
1585 if (_e5aDataInValid) {
1586 SVhealth |= (1<<3);
1587 }
1588 // Bit 4-5: E5a HS
1589 if (_E5aHS == 1.0) {
1590 SVhealth |= (1<<4);
1591 }
1592 else if (_E5aHS == 2.0) {
1593 SVhealth |= (1<<5);
1594 }
1595 else if (_E5aHS == 3.0) {
1596 SVhealth |= (1<<4);
1597 SVhealth |= (1<<5);
1598 }
1599 }
1600 else if(_inav) {
1601 // Bit 2 and 0 are set because from MT1046 the data source cannot be determined
1602 // and RNXv3.03 says both can be set if the navigation messages were merged
1603 dataSource |= (1<<0);
1604 dataSource |= (1<<2);
1605 dataSource |= (1<<9);
1606 // SVhealth
1607 // Bit 0 : E1-B DVS
1608 if (_e1DataInValid) {
1609 SVhealth |= (1<<0);
1610 }
1611 // Bit 1-2: E1-B HS
1612 if (_E1_bHS == 1.0) {
1613 SVhealth |= (1<<1);
1614 }
1615 else if (_E1_bHS == 2.0) {
1616 SVhealth |= (1<<2);
1617 }
1618 else if (_E1_bHS == 3.0) {
1619 SVhealth |= (1<<1);
1620 SVhealth |= (1<<2);
1621 }
1622 // Bit 3 : E5a DVS
1623 if (_e5aDataInValid) {
1624 SVhealth |= (1<<3);
1625 }
1626 // Bit 4-5: E5a HS
1627 if (_E5aHS == 1.0) {
1628 SVhealth |= (1<<4);
1629 }
1630 else if (_E5aHS == 2.0) {
1631 SVhealth |= (1<<5);
1632 }
1633 else if (_E5aHS == 3.0) {
1634 SVhealth |= (1<<4);
1635 SVhealth |= (1<<5);
1636 }
1637 // Bit 6 : E5b DVS
1638 if (_e5bDataInValid) {
1639 SVhealth |= (1<<6);
1640 }
1641 // Bit 7-8: E5b HS
1642 if (_E5bHS == 1.0) {
1643 SVhealth |= (1<<7);
1644 }
1645 else if (_E5bHS == 2.0) {
1646 SVhealth |= (1<<8);
1647 }
1648 else if (_E5bHS == 3.0) {
1649 SVhealth |= (1<<7);
1650 SVhealth |= (1<<8);
1651 }
1652 }
1653
1654 out << QString(fmt)
1655 .arg(_IDOT, 19, 'e', 12)
1656 .arg(double(dataSource), 19, 'e', 12)
1657 .arg(_TOEweek + 1024.0, 19, 'e', 12)
1658 .arg(0.0, 19, 'e', 12);
1659 // =====================
1660 // BROADCAST ORBIT - 6
1661 // =====================
1662 out << QString(fmt)
1663 .arg(_SISA, 19, 'e', 12)
1664 .arg(double(SVhealth), 19, 'e', 12)
1665 .arg(BGD_1_5A, 19, 'e', 12)
1666 .arg(BGD_1_5B, 19, 'e', 12);
1667 // =====================
1668 // BROADCAST ORBIT - 7
1669 // =====================
1670 double tot = _TOT;
1671 if (tot == 0.9999e9 && version < 3.0) {
1672 tot = 0.0;
1673 }
1674 out << QString(fmt)
1675 .arg(tot, 19, 'e', 12)
1676 .arg("", 19, QChar(' '))
1677 .arg("", 19, QChar(' '))
1678 .arg("", 19, QChar(' '));
1679
1680 return rnxStr;
1681}
1682
1683// Constructor
1684//////////////////////////////////////////////////////////////////////////////
1685t_ephSBAS::t_ephSBAS(double rnxVersion, const QStringList& lines) {
1686
1687 const int nLines = 4;
1688
1689 if (lines.size() != nLines) {
1690 _checkState = bad;
1691 return;
1692 }
1693
1694 // RINEX Format
1695 // ------------
1696 int fieldLen = 19;
1697
1698 int pos[4];
1699 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
1700 pos[1] = pos[0] + fieldLen;
1701 pos[2] = pos[1] + fieldLen;
1702 pos[3] = pos[2] + fieldLen;
1703
1704 // Read four lines
1705 // ---------------
1706 for (int iLine = 0; iLine < nLines; iLine++) {
1707 QString line = lines[iLine];
1708
1709 if ( iLine == 0 ) {
1710 QTextStream in(line.left(pos[1]).toLatin1());
1711
1712 int year, month, day, hour, min;
1713 double sec;
1714
1715 QString prnStr, n;
1716 in >> prnStr;
1717 if (prnStr.size() == 1 && prnStr[0] == 'S') {
1718 in >> n;
1719 prnStr.append(n);
1720 }
1721 in >> year >> month >> day >> hour >> min >> sec;
1722 if (prnStr.at(0) == 'S') {
1723 _prn.set('S', prnStr.mid(1).toInt());
1724 }
1725 else {
1726 _prn.set('S', prnStr.toInt());
1727 }
1728
1729 if (year < 80) {
1730 year += 2000;
1731 }
1732 else if (year < 100) {
1733 year += 1900;
1734 }
1735
1736 _TOC.set(year, month, day, hour, min, sec);
1737
1738 if ( readDbl(line, pos[1], fieldLen, _agf0 ) ||
1739 readDbl(line, pos[2], fieldLen, _agf1 ) ||
1740 readDbl(line, pos[3], fieldLen, _TOT ) ) {
1741 _checkState = bad;
1742 return;
1743 }
1744 }
1745 // =====================
1746 // BROADCAST ORBIT - 1
1747 // =====================
1748 else if ( iLine == 1 ) {
1749 if ( readDbl(line, pos[0], fieldLen, _x_pos ) ||
1750 readDbl(line, pos[1], fieldLen, _x_velocity ) ||
1751 readDbl(line, pos[2], fieldLen, _x_acceleration) ||
1752 readDbl(line, pos[3], fieldLen, _health ) ) {
1753 _checkState = bad;
1754 return;
1755 }
1756 }
1757 // =====================
1758 // BROADCAST ORBIT - 2
1759 // =====================
1760 else if ( iLine == 2 ) {
1761 if ( readDbl(line, pos[0], fieldLen, _y_pos ) ||
1762 readDbl(line, pos[1], fieldLen, _y_velocity ) ||
1763 readDbl(line, pos[2], fieldLen, _y_acceleration ) ||
1764 readDbl(line, pos[3], fieldLen, _ura ) ) {
1765 _checkState = bad;
1766 return;
1767 }
1768 }
1769 // =====================
1770 // BROADCAST ORBIT - 3
1771 // =====================
1772 else if ( iLine == 3 ) {
1773 double iodn;
1774 if ( readDbl(line, pos[0], fieldLen, _z_pos ) ||
1775 readDbl(line, pos[1], fieldLen, _z_velocity ) ||
1776 readDbl(line, pos[2], fieldLen, _z_acceleration) ||
1777 readDbl(line, pos[3], fieldLen, iodn ) ) {
1778 _checkState = bad;
1779 return;
1780 } else {
1781 _IODN = int(iodn);
1782 }
1783 }
1784 }
1785
1786 _x_pos *= 1.e3;
1787 _y_pos *= 1.e3;
1788 _z_pos *= 1.e3;
1789 _x_velocity *= 1.e3;
1790 _y_velocity *= 1.e3;
1791 _z_velocity *= 1.e3;
1792 _x_acceleration *= 1.e3;
1793 _y_acceleration *= 1.e3;
1794 _z_acceleration *= 1.e3;
1795}
1796
1797// IOD of SBAS Ephemeris (virtual)
1798////////////////////////////////////////////////////////////////////////////
1799
1800unsigned int t_ephSBAS::IOD() const {
1801 unsigned char buffer[80];
1802 int size = 0;
1803 int numbits = 0;
1804 long long bitbuffer = 0;
1805 unsigned char *startbuffer = buffer;
1806
1807 SBASADDBITSFLOAT(30, this->_x_pos, 0.08)
1808 SBASADDBITSFLOAT(30, this->_y_pos, 0.08)
1809 SBASADDBITSFLOAT(25, this->_z_pos, 0.4)
1810 SBASADDBITSFLOAT(17, this->_x_velocity, 0.000625)
1811 SBASADDBITSFLOAT(17, this->_y_velocity, 0.000625)
1812 SBASADDBITSFLOAT(18, this->_z_velocity, 0.004)
1813 SBASADDBITSFLOAT(10, this->_x_acceleration, 0.0000125)
1814 SBASADDBITSFLOAT(10, this->_y_acceleration, 0.0000125)
1815 SBASADDBITSFLOAT(10, this->_z_acceleration, 0.0000625)
1816 SBASADDBITSFLOAT(12, this->_agf0, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
1817 SBASADDBITSFLOAT(8, this->_agf1, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<10))
1818 SBASADDBITS(5,0); // the last byte is filled by 0-bits to obtain a length of an integer multiple of 8
1819
1820 return CRC24(size, startbuffer);
1821}
1822
1823// Compute SBAS Satellite Position (virtual)
1824////////////////////////////////////////////////////////////////////////////
1825t_irc t_ephSBAS::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
1826
1827 bncTime tt(GPSweek, GPSweeks);
1828 double dt = tt - _TOC;
1829
1830 xc[0] = _x_pos + _x_velocity * dt + _x_acceleration * dt * dt / 2.0;
1831 xc[1] = _y_pos + _y_velocity * dt + _y_acceleration * dt * dt / 2.0;
1832 xc[2] = _z_pos + _z_velocity * dt + _z_acceleration * dt * dt / 2.0;
1833
1834 vv[0] = _x_velocity + _x_acceleration * dt;
1835 vv[1] = _y_velocity + _y_acceleration * dt;
1836 vv[2] = _z_velocity + _z_acceleration * dt;
1837
1838 xc[3] = _agf0 + _agf1 * dt;
1839
1840 xc[4] = _agf1;
1841 xc[5] = 0.0;
1842
1843 return success;
1844}
1845
1846// Health status of SBAS Ephemeris (virtual)
1847////////////////////////////////////////////////////////////////////////////
1848unsigned int t_ephSBAS::isUnhealthy() const {
1849
1850 // Bit 5
1851 bool URAindexIs15 = (int(_health) & (1<<5));
1852 if (URAindexIs15) {
1853 // in this case it is recommended
1854 // to set the bits 0,1,2,3 to 1 (MT17health = 15)
1855 return 1;
1856 }
1857
1858 // Bit 0-3
1859 int MT17health = (int(_health)) & (0x0f);
1860 if (MT17health) {
1861 return 1;
1862 }
1863
1864 // Bit 4
1865 // bool MT17HealthIsUnavailable = (int(_health) & (1<<4));
1866 // is not used because the MT17health bits can be independently set if URA index is 15
1867
1868 return 0;
1869}
1870
1871
1872// RINEX Format String
1873//////////////////////////////////////////////////////////////////////////////
1874QString t_ephSBAS::toString(double version) const {
1875
1876 QString navStr = navTypeString(_navType, _prn, version);
1877 QString rnxStr = navStr + rinexDateStr(_TOC, _prn, version);
1878
1879 QTextStream out(&rnxStr);
1880
1881 out << QString("%1%2%3\n")
1882 .arg(_agf0, 19, 'e', 12)
1883 .arg(_agf1, 19, 'e', 12)
1884 .arg(_TOT, 19, 'e', 12);
1885
1886 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1887 // =====================
1888 // BROADCAST ORBIT - 1
1889 // =====================
1890 out << QString(fmt)
1891 .arg(1.e-3*_x_pos, 19, 'e', 12)
1892 .arg(1.e-3*_x_velocity, 19, 'e', 12)
1893 .arg(1.e-3*_x_acceleration, 19, 'e', 12)
1894 .arg(_health, 19, 'e', 12);
1895 // =====================
1896 // BROADCAST ORBIT - 2
1897 // =====================
1898 out << QString(fmt)
1899 .arg(1.e-3*_y_pos, 19, 'e', 12)
1900 .arg(1.e-3*_y_velocity, 19, 'e', 12)
1901 .arg(1.e-3*_y_acceleration, 19, 'e', 12)
1902 .arg(_ura, 19, 'e', 12);
1903 // =====================
1904 // BROADCAST ORBIT - 3
1905 // =====================
1906 out << QString(fmt)
1907 .arg(1.e-3*_z_pos, 19, 'e', 12)
1908 .arg(1.e-3*_z_velocity, 19, 'e', 12)
1909 .arg(1.e-3*_z_acceleration, 19, 'e', 12)
1910 .arg(double(_IODN), 19, 'e', 12);
1911
1912 return rnxStr;
1913}
1914
1915// Constructor
1916//////////////////////////////////////////////////////////////////////////////
1917t_ephBDS::t_ephBDS(double rnxVersion, const QStringList& lines) {
1918
1919 int nLines = 8;
1920
1921 if (navType() == t_eph::CNV1 ||
1922 navType() == t_eph::CNV2) {
1923 nLines += 2;
1924 }
1925 if (navType() == t_eph::CNV3) {
1926 nLines += 1;
1927 }
1928
1929 if (lines.size() != nLines) {
1930 _checkState = bad;
1931 return;
1932 }
1933
1934 // RINEX Format
1935 // ------------
1936 int fieldLen = 19;
1937
1938 int pos[4];
1939 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
1940 pos[1] = pos[0] + fieldLen;
1941 pos[2] = pos[1] + fieldLen;
1942 pos[3] = pos[2] + fieldLen;
1943
1944 // Read eight lines
1945 // ----------------
1946 for (int iLine = 0; iLine < nLines; iLine++) {
1947 QString line = lines[iLine];
1948
1949 if ( iLine == 0 ) {
1950 QTextStream in(line.left(pos[1]).toLatin1());
1951
1952 int year, month, day, hour, min;
1953 double sec;
1954
1955 QString prnStr, n;
1956 in >> prnStr;
1957 if (prnStr.size() == 1 && prnStr[0] == 'C') {
1958 in >> n;
1959 prnStr.append(n);
1960 }
1961 in >> year >> month >> day >> hour >> min >> sec;
1962 if (prnStr.at(0) == 'C') {
1963 _prn.set('C', prnStr.mid(1).toInt());
1964 }
1965 else {
1966 _prn.set('C', prnStr.toInt());
1967 }
1968
1969 if (year < 80) {
1970 year += 2000;
1971 }
1972 else if (year < 100) {
1973 year += 1900;
1974 }
1975
1976 _TOC.setBDS(year, month, day, hour, min, sec);
1977
1978 if ( readDbl(line, pos[1], fieldLen, _clock_bias ) ||
1979 readDbl(line, pos[2], fieldLen, _clock_drift ) ||
1980 readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
1981 _checkState = bad;
1982 return;
1983 }
1984 }
1985 // =====================
1986 // BROADCAST ORBIT - 1
1987 // =====================
1988 else if ( iLine == 1 ) {
1989 double aode;
1990 if ( readDbl(line, pos[0], fieldLen, aode ) ||
1991 readDbl(line, pos[1], fieldLen, _Crs ) ||
1992 readDbl(line, pos[2], fieldLen, _Delta_n) ||
1993 readDbl(line, pos[3], fieldLen, _M0 ) ) {
1994 _checkState = bad;
1995 return;
1996 }
1997 _AODE = int(aode);
1998 }
1999 // =====================
2000 // BROADCAST ORBIT - 2
2001 // =====================
2002 else if ( iLine == 2 ) {
2003 if ( readDbl(line, pos[0], fieldLen, _Cuc ) ||
2004 readDbl(line, pos[1], fieldLen, _e ) ||
2005 readDbl(line, pos[2], fieldLen, _Cus ) ||
2006 readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
2007 _checkState = bad;
2008 return;
2009 }
2010 }
2011 // =====================
2012 // BROADCAST ORBIT - 3
2013 // =====================
2014 else if ( iLine == 3 ) {
2015 if ( readDbl(line, pos[0], fieldLen, _TOEsec ) ||
2016 readDbl(line, pos[1], fieldLen, _Cic ) ||
2017 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
2018 readDbl(line, pos[3], fieldLen, _Cis ) ) {
2019 _checkState = bad;
2020 return;
2021 }
2022 }
2023 // =====================
2024 // BROADCAST ORBIT - 4
2025 // =====================
2026 else if ( iLine == 4 ) {
2027 if ( readDbl(line, pos[0], fieldLen, _i0 ) ||
2028 readDbl(line, pos[1], fieldLen, _Crc ) ||
2029 readDbl(line, pos[2], fieldLen, _omega ) ||
2030 readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
2031 _checkState = bad;
2032 return;
2033 }
2034 }
2035 // =====================
2036 // BROADCAST ORBIT - 5
2037 // =====================
2038 else if ( iLine == 5 ) {
2039
2040 if (navType() == t_eph::CNV1 ||
2041 navType() == t_eph::CNV2 ||
2042 navType() == t_eph::CNV3 ) {
2043 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
2044 readDbl(line, pos[1], fieldLen, _Delta_n_dot) ||
2045 readDbl(line, pos[2], fieldLen, _satType ) ||
2046 readDbl(line, pos[3], fieldLen, _top ) ) {
2047 _checkState = bad;
2048 return;
2049 }
2050 }
2051 else { // D1, D2, undefined
2052 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
2053 readDbl(line, pos[2], fieldLen, _BDTweek)) {
2054 _checkState = bad;
2055 return;
2056 }
2057 }
2058 }
2059 // =====================
2060 // BROADCAST ORBIT - 6
2061 // =====================
2062 else if ( iLine == 6 ) {
2063 if (navType() == t_eph::CNV1 ||
2064 navType() == t_eph::CNV2 ||
2065 navType() == t_eph::CNV3 ) {
2066 if ( readDbl(line, pos[0], fieldLen, _SISAI_oe ) ||
2067 readDbl(line, pos[1], fieldLen, _SISAI_ocb ) ||
2068 readDbl(line, pos[2], fieldLen, _SISAI_oc1 ) ||
2069 readDbl(line, pos[3], fieldLen, _SISAI_oc2 ) ) {
2070 _checkState = bad;
2071 return;
2072 }
2073 }
2074 else { // D1, D2, undefined
2075 double SatH1;
2076 if ( readDbl(line, pos[0], fieldLen, _URA ) ||
2077 readDbl(line, pos[1], fieldLen, SatH1) ||
2078 readDbl(line, pos[2], fieldLen, _TGD1) ||
2079 readDbl(line, pos[3], fieldLen, _TGD2) ) {
2080 _checkState = bad;
2081 return;
2082 }
2083 _SatH1 = int(SatH1);
2084 }
2085 }
2086 // =====================
2087 // BROADCAST ORBIT - 7
2088 // =====================
2089 else if ( iLine == 7 ) {
2090 if (navType() == t_eph::CNV1) {
2091 if ( readDbl(line, pos[0], fieldLen, _ISC_B1Cd) ||
2092 readDbl(line, pos[2], fieldLen, _TGD_B1Cp) ||
2093 readDbl(line, pos[3], fieldLen, _TGD_B2ap) ) {
2094 _checkState = bad;
2095 return;
2096 }
2097 }
2098 else if (navType() == t_eph::CNV2) {
2099 if ( readDbl(line, pos[1], fieldLen, _ISC_B2ad) ||
2100 readDbl(line, pos[2], fieldLen, _TGD_B1Cp) ||
2101 readDbl(line, pos[3], fieldLen, _TGD_B2ap) ) {
2102 _checkState = bad;
2103 return;
2104 }
2105 }
2106 else if (navType() == t_eph::CNV3) {
2107 double health;
2108 if ( readDbl(line, pos[0], fieldLen, _SISMAI ) ||
2109 readDbl(line, pos[1], fieldLen, health ) ||
2110 readDbl(line, pos[2], fieldLen, _INTEGRITYF_B2b) ||
2111 readDbl(line, pos[3], fieldLen, _TGD_B2bI) ) {
2112 _checkState = bad;
2113 return;
2114 }
2115 _health = int(health);
2116 }
2117 else { // D1, D2 or undefined
2118 double aodc;
2119 if ( readDbl(line, pos[0], fieldLen, _TOT) ||
2120 readDbl(line, pos[1], fieldLen, aodc) ) {
2121 _checkState = bad;
2122 return;
2123 }
2124 if (_TOT == 0.9999e9) { // 0.9999e9 means not known (RINEX standard)
2125 _TOT = _TOEsec;
2126 }
2127 _AODC = int(aodc);
2128 }
2129 }
2130 // =====================
2131 // BROADCAST ORBIT - 8
2132 // =====================
2133 else if ( iLine == 8 ) {
2134 double health;
2135 if (navType() == t_eph::CNV1) {
2136 if ( readDbl(line, pos[0], fieldLen, _SISMAI ) ||
2137 readDbl(line, pos[1], fieldLen, health ) ||
2138 readDbl(line, pos[2], fieldLen, _INTEGRITYF_B1C) ||
2139 readDbl(line, pos[3], fieldLen, _IODC) ) {
2140 _checkState = bad;
2141 return;
2142 }
2143 _health = int(health);
2144 }
2145 else if (navType() == t_eph::CNV2) {
2146 if ( readDbl(line, pos[0], fieldLen, _SISMAI ) ||
2147 readDbl(line, pos[1], fieldLen, health ) ||
2148 readDbl(line, pos[2], fieldLen, _INTEGRITYF_B2aB1C) ||
2149 readDbl(line, pos[3], fieldLen, _IODC) ) {
2150 _checkState = bad;
2151 return;
2152 }
2153 _health = int(health);
2154 }
2155 else if (navType() == t_eph::CNV3) {
2156 if ( readDbl(line, pos[0], fieldLen, _TOT)) {
2157 _checkState = bad;
2158 return;
2159 }
2160 }
2161
2162 }
2163 // =====================
2164 // BROADCAST ORBIT - 9
2165 // =====================
2166 else if ( iLine == 9 ) {
2167
2168 if (navType() == t_eph::CNV1 ||
2169 navType() == t_eph::CNV2) {
2170 if ( readDbl(line, pos[0], fieldLen, _TOT) ||
2171 readDbl(line, pos[3], fieldLen, _IODE) ) {
2172 _checkState = bad;
2173 return;
2174 }
2175 }
2176
2177 }
2178 }
2179
2180 _TOE.setBDS(int(_BDTweek), _TOEsec);
2181 // remark: actually should be computed from second_tot
2182 // but it seems to be unreliable in RINEX files
2183 //_TOT = _TOC.bdssec();
2184}
2185
2186// IOD of BDS Ephemeris (virtual)
2187////////////////////////////////////////////////////////////////////////////
2188unsigned int t_ephBDS::IOD() const {
2189 return (int(_TOC.gpssec())/720) % 240; //return (int(_TOEsec)/720) % 240;
2190}
2191
2192// Compute BDS Satellite Position (virtual)
2193//////////////////////////////////////////////////////////////////////////////
2194t_irc t_ephBDS::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
2195
2196 static const double gmBDS = 398.6004418e12;
2197 static const double omegaBDS = 7292115.0000e-11;
2198
2199 xc[0] = xc[1] = xc[2] = xc[3] = 0.0;
2200 vv[0] = vv[1] = vv[2] = 0.0;
2201
2202 bncTime tt(GPSweek, GPSweeks);
2203
2204 if (_sqrt_A == 0) {
2205 return failure;
2206 }
2207 double a0 = _sqrt_A * _sqrt_A;
2208
2209 double n0 = sqrt(gmBDS/(a0*a0*a0));
2210 double tk = tt - _TOE;
2211 double n = n0 + _Delta_n;
2212 double M = _M0 + n*tk;
2213 double E = M;
2214 double E_last;
2215 int nLoop = 0;
2216 do {
2217 E_last = E;
2218 E = M + _e*sin(E);
2219
2220 if (++nLoop == 100) {
2221 return failure;
2222 }
2223 } while ( fabs(E-E_last)*a0 > 0.001 );
2224
2225 double v = atan2(sqrt(1-_e*_e) * sin(E), cos(E) - _e);
2226 double u0 = v + _omega;
2227 double sin2u0 = sin(2*u0);
2228 double cos2u0 = cos(2*u0);
2229 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
2230 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
2231 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
2232 double xp = r*cos(u);
2233 double yp = r*sin(u);
2234 double toesec = (_TOE.gpssec() - 14.0);
2235 double sinom = 0;
2236 double cosom = 0;
2237 double sini = 0;
2238 double cosi = 0;
2239
2240 // Velocity
2241 // --------
2242 double tanv2 = tan(v/2);
2243 double dEdM = 1 / (1 - _e*cos(E));
2244 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2)
2245 / (1 + tanv2*tanv2) * dEdM * n;
2246 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
2247 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
2248 double dotr = a0 * _e*sin(E) * dEdM * n
2249 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
2250
2251 double dotx = dotr*cos(u) - r*sin(u)*dotu;
2252 double doty = dotr*sin(u) + r*cos(u)*dotu;
2253
2254 const double iMaxGEO = 10.0 / 180.0 * M_PI;
2255
2256 // MEO/IGSO satellite
2257 // ------------------
2258 if (_i0 > iMaxGEO) {
2259 double OM = _OMEGA0 + (_OMEGADOT - omegaBDS)*tk - omegaBDS*toesec;
2260
2261 sinom = sin(OM);
2262 cosom = cos(OM);
2263 sini = sin(i);
2264 cosi = cos(i);
2265
2266 xc[0] = xp*cosom - yp*cosi*sinom;
2267 xc[1] = xp*sinom + yp*cosi*cosom;
2268 xc[2] = yp*sini;
2269
2270 // Velocity
2271 // --------
2272
2273 double dotom = _OMEGADOT - t_CST::omega;
2274
2275 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
2276 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
2277 + yp*sini*sinom*doti; // dX / di
2278
2279 vv[1] = sinom *dotx + cosi*cosom *doty
2280 + xp*cosom*dotom - yp*cosi*sinom*dotom
2281 - yp*sini*cosom*doti;
2282
2283 vv[2] = sini *doty + yp*cosi *doti;
2284
2285 }
2286
2287 // GEO satellite
2288 // -------------
2289 else {
2290 double OM = _OMEGA0 + _OMEGADOT*tk - omegaBDS*toesec;
2291 double ll = omegaBDS*tk;
2292
2293 sinom = sin(OM);
2294 cosom = cos(OM);
2295 sini = sin(i);
2296 cosi = cos(i);
2297
2298 double xx = xp*cosom - yp*cosi*sinom;
2299 double yy = xp*sinom + yp*cosi*cosom;
2300 double zz = yp*sini;
2301
2302 Matrix RX = BNC_PPP::t_astro::rotX(-5.0 / 180.0 * M_PI);
2303 Matrix RZ = BNC_PPP::t_astro::rotZ(ll);
2304
2305 ColumnVector X1(3); X1 << xx << yy << zz;
2306 ColumnVector X2 = RZ*RX*X1;
2307
2308 xc[0] = X2(1);
2309 xc[1] = X2(2);
2310 xc[2] = X2(3);
2311
2312 double dotom = _OMEGADOT;
2313
2314 double vx = cosom *dotx - cosi*sinom *doty
2315 - xp*sinom*dotom - yp*cosi*cosom*dotom
2316 + yp*sini*sinom*doti;
2317
2318 double vy = sinom *dotx + cosi*cosom *doty
2319 + xp*cosom*dotom - yp*cosi*sinom*dotom
2320 - yp*sini*cosom*doti;
2321
2322 double vz = sini *doty + yp*cosi *doti;
2323
2324 ColumnVector V(3); V << vx << vy << vz;
2325
2326 Matrix RdotZ(3,3);
2327 double C = cos(ll);
2328 double S = sin(ll);
2329 Matrix UU(3,3);
2330 UU[0][0] = -S; UU[0][1] = +C; UU[0][2] = 0.0;
2331 UU[1][0] = -C; UU[1][1] = -S; UU[1][2] = 0.0;
2332 UU[2][0] = 0.0; UU[2][1] = 0.0; UU[2][2] = 0.0;
2333 RdotZ = omegaBDS * UU;
2334
2335 ColumnVector VV(3);
2336 VV = RZ*RX*V + RdotZ*RX*X1;
2337
2338 vv[0] = VV(1);
2339 vv[1] = VV(2);
2340 vv[2] = VV(3);
2341 }
2342
2343 double tc = tt - _TOC;
2344 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
2345
2346// dotC = _clock_drift + _clock_driftrate*tc
2347// - 4.442807309e-10*_e * sqrt(a0) * cos(E) * dEdM * n;
2348
2349 // Relativistic Correction
2350 // -----------------------
2351 xc[3] -= 4.442807309e-10 * _e * sqrt(a0) *sin(E);
2352
2353 xc[4] = _clock_drift + _clock_driftrate*tc;
2354 xc[5] = _clock_driftrate;
2355
2356 return success;
2357}
2358
2359
2360// Health status of SBAS Ephemeris (virtual)
2361////////////////////////////////////////////////////////////////////////////
2362unsigned int t_ephBDS::isUnhealthy() const {
2363
2364 if (navType() == t_eph::CNV1 ||
2365 navType() == t_eph::CNV2 ||
2366 navType() == t_eph::CNV3) {
2367 return static_cast<unsigned int>(_health);
2368 }
2369
2370 return static_cast<unsigned int>(_SatH1);
2371
2372}
2373
2374
2375// RINEX Format String
2376//////////////////////////////////////////////////////////////////////////////
2377QString t_ephBDS::toString(double version) const {
2378
2379 QString navStr = navTypeString(_navType, _prn, version);
2380 QString rnxStr = navStr + rinexDateStr(_TOC-14.0, _prn, version);
2381
2382 QTextStream out(&rnxStr);
2383
2384 out << QString("%1%2%3\n")
2385 .arg(_clock_bias, 19, 'e', 12)
2386 .arg(_clock_drift, 19, 'e', 12)
2387 .arg(_clock_driftrate, 19, 'e', 12);
2388
2389 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
2390 // =====================
2391 // BROADCAST ORBIT - 1
2392 // =====================
2393 out << QString(fmt)
2394 .arg(double(_AODE), 19, 'e', 12)
2395 .arg(_Crs, 19, 'e', 12)
2396 .arg(_Delta_n, 19, 'e', 12)
2397 .arg(_M0, 19, 'e', 12);
2398 // =====================
2399 // BROADCAST ORBIT - 2
2400 // =====================
2401 out << QString(fmt)
2402 .arg(_Cuc, 19, 'e', 12)
2403 .arg(_e, 19, 'e', 12)
2404 .arg(_Cus, 19, 'e', 12)
2405 .arg(_sqrt_A, 19, 'e', 12);
2406 // =====================
2407 // BROADCAST ORBIT - 3
2408 // =====================
2409 out << QString(fmt)
2410 .arg(_TOEsec, 19, 'e', 12)
2411 .arg(_Cic, 19, 'e', 12)
2412 .arg(_OMEGA0, 19, 'e', 12)
2413 .arg(_Cis, 19, 'e', 12);
2414 // =====================
2415 // BROADCAST ORBIT - 4
2416 // =====================
2417 out << QString(fmt)
2418 .arg(_i0, 19, 'e', 12)
2419 .arg(_Crc, 19, 'e', 12)
2420 .arg(_omega, 19, 'e', 12)
2421 .arg(_OMEGADOT, 19, 'e', 12);
2422 // =====================
2423 // BROADCAST ORBIT - 5
2424 // =====================
2425 if (navType() == t_eph::CNV1 ||
2426 navType() == t_eph::CNV2 ||
2427 navType() == t_eph::CNV3 ) {
2428 out << QString(fmt)
2429 .arg(_IDOT, 19, 'e', 12)
2430 .arg(_Delta_n_dot, 19, 'e', 12)
2431 .arg(_satType, 19, 'e', 12)
2432 .arg(_top, 19, 'e', 12);
2433 }
2434 else { // D1, D2, undefined
2435 out << QString(fmt)
2436 .arg(_IDOT, 19, 'e', 12)
2437 .arg("", 19, QChar(' '))
2438 .arg(_BDTweek, 19, 'e', 12)
2439 .arg("", 19, QChar(' '));
2440 }
2441 // =====================
2442 // BROADCAST ORBIT - 6
2443 // =====================
2444 if (navType() == t_eph::CNV1 ||
2445 navType() == t_eph::CNV2 ||
2446 navType() == t_eph::CNV3 ) {
2447 out << QString(fmt)
2448 .arg(_SISAI_oe, 19, 'e', 12)
2449 .arg(_SISAI_ocb, 19, 'e', 12)
2450 .arg(_SISAI_oc1, 19, 'e', 12)
2451 .arg(_SISAI_oc2, 19, 'e', 12);
2452 }
2453 else { // D1, D2, undefined
2454 out << QString(fmt)
2455 .arg(_URA, 19, 'e', 12)
2456 .arg(double(_SatH1), 19, 'e', 12)
2457 .arg(_TGD1, 19, 'e', 12)
2458 .arg(_TGD2, 19, 'e', 12);
2459 }
2460 // =====================
2461 // BROADCAST ORBIT - 7
2462 // =====================
2463 if (navType() == t_eph::CNV1) {
2464 out << QString(fmt)
2465 .arg(_ISC_B1Cd, 19, 'e', 12)
2466 .arg("", 19, QChar(' '))
2467 .arg(_TGD_B1Cp, 19, 'e', 12)
2468 .arg(_TGD_B2ap, 19, 'e', 12);
2469 }
2470 else if (navType() == t_eph::CNV2) {
2471 out << QString(fmt)
2472 .arg("", 19, QChar(' '))
2473 .arg(_ISC_B2ad, 19, 'e', 12)
2474 .arg(_TGD_B1Cp, 19, 'e', 12)
2475 .arg(_TGD_B2ap, 19, 'e', 12);
2476 }
2477 else if (navType() == t_eph::CNV3) {
2478 out << QString(fmt)
2479 .arg(_SISMAI, 19, 'e', 12)
2480 .arg(double(_health), 19, 'e', 12)
2481 .arg(_INTEGRITYF_B2b, 19, 'e', 12)
2482 .arg(_TGD_B2bI, 19, 'e', 12);
2483 }
2484 else { // D1, D2, undefined
2485 double tots = 0.0;
2486 if (_receptDateTime.isValid()) {// RTCM stream input
2487 tots = _TOE.bdssec();
2488 }
2489 else { // RINEX input
2490 tots = _TOT;
2491 }
2492 out << QString(fmt)
2493 .arg(tots, 19, 'e', 12)
2494 .arg(double(_AODC), 19, 'e', 12)
2495 .arg("", 19, QChar(' '))
2496 .arg("", 19, QChar(' '));
2497 }
2498
2499 // =====================
2500 // BROADCAST ORBIT - 8
2501 // =====================
2502 if (navType() == t_eph::CNV1) {
2503 out << QString(fmt)
2504 .arg(_SISMAI, 19, 'e', 12)
2505 .arg(double(_health), 19, 'e', 12)
2506 .arg(_INTEGRITYF_B1C, 19, 'e', 12)
2507 .arg(_IODC, 19, 'e', 12);
2508 }
2509 else if (navType() == t_eph::CNV2) {
2510 out << QString(fmt)
2511 .arg(_SISMAI, 19, 'e', 12)
2512 .arg(double(_health), 19, 'e', 12)
2513 .arg(_INTEGRITYF_B2aB1C, 19, 'e', 12)
2514 .arg(_IODC, 19, 'e', 12);
2515 }
2516 else if (navType() == t_eph::CNV3) {
2517 out << QString(fmt)
2518 .arg(_TOT, 19, 'e', 12)
2519 .arg("", 19, QChar(' '))
2520 .arg("", 19, QChar(' '))
2521 .arg("", 19, QChar(' '));
2522 }
2523
2524 // =====================
2525 // BROADCAST ORBIT - 9
2526 // =====================
2527 if (navType() == t_eph::CNV1 ||
2528 navType() == t_eph::CNV2) {
2529 out << QString(fmt)
2530 .arg(_TOT, 19, 'e', 12)
2531 .arg("", 19, QChar(' '))
2532 .arg("", 19, QChar(' '))
2533 .arg(_IODE, 19, 'e', 12);
2534 }
2535
2536
2537 return rnxStr;
2538}
Note: See TracBrowser for help on using the repository browser.