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

Last change on this file since 10542 was 10533, checked in by stuerze, 11 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.