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

Last change on this file since 10904 was 10898, checked in by stuerze, 9 days ago

minor changes
File size: 88.0 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 _type = 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,
60 bool useCorr) const {
61
62 if (_checkState == bad ||
63 _checkState == unhealthy ||
64 _checkState == outdated) {
65 return failure;
66 }
67
68 xc.ReSize(6);
69 vv.ReSize(3);
70 if (position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data()) != success) {
71 return failure;
72 }
73 if (useCorr) {
74 if (_orbCorr && _clkCorr) {
75 double dtO = tt - _orbCorr->_time;
76 if (_orbCorr->_updateInt) {
77 dtO -= (0.5 * ssrUpdateInt[_orbCorr->_updateInt]);
78 }
79 ColumnVector dx(3);
80 dx[0] = _orbCorr->_xr[0] + _orbCorr->_dotXr[0] * dtO;
81 dx[1] = _orbCorr->_xr[1] + _orbCorr->_dotXr[1] * dtO;
82 dx[2] = _orbCorr->_xr[2] + _orbCorr->_dotXr[2] * dtO;
83
84 RSW_to_XYZ(xc.Rows(1, 3), vv.Rows(1, 3), dx, dx);
85
86 xc[0] -= dx[0];
87 xc[1] -= dx[1];
88 xc[2] -= dx[2];
89
90 ColumnVector dv(3);
91 RSW_to_XYZ(xc.Rows(1, 3), vv.Rows(1, 3), _orbCorr->_dotXr, dv);
92
93 vv[0] -= dv[0];
94 vv[1] -= dv[1];
95 vv[2] -= dv[2];
96
97 double dtC = tt - _clkCorr->_time;
98 if (_clkCorr->_updateInt) {
99 dtC -= (0.5 * ssrUpdateInt[_clkCorr->_updateInt]);
100 }
101 xc[3] += _clkCorr->_dClk + _clkCorr->_dotDClk * dtC
102 + _clkCorr->_dotDotDClk * dtC * dtC;
103 } else {
104 return failure;
105 }
106 }
107 return success;
108}
109
110//
111//////////////////////////////////////////////////////////////////////////////
112void t_eph::setType(QString typeStr) {
113
114 if (typeStr == "LNAV") {
115 _type = t_eph::LNAV;
116 } else if (typeStr == "FDMA") {
117 _type = t_eph::FDMA;
118 } else if (typeStr == "FNAV") {
119 _type = t_eph::FNAV;
120 } else if (typeStr == "INAV") {
121 _type = t_eph::INAV;
122 } else if (typeStr == "D1") {
123 _type = t_eph::D1;
124 } else if (typeStr == "D2") {
125 _type = t_eph::D2;
126 } else if (typeStr == "SBAS") {
127 _type = t_eph::SBASL1;
128 } else if (typeStr == "CNAV") {
129 _type = t_eph::CNAV;
130 } else if (typeStr == "CNV1") {
131 _type = t_eph::CNV1;
132 } else if (typeStr == "CNV2") {
133 _type = t_eph::CNV2;
134 } else if (typeStr == "CNV3") {
135 _type = t_eph::CNV3;
136 } else if (typeStr == "L1NV") {
137 _type = t_eph::L1NV;
138 } else if (typeStr == "L1OC") {
139 _type = t_eph::L1OC;
140 } else if (typeStr == "L3OC") {
141 _type = t_eph::L3OC;
142 } else {
143 _type = t_eph::undefined;
144 }
145
146}
147
148
149//
150//////////////////////////////////////////////////////////////////////////////
151QString t_eph::typeStr(e_type type, const t_prn &prn, double version) {
152 QString typeStr = "";
153 QString epochStart;
154 QString eolStr;
155
156 if (version < 4.0) {
157 return typeStr;
158 }
159
160 if (version == 99.0) { // log output for OUTDATED, WRONG or UNHEALTHY satellites
161 epochStart = "";
162 eolStr = "";
163 } else {
164 epochStart = "> ";
165 eolStr = "\n";
166 }
167
168 QString ephStr = QString("EPH %1 ").arg(prn.toString().c_str());
169 switch (type) {
170 case undefined:
171 typeStr = epochStart + ephStr + "unknown" + eolStr;
172 break;
173 case LNAV:
174 typeStr = epochStart + ephStr + "LNAV" + eolStr;
175 break;
176 case FDMA:
177 case FDMA_M:
178 typeStr = epochStart + ephStr + "FDMA" + eolStr;
179 break;
180 case FNAV:
181 typeStr = epochStart + ephStr + "FNAV" + eolStr;
182 break;
183 case INAV:
184 typeStr = epochStart + ephStr + "INAV" + eolStr;
185 break;
186 case D1:
187 typeStr = epochStart + ephStr + "D1 " + eolStr;
188 break;
189 case D2:
190 typeStr = epochStart + ephStr + "D2 " + eolStr;
191 break;
192 case SBASL1:
193 typeStr = epochStart + ephStr + "SBAS" + eolStr;
194 break;
195 case CNAV:
196 typeStr = epochStart + ephStr + "CNAV" + eolStr;
197 break;
198 case CNV1:
199 typeStr = epochStart + ephStr + "CNV1" + eolStr;
200 break;
201 case CNV2:
202 typeStr = epochStart + ephStr + "CNV2" + eolStr;
203 break;
204 case CNV3:
205 typeStr = epochStart + ephStr + "CNV3" + eolStr;
206 break;
207 case L1NV:
208 typeStr = epochStart + ephStr + "L1NV" + eolStr;
209 break;
210 case L1OC:
211 typeStr = epochStart + ephStr + "L1OC" + eolStr;
212 break;
213 case L3OC:
214 typeStr = epochStart + ephStr + "L3OC" + eolStr;
215 break;
216 }
217 return typeStr;
218}
219
220//
221//////////////////////////////////////////////////////////////////////////////
222QString t_eph::rinexDateStr(const bncTime &tt, const t_prn &prn,
223 double version) {
224 QString prnStr(prn.toString().c_str());
225 return rinexDateStr(tt, prnStr, version);
226}
227
228//
229//////////////////////////////////////////////////////////////////////////////
230QString t_eph::rinexDateStr(const bncTime &tt, const QString &prnStr,
231 double version) {
232
233 QString datStr;
234
235 unsigned year, month, day, hour, min;
236 double sec;
237 tt.civil_date(year, month, day);
238 tt.civil_time(hour, min, sec);
239
240 QTextStream out(&datStr);
241
242 if (version < 3.0) {
243 QString prnHlp = prnStr.mid(1, 2);
244 if (prnHlp[0] == '0')
245 prnHlp[0] = ' ';
246 out << prnHlp
247 << QString(" %1 %2 %3 %4 %5%6").arg(year % 100, 2, 10, QChar('0')).arg(
248 month, 2).arg(day, 2).arg(hour, 2).arg(min, 2).arg(sec, 5, 'f', 1);
249 }
250 else if (version == 99) {
251 out
252 << QString(" %1 %2 %3 %4 %5 %6").arg(year, 4).arg(month, 2, 10,
253 QChar('0')).arg(day, 2, 10, QChar('0')).arg(hour, 2, 10, QChar('0')).arg(
254 min, 2, 10, QChar('0')).arg(int(sec), 2, 10, QChar('0'));
255 }
256 else {
257 out << prnStr
258 << QString(" %1 %2 %3 %4 %5 %6").arg(year, 4).arg(month, 2, 10,
259 QChar('0')).arg(day, 2, 10, QChar('0')).arg(hour, 2, 10, QChar('0')).arg(
260 min, 2, 10, QChar('0')).arg(int(sec), 2, 10, QChar('0'));
261 }
262
263 return datStr;
264}
265
266// Constructor
267//////////////////////////////////////////////////////////////////////////////
268t_ephGPS::t_ephGPS(double rnxVersion, const QStringList &lines, QString typeStr) {
269
270 setType(typeStr);
271
272 int nLines = 8; // LNAV
273 // Source RINEX version < 4
274 if (type() == t_eph::undefined) {
275 _type = t_eph::LNAV;
276 }
277
278 if (type() == t_eph::CNAV ||
279 type() == t_eph::L1NV) {
280 nLines += 1;
281 }
282 if (type() == t_eph::CNV2) {
283 nLines += 2;
284 }
285
286 if (lines.size() != nLines) {
287 _checkState = bad;
288 return;
289 }
290
291 // RINEX Format
292 // ------------
293 int fieldLen = 19;
294 double statusflags = 0.0;
295
296 int pos[4];
297 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
298 pos[1] = pos[0] + fieldLen;
299 pos[2] = pos[1] + fieldLen;
300 pos[3] = pos[2] + fieldLen;
301
302 // Read nLines lines
303 // ------------------
304 for (int iLine = 0; iLine < nLines; iLine++) {
305 QString line = lines[iLine];
306
307 if (iLine == 0) {
308 QTextStream in(line.left(pos[1]).toLatin1());
309 int year, month, day, hour, min;
310 double sec;
311
312 QString prnStr, n;
313 in >> prnStr;
314
315 if (prnStr.size() == 1 &&
316 (prnStr[0] == 'G' ||
317 prnStr[0] == 'J' ||
318 prnStr[0] == 'I')) {
319 in >> n;
320 prnStr.append(n);
321 }
322
323 if ( prnStr.at(0) == 'G') {
324 _prn.set('G', prnStr.mid(1).toInt());
325 } else if (prnStr.at(0) == 'J') {
326 _prn.set('J', prnStr.mid(1).toInt());
327 } else if (prnStr.at(0) == 'I') {
328 _prn.set('I', prnStr.mid(1).toInt());
329 } else {
330 _prn.set('G', prnStr.toInt());
331 }
332 _prn.setFlag(type());
333
334 in >> year >> month >> day >> hour >> min >> sec;
335
336 if (year < 80) {
337 year += 2000;
338 } else if (year < 100) {
339 year += 1900;
340 }
341
342 _TOC.set(year, month, day, hour, min, sec);
343
344 if ( readDbl(line, pos[1], fieldLen, _clock_bias)
345 || readDbl(line, pos[2], fieldLen, _clock_drift)
346 || readDbl(line, pos[3], fieldLen, _clock_driftrate)) {
347 _checkState = bad;
348 return;
349 }
350 }
351 // =====================
352 // BROADCAST ORBIT - 1
353 // =====================
354 else if (iLine == 1) {
355 if (type() == t_eph::CNAV ||
356 type() == t_eph::CNV2 ||
357 type() == t_eph::L1NV) {
358 if ( readDbl(line, pos[0], fieldLen, _ADOT)
359 || readDbl(line, pos[1], fieldLen, _Crs)
360 || readDbl(line, pos[2], fieldLen, _Delta_n)
361 || readDbl(line, pos[3], fieldLen, _M0)) {
362 _checkState = bad;
363 return;
364 }
365 } else { // LNAV
366 if ( readDbl(line, pos[0], fieldLen, _IODE)
367 || readDbl(line, pos[1], fieldLen, _Crs)
368 || readDbl(line, pos[2], fieldLen, _Delta_n)
369 || readDbl(line, pos[3], fieldLen, _M0)) {
370 _checkState = bad;
371 return;
372 }
373 }
374 }
375 // =====================
376 // BROADCAST ORBIT - 2
377 // =====================
378 else if (iLine == 2) {
379 if ( readDbl(line, pos[0], fieldLen, _Cuc)
380 || readDbl(line, pos[1], fieldLen, _e)
381 || readDbl(line, pos[2], fieldLen, _Cus)
382 || readDbl(line, pos[3], fieldLen, _sqrt_A)) {
383 _checkState = bad;
384 return;
385 }
386 }
387 // =====================
388 // BROADCAST ORBIT - 3
389 // =====================
390 else if (iLine == 3) {
391 if (type() == t_eph::CNAV ||
392 type() == t_eph::CNV2) {
393 if ( readDbl(line, pos[0], fieldLen, _top)
394 || readDbl(line, pos[1], fieldLen, _Cic)
395 || readDbl(line, pos[2], fieldLen, _OMEGA0)
396 || readDbl(line, pos[3], fieldLen, _Cis)) {
397 _checkState = bad;
398 return;
399 }
400 } else if (type() == t_eph::L1NV) {
401 if ( readDbl(line, pos[0], fieldLen, _IODE)
402 || readDbl(line, pos[1], fieldLen, _Cic)
403 || readDbl(line, pos[2], fieldLen, _OMEGA0)
404 || readDbl(line, pos[3], fieldLen, _Cis)) {
405 _checkState = bad;
406 return;
407 }
408 } else { // LNAV
409 if ( readDbl(line, pos[0], fieldLen, _TOEsec)
410 || readDbl(line, pos[1], fieldLen, _Cic)
411 || readDbl(line, pos[2], fieldLen, _OMEGA0)
412 || readDbl(line, pos[3], fieldLen, _Cis)) {
413 _checkState = bad;
414 return;
415 }
416 }
417 }
418 // =====================
419 // BROADCAST ORBIT - 4
420 // =====================
421 else if (iLine == 4) {
422 if ( readDbl(line, pos[0], fieldLen, _i0)
423 || readDbl(line, pos[1], fieldLen, _Crc)
424 || readDbl(line, pos[2], fieldLen, _omega)
425 || readDbl(line, pos[3], fieldLen, _OMEGADOT)) {
426 _checkState = bad;
427 return;
428 }
429 }
430 // =====================
431 // BROADCAST ORBIT - 5
432 // =====================
433 else if (iLine == 5 && system() != t_eph::NavIC) {
434 if (type() == t_eph::CNAV ||
435 type() == t_eph::CNV2) {
436 if ( readDbl(line, pos[0], fieldLen, _IDOT)
437 || readDbl(line, pos[1], fieldLen, _Delta_n_dot)
438 || readDbl(line, pos[2], fieldLen, _URAI_NED0)
439 || readDbl(line, pos[3], fieldLen, _URAI_NED1)) {
440 _checkState = bad;
441 return;
442 }
443 }
444 else { // LNAV
445 if ( readDbl(line, pos[0], fieldLen, _IDOT)
446 || readDbl(line, pos[1], fieldLen, _L2Codes)
447 || readDbl(line, pos[2], fieldLen, _TOEweek)
448 || readDbl(line, pos[3], fieldLen, _L2PFlag)) {
449 _checkState = bad;
450 return;
451 }
452 }
453 } else if (iLine == 5 && system() == t_eph::NavIC) {
454 if (type() == t_eph::LNAV) {
455 if ( readDbl(line, pos[0], fieldLen, _IDOT)
456 || readDbl(line, pos[2], fieldLen, _TOEweek)) {
457 _checkState = bad;
458 return;
459 }
460 }
461 else if (type() == t_eph::L1NV) {
462 if ( readDbl(line, pos[0], fieldLen, _IDOT)
463 || readDbl(line, pos[1], fieldLen, _Delta_n_dot)
464 || readDbl(line, pos[3], fieldLen, _RSF)) {
465 _checkState = bad;
466 return;
467 }
468 }
469 }
470 // =====================
471 // BROADCAST ORBIT - 6
472 // =====================
473 else if (iLine == 6 && system() != t_eph::NavIC) {
474 if (type() == t_eph::CNAV ||
475 type() == t_eph::CNV2) {
476 if ( readDbl(line, pos[0], fieldLen, _URAI_ED)
477 || readDbl(line, pos[1], fieldLen, _health)
478 || readDbl(line, pos[2], fieldLen, _TGD)
479 || readDbl(line, pos[3], fieldLen, _URAI_NED2)) {
480 _checkState = bad;
481 return;
482 }
483 } else { // LNAV
484 if ( readDbl(line, pos[0], fieldLen, _ura)
485 || readDbl(line, pos[1], fieldLen, _health)
486 || readDbl(line, pos[2], fieldLen, _TGD)
487 || readDbl(line, pos[3], fieldLen, _IODC)) {
488 _checkState = bad;
489 return;
490 }
491 }
492 }
493 else if (iLine == 6 && system() == t_eph::NavIC) {
494 if (type() == t_eph::LNAV) {
495 if ( readDbl(line, pos[0], fieldLen, _ura)
496 || readDbl(line, pos[1], fieldLen, _health)
497 || readDbl(line, pos[2], fieldLen, _TGD)) {
498 _checkState = bad;
499 return;
500 }
501 }
502 else if (type() == t_eph::L1NV) {
503 int i = 0;
504 (!_RSF) ? i = 2 : i = 3;
505 if ( readDbl(line, pos[0], fieldLen, _URAI)
506 || readDbl(line, pos[1], fieldLen, _health)
507 || readDbl(line, pos[i], fieldLen, _TGD)) {
508 _checkState = bad;
509 return;
510 }
511 _ura = accuracyFromIndex(int(_URAI), system());
512 }
513 }
514 // =====================
515 // BROADCAST ORBIT - 7
516 // =====================
517 else if (iLine == 7) {
518 if (type() == t_eph::LNAV) {
519 if (readDbl(line, pos[0], fieldLen, _TOT)) {
520 _checkState = bad;
521 return;
522 }
523 if (system() != t_eph::NavIC) {
524 double fitIntervalRnx;
525 if (readDbl(line, pos[1], fieldLen, fitIntervalRnx)) {
526 //fit interval BLK, do nothing
527 _flags_unknown = true;
528 } else {
529 _flags_unknown = false;
530 if (system() == t_eph::GPS) { // in RINEX specified always as time period for GPS
531 _fitInterval = fitIntervalRnx;
532 }
533 else if (system() == t_eph::QZSS) { // specified as flag for QZSS
534 if (rnxVersion == 3.02) {
535 _fitInterval = fitIntervalRnx; // specified as time period
536 } else {
537 _fitInterval = fitIntervalFromFlag(fitIntervalRnx, _IODC, t_eph::QZSS);
538 }
539 }
540 }
541 }
542 }
543 else if (type() == t_eph::CNAV ||
544 type() == t_eph::CNV2) {
545 if ( readDbl(line, pos[0], fieldLen, _ISC_L1CA)
546 || readDbl(line, pos[1], fieldLen, _ISC_L2C)
547 || readDbl(line, pos[2], fieldLen, _ISC_L5I5)
548 || readDbl(line, pos[3], fieldLen, _ISC_L5Q5)) {
549 _checkState = bad;
550 return;
551 }
552 }
553 else if (type() == t_eph::L1NV) {
554 if (!_RSF) {
555 if ( readDbl(line, pos[0], fieldLen, _ISC_S)
556 || readDbl(line, pos[1], fieldLen, _ISC_L1D)) {
557 _checkState = bad;
558 return;
559 }
560 } else {
561 if ( readDbl(line, pos[2], fieldLen, _ISC_L1P)
562 || readDbl(line, pos[3], fieldLen, _ISC_L1D)) {
563 _checkState = bad;
564 return;
565 }
566 }
567 }
568 }
569 // =====================
570 // BROADCAST ORBIT - 8
571 // =====================
572 else if (iLine == 8) {
573 if (type() == t_eph::CNAV) {
574 if ( readDbl(line, pos[0], fieldLen, _TOT)
575 || readDbl(line, pos[1], fieldLen, _wnop)) {
576 _checkState = bad;
577 return;
578 }
579 if (readDbl(line, pos[2], fieldLen, statusflags)) {
580 _flags_unknown = true;
581 }
582 else {
583 _flags_unknown = false;
584 // Bit 0:
585 _intSF = double(bitExtracted(unsigned(statusflags), 1, 0));
586 // Bit 1:
587 _L2Cphasing = double(bitExtracted(unsigned(statusflags), 1, 1));
588 // Bit 2:
589 _alert = double(bitExtracted(unsigned(statusflags), 1, 2));
590 }
591 }
592 else if (type() == t_eph::CNV2) {
593 if ( readDbl(line, pos[0], fieldLen, _ISC_L1Cd)
594 || readDbl(line, pos[1], fieldLen, _ISC_L1Cp)) {
595 _checkState = bad;
596 return;
597 }
598 }
599 else if (type() == t_eph::L1NV) {
600 if ( readDbl(line, pos[0], fieldLen, _TOT)) {
601 _checkState = bad;
602 return;
603 }
604 }
605 }
606 // =====================
607 // BROADCAST ORBIT - 9
608 // =====================
609 else if (iLine == 9) {
610 if (type() == t_eph::CNV2) {
611 if ( readDbl(line, pos[0], fieldLen, _TOT)
612 || readDbl(line, pos[1], fieldLen, _wnop)) {
613 _checkState = bad;
614 return;
615 }
616 if (readDbl(line, pos[2], fieldLen, statusflags)) {
617 _flags_unknown = true;
618 }
619 else {
620 _flags_unknown = false;
621 // Bit 0:
622 _intSF = double(bitExtracted(unsigned(statusflags), 1, 0));
623 if (system() == t_eph::QZSS) {
624 // Bit 1:
625 _ephSF = double(bitExtracted(unsigned(statusflags), 1, 1));
626 }
627 }
628 }
629 }
630 }
631}
632
633// Compute GPS Satellite Position (virtual)
634////////////////////////////////////////////////////////////////////////////
635t_irc t_ephGPS::position(int GPSweek, double GPSweeks, double *xc,
636 double *vv) const {
637
638 static const double omegaEarth = 7292115.1467e-11;
639 static const double gmGRS = 398.6005e12;
640
641 memset(xc, 0, 6 * sizeof(double));
642 memset(vv, 0, 3 * sizeof(double));
643
644 double a0 = _sqrt_A * _sqrt_A;
645 if (a0 == 0) {
646 return failure;
647 }
648
649 double n0 = sqrt(gmGRS / (a0 * a0 * a0));
650
651 bncTime tt(GPSweek, GPSweeks);
652 double tk = tt - bncTime(int(_TOEweek), _TOEsec);
653
654 double n = n0 + _Delta_n;
655 double M = _M0 + n * tk;
656 double E = M;
657 double E_last;
658 int nLoop = 0;
659 do {
660 E_last = E;
661 E = M + _e * sin(E);
662
663 if (++nLoop == 100) {
664 return failure;
665 }
666 } while (fabs(E - E_last) * a0 > 0.001);
667 double v = 2.0 * atan(sqrt((1.0 + _e) / (1.0 - _e)) * tan(E / 2));
668 double u0 = v + _omega;
669 double sin2u0 = sin(2 * u0);
670 double cos2u0 = cos(2 * u0);
671 double r = a0 * (1 - _e * cos(E)) + _Crc * cos2u0 + _Crs * sin2u0;
672 double i = _i0 + _IDOT * tk + _Cic * cos2u0 + _Cis * sin2u0;
673 double u = u0 + _Cuc * cos2u0 + _Cus * sin2u0;
674 double xp = r * cos(u);
675 double yp = r * sin(u);
676 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth) * tk - omegaEarth * _TOEsec;
677
678 double sinom = sin(OM);
679 double cosom = cos(OM);
680 double sini = sin(i);
681 double cosi = cos(i);
682 xc[0] = xp * cosom - yp * cosi * sinom;
683 xc[1] = xp * sinom + yp * cosi * cosom;
684 xc[2] = yp * sini;
685
686 double tc = tt - _TOC;
687 xc[3] = _clock_bias + _clock_drift * tc + _clock_driftrate * tc * tc;
688
689 // Velocity
690 // --------
691 double tanv2 = tan(v / 2);
692 double dEdM = 1 / (1 - _e * cos(E));
693 double dotv = sqrt((1.0 + _e) / (1.0 - _e)) / cos(E / 2) / cos(E / 2)
694 / (1 + tanv2 * tanv2) * dEdM * n;
695 double dotu = dotv + (-_Cuc * sin2u0 + _Cus * cos2u0) * 2 * dotv;
696 double dotom = _OMEGADOT - omegaEarth;
697 double doti = _IDOT + (-_Cic * sin2u0 + _Cis * cos2u0) * 2 * dotv;
698 double dotr = a0 * _e * sin(E) * dEdM * n
699 + (-_Crc * sin2u0 + _Crs * cos2u0) * 2 * dotv;
700 double dotx = dotr * cos(u) - r * sin(u) * dotu;
701 double doty = dotr * sin(u) + r * cos(u) * dotu;
702
703 vv[0] = cosom * dotx - cosi * sinom * doty // dX / dr
704 - xp * sinom * dotom - yp * cosi * cosom * dotom // dX / dOMEGA
705 + yp * sini * sinom * doti; // dX / di
706
707 vv[1] = sinom * dotx + cosi * cosom * doty + xp * cosom * dotom
708 - yp * cosi * sinom * dotom - yp * sini * cosom * doti;
709
710 vv[2] = sini * doty + yp * cosi * doti;
711
712 // Relativistic Correction
713 // -----------------------
714 xc[3] -= 4.442807633e-10 * _e * sqrt(a0) * sin(E);
715
716 xc[4] = _clock_drift + _clock_driftrate * tc;
717 xc[5] = _clock_driftrate;
718
719 return success;
720}
721
722// RINEX Format String
723//////////////////////////////////////////////////////////////////////////////
724QString t_ephGPS::toString(double version) const {
725
726 if (version < 4.0 &&
727 (type() == t_eph::CNAV ||
728 type() == t_eph::CNV2 ||
729 type() == t_eph::L1NV )) {
730 return "";
731 }
732
733 QString ephStr = typeStr(_type, _prn, version);
734 QString rnxStr = ephStr + rinexDateStr(_TOC, _prn, version);
735
736 QTextStream out(&rnxStr);
737
738 out
739 << QString("%1%2%3\n")
740 .arg(_clock_bias, 19, 'e', 12)
741 .arg(_clock_drift, 19, 'e', 12)
742 .arg(_clock_driftrate, 19, 'e', 12);
743
744 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
745
746 // =====================
747 // BROADCAST ORBIT - 1
748 // =====================
749 if (type() == t_eph::CNAV ||
750 type() == t_eph::CNV2 ||
751 type() == t_eph::L1NV) {
752 out
753 << QString(fmt)
754 .arg(_ADOT, 19, 'e', 12)
755 .arg(_Crs, 19, 'e', 12)
756 .arg(_Delta_n, 19, 'e', 12)
757 .arg(_M0, 19, 'e', 12);
758 } else { // LNAV, undefined
759 out
760 << QString(fmt)
761 .arg(_IODE, 19, 'e', 12)
762 .arg(_Crs, 19, 'e', 12)
763 .arg(_Delta_n, 19, 'e', 12)
764 .arg(_M0, 19, 'e', 12);
765 }
766 // =====================
767 // BROADCAST ORBIT - 2
768 // =====================
769 out
770 << QString(fmt)
771 .arg(_Cuc, 19, 'e', 12)
772 .arg(_e, 19, 'e', 12)
773 .arg(_Cus, 19, 'e', 12)
774 .arg(_sqrt_A, 19, 'e', 12);
775 // =====================
776 // BROADCAST ORBIT - 3
777 // =====================
778 if (type() == t_eph::CNAV ||
779 type() == t_eph::CNV2) {
780 out
781 << QString(fmt)
782 .arg(_top, 19, 'e', 12)
783 .arg(_Cic, 19, 'e', 12)
784 .arg(_OMEGA0, 19, 'e', 12)
785 .arg(_Cis, 19, 'e', 12);
786 }
787 else if (type() == t_eph::L1NV) {
788 out
789 << QString(fmt)
790 .arg(_IODE, 19, 'e', 12)
791 .arg(_Cic, 19, 'e', 12)
792 .arg(_OMEGA0, 19, 'e', 12)
793 .arg(_Cis, 19, 'e', 12);
794 }
795 else { // LNAV, undefined
796 out
797 << QString(fmt)
798 .arg(_TOEsec, 19, 'e', 12)
799 .arg(_Cic, 19, 'e', 12)
800 .arg(_OMEGA0, 19, 'e', 12)
801 .arg(_Cis, 19, 'e', 12);
802 }
803 // =====================
804 // BROADCAST ORBIT - 4
805 // =====================
806 out
807 << QString(fmt)
808 .arg(_i0, 19, 'e', 12)
809 .arg(_Crc, 19, 'e', 12)
810 .arg(_omega, 19, 'e', 12)
811 .arg(_OMEGADOT, 19, 'e', 12);
812 // =====================
813 // BROADCAST ORBIT - 5
814 // =====================
815 if (system() != t_eph::NavIC) {
816 if (type() == t_eph::CNAV ||
817 type() == t_eph::CNV2) {
818 out
819 << QString(fmt)
820 .arg(_IDOT, 19, 'e', 12)
821 .arg(_Delta_n_dot, 19, 'e', 12)
822 .arg(_URAI_NED0, 19, 'e', 12)
823 .arg(_URAI_NED1, 19, 'e', 12);
824 }
825 else { // LNAV, undefined
826 out
827 << QString(fmt)
828 .arg(_IDOT, 19, 'e', 12)
829 .arg(_L2Codes, 19, 'e', 12)
830 .arg(_TOEweek, 19, 'e', 12)
831 .arg(_L2PFlag, 19, 'e', 12);
832 }
833 }
834 else {
835 if (type() == t_eph::LNAV ||
836 type() == t_eph::undefined) {
837 out
838 << QString(fmt)
839 .arg(_IDOT, 19, 'e', 12)
840 .arg("", 19, QChar(' '))
841 .arg(_TOEweek, 19, 'e', 12)
842 .arg("", 19, QChar(' '));
843 }
844 else if (type() == t_eph::L1NV) {
845 out
846 << QString(fmt)
847 .arg(_IDOT, 19, 'e', 12)
848 .arg(_Delta_n_dot, 19, 'e', 12)
849 .arg("", 19, QChar(' '))
850 .arg(_RSF, 19, 'e', 12);
851 }
852 }
853 // =====================
854 // BROADCAST ORBIT - 6
855 // =====================
856 if (system() != t_eph::NavIC) {
857 if (type() == t_eph::CNAV ||
858 type() == t_eph::CNV2) {
859 out
860 << QString(fmt)
861 .arg(_URAI_ED, 19, 'e', 12)
862 .arg(_health, 19, 'e', 12)
863 .arg(_TGD, 19, 'e', 12)
864 .arg(_URAI_NED2, 19, 'e', 12);
865 }
866 else { // LNAV, undefined
867 out
868 << QString(fmt)
869 .arg(_ura, 19, 'e', 12)
870 .arg(_health, 19, 'e', 12)
871 .arg(_TGD, 19, 'e', 12)
872 .arg(_IODC, 19, 'e', 12);
873 }
874 }
875 else {
876 if (type() == t_eph::LNAV ||
877 type() == t_eph::undefined) {
878 out
879 << QString(fmt)
880 .arg(_ura, 19, 'e', 12)
881 .arg(_health, 19, 'e', 12)
882 .arg(_TGD, 19, 'e', 12)
883 .arg("", 19, QChar(' '));
884 }
885 else if (type() == t_eph::L1NV) {
886 int i = 0; (!_RSF) ? i = 2 : i = 3;
887 if (i == 2) {
888 out
889 << QString(fmt)
890 .arg(_URAI, 19, 'e', 12)
891 .arg(_health, 19, 'e', 12)
892 .arg(_TGD, 19, 'e', 12)
893 .arg("", 19, QChar(' '));
894 }
895 else {
896 out
897 << QString(fmt)
898 .arg(_URAI, 19, 'e', 12)
899 .arg(_health, 19, 'e', 12)
900 .arg("", 19, QChar(' '))
901 .arg(_TGD, 19, 'e', 12);
902 }
903 }
904 }
905 // =====================
906 // BROADCAST ORBIT - 7
907 // =====================
908 if (type() == t_eph::LNAV ||
909 type() == t_eph::undefined) {
910
911 double tot = _TOT;
912 if (tot == 0.9999e9 && version < 3.0) {
913 tot = 0.0;
914 }
915 // fitInterval
916 if (system() == t_eph::NavIC) {
917 out
918 << QString(fmt)
919 .arg(tot, 19, 'e', 12)
920 .arg("", 19, QChar(' '))
921 .arg("", 19, QChar(' '))
922 .arg("", 19, QChar(' '));
923 }
924 else {
925 if (_flags_unknown) {
926 out
927 << QString(fmt)
928 .arg(tot, 19, 'e', 12)
929 .arg("", 19, QChar(' '))
930 .arg("", 19, QChar(' '))
931 .arg("", 19, QChar(' '));
932 }
933 else {
934 // for GPS and QZSS in version 3.02 specified in hours
935 double fitIntervalRnx = _fitInterval;
936 // otherwise specified as flag
937 if (system() == t_eph::QZSS && version != 3.02) {
938 (_fitInterval == 2.0) ? fitIntervalRnx = 0.0 : fitIntervalRnx = 1.0;
939 }
940 out
941 << QString(fmt)
942 .arg(tot, 19, 'e', 12)
943 .arg(fitIntervalRnx, 19, 'e', 12)
944 .arg("", 19, QChar(' '))
945 .arg("", 19, QChar(' '));
946 }
947 }
948 }
949 else if (type() == t_eph::CNAV ||
950 type() == t_eph::CNV2) {
951 out
952 << QString(fmt)
953 .arg(_ISC_L1CA, 19, 'e', 12)
954 .arg(_ISC_L2C, 19, 'e', 12)
955 .arg(_ISC_L5I5, 19, 'e', 12)
956 .arg(_ISC_L5Q5, 19, 'e', 12);
957 }
958 else if (type() == t_eph::L1NV) {
959 if (_RSF) {
960 out
961 << QString(fmt)
962 .arg(_ISC_S, 19, 'e', 12)
963 .arg(_ISC_L1D, 19, 'e', 12)
964 .arg("", 19, QChar(' '))
965 .arg("", 19, QChar(' '));
966 }
967 else {
968 out
969 << QString(fmt)
970 .arg("", 19, QChar(' '))
971 .arg("", 19, QChar(' '))
972 .arg(_ISC_L1P, 19, 'e', 12)
973 .arg(_ISC_L1D, 19, 'e', 12);
974 }
975 }
976 // =====================
977 // BROADCAST ORBIT - 8
978 // =====================
979 if (type() == t_eph::CNAV) {
980 int intFlags = 0;
981 if (!_flags_unknown) {
982 // Bit 0:
983 if (_intSF) {intFlags |= (1 << 0);}
984 // Bit 1:
985 if (_L2Cphasing) {intFlags |= (1 << 1);}
986 // Bit 2:
987 if (_alert) {intFlags |= (1 << 2);}
988 out
989 << QString(fmt)
990 .arg(_TOT, 19, 'e', 12)
991 .arg(_wnop, 19, 'e', 12)
992 .arg(double(intFlags), 19, 'e', 12)
993 .arg("", 19, QChar(' '));
994 }
995 else {
996 out
997 << QString(fmt)
998 .arg(_TOT, 19, 'e', 12)
999 .arg(_wnop, 19, 'e', 12)
1000 .arg("", 19, QChar(' '))
1001 .arg("", 19, QChar(' '));
1002 }
1003 }
1004 else if (type() == t_eph::CNV2) {
1005 out
1006 << QString(fmt)
1007 .arg(_ISC_L1Cd, 19, 'e', 12)
1008 .arg(_ISC_L1Cp, 19, 'e', 12)
1009 .arg("", 19, QChar(' '))
1010 .arg("", 19, QChar(' '));
1011 }
1012 else if (type() == t_eph::L1NV) {
1013 out
1014 << QString(fmt)
1015 .arg(_TOT, 19, 'e', 12)
1016 .arg("", 19, QChar(' '))
1017 .arg("", 19, QChar(' '))
1018 .arg("", 19, QChar(' '));
1019 }
1020 // =====================
1021 // BROADCAST ORBIT - 9
1022 // =====================
1023 if (type() == t_eph::CNV2) {
1024 int intFlags = 0;
1025 if (!_flags_unknown) {
1026 // Bit 0:
1027 if (_intSF) {intFlags |= (1 << 0);}
1028 if (system() == t_eph::QZSS) {
1029 // Bit 1:
1030 if (_ephSF) {intFlags |= (1 << 1);}
1031 }
1032 out
1033 << QString(fmt)
1034 .arg(_TOT, 19, 'e', 12)
1035 .arg(_wnop, 19, 'e', 12)
1036 .arg(double(intFlags), 19, 'e', 12)
1037 .arg("", 19, QChar(' '));
1038 }
1039 else {
1040 out
1041 << QString(fmt)
1042 .arg(_TOT, 19, 'e', 12)
1043 .arg(_wnop, 19, 'e', 12)
1044 .arg("", 19, QChar(' '))
1045 .arg("", 19, QChar(' '));
1046 }
1047 }
1048 return rnxStr;
1049}
1050
1051// Health status of GPS Ephemeris (virtual)
1052////////////////////////////////////////////////////////////////////////////
1053unsigned int t_ephGPS::isUnhealthy() const {
1054
1055 switch (system()) {
1056 case t_eph::GPS:
1057 case t_eph::QZSS:
1058 switch (type()) {
1059 case t_eph::LNAV:
1060 case t_eph::CNAV:
1061 case t_eph::CNV2:
1062 if (_health == 0.0) {
1063 return 0;
1064 }
1065 else {
1066 return 1;
1067 }
1068 break;
1069 }
1070 break;
1071 case t_eph::NavIC:
1072 switch (type()) {
1073 case t_eph::LNAV:
1074 if (_health == 0.0) { // L1 & S healthy
1075 return 0;
1076 }
1077 if (_health == 1.0 || // L5 healthy and S unhealthy,
1078 _health == 2.0 || // L5 unhealthy and S healthy
1079 _health == 3.0) { // both L5 and S unhealthy
1080 return 1;
1081 }
1082 break;
1083 case t_eph::L1NV:
1084 if (_health == 0.0) { // All navigation data on L1-SPS signal OK
1085 return 0;
1086 }
1087 if (_health == 1.0) { // Some or all navigation data on L1-SPS signal bad
1088 return 1;
1089 }
1090 break;
1091 }
1092 break;
1093 }
1094 return 0;
1095}
1096
1097// Constructor
1098//////////////////////////////////////////////////////////////////////////////
1099t_ephGlo::t_ephGlo(double rnxVersion, const QStringList &lines, const QString typeStr) {
1100
1101 setType(typeStr);
1102
1103 int nLines = 4;
1104
1105 // Source RINEX version < 4
1106 if (type() == t_eph::undefined) {
1107 // cannot be determined from input data
1108 // but is set to be able to work with old RINEX files in BNC applications
1109 _type = t_eph::FDMA_M;
1110 }
1111
1112 if (rnxVersion >= 3.05) {
1113 nLines += 1;
1114 } else {
1115 _M_delta_tau = 0.9999e9; // unknown
1116 _M_FT = 1.5e1; // unknown
1117 _statusflags_unknown = true;
1118 _healthflags_unknown = true;
1119 }
1120
1121 if (lines.size() != nLines) {
1122 _checkState = bad;
1123 return;
1124 }
1125
1126 // RINEX Format
1127 // ------------
1128 int fieldLen = 19;
1129 double statusflags = 0.0;
1130 double healthflags = 0.0;
1131 double sourceflags = 0.0;
1132 _tauC = 0.0;
1133 _tau1 = 0.0;
1134 _tau2 = 0.0;
1135 _additional_data_availability = 0.0;
1136
1137 int pos[4];
1138 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
1139 pos[1] = pos[0] + fieldLen;
1140 pos[2] = pos[1] + fieldLen;
1141 pos[3] = pos[2] + fieldLen;
1142
1143 // Read four lines
1144 // ---------------
1145 for (int iLine = 0; iLine < nLines; iLine++) {
1146 QString line = lines[iLine];
1147
1148 if (iLine == 0) {
1149 QTextStream in(line.left(pos[1]).toLatin1());
1150
1151 int year, month, day, hour, min;
1152 double sec;
1153
1154 QString prnStr, n;
1155 in >> prnStr;
1156 if (prnStr.size() == 1 && prnStr[0] == 'R') {
1157 in >> n;
1158 prnStr.append(n);
1159 }
1160
1161 if (prnStr.at(0) == 'R') {
1162 _prn.set('R', prnStr.mid(1).toInt());
1163 } else {
1164 _prn.set('R', prnStr.toInt());
1165 }
1166
1167 in >> year >> month >> day >> hour >> min >> sec;
1168 if (year < 80) {
1169 year += 2000;
1170 } else if (year < 100) {
1171 year += 1900;
1172 }
1173
1174 _gps_utc = gnumleap(year, month, day);
1175
1176 _TOC.set(year, month, day, hour, min, sec);
1177 _TOC = _TOC + _gps_utc;
1178 int nd = int((_TOC.gpssec())) / (24.0 * 60.0 * 60.0);
1179 if ( readDbl(line, pos[1], fieldLen, _tau)
1180 || readDbl(line, pos[2], fieldLen, _gamma)
1181 || readDbl(line, pos[3], fieldLen, _tki)) {
1182 _checkState = bad;
1183 return;
1184 }
1185 _tki -= nd * 86400.0;
1186 _tau = -_tau;
1187 }
1188 // =====================
1189 // BROADCAST ORBIT - 1
1190 // =====================
1191 else if (iLine == 1) {
1192 if ( readDbl(line, pos[0], fieldLen, _x_pos)
1193 || readDbl(line, pos[1], fieldLen, _x_vel)
1194 || readDbl(line, pos[2], fieldLen, _x_acc)
1195 || readDbl(line, pos[3], fieldLen, _health)) {
1196 _checkState = bad;
1197 return;
1198 }
1199 }
1200 // =====================
1201 // BROADCAST ORBIT - 2
1202 // =====================
1203 else if (iLine == 2) {
1204 if (type() == t_eph::FDMA ||
1205 type() == t_eph::FDMA_M) {
1206 if ( readDbl(line, pos[0], fieldLen, _y_pos)
1207 || readDbl(line, pos[1], fieldLen, _y_vel)
1208 || readDbl(line, pos[2], fieldLen, _y_acc)
1209 || readDbl(line, pos[3], fieldLen, _frq_num)) {
1210 _checkState = bad;
1211 return;
1212 }
1213 }
1214 else { //L1OC, L3OC
1215 if ( readDbl(line, pos[0], fieldLen, _y_pos)
1216 || readDbl(line, pos[1], fieldLen, _y_vel)
1217 || readDbl(line, pos[2], fieldLen, _y_acc)
1218 || readDbl(line, pos[3], fieldLen, statusflags)) {
1219 _checkState = bad;
1220 return;
1221 }
1222 _data_validity = int(statusflags);
1223 }
1224 }
1225 // =====================
1226 // BROADCAST ORBIT - 3
1227 // =====================
1228 else if (iLine == 3) {
1229 if (type() == t_eph::FDMA ||
1230 type() == t_eph::FDMA_M) {
1231 if ( readDbl(line, pos[0], fieldLen, _z_pos)
1232 || readDbl(line, pos[1], fieldLen, _z_vel)
1233 || readDbl(line, pos[2], fieldLen, _z_acc)
1234 || readDbl(line, pos[3], fieldLen, _E)) {
1235 _checkState = bad;
1236 return;
1237 }
1238 }
1239 else if (type() == t_eph::L1OC) {
1240 if ( readDbl(line, pos[0], fieldLen, _z_pos)
1241 || readDbl(line, pos[1], fieldLen, _z_vel)
1242 || readDbl(line, pos[2], fieldLen, _z_acc)
1243 || readDbl(line, pos[3], fieldLen, _TGD_L2OCp)) {
1244 _checkState = bad;
1245 return;
1246 }
1247 }
1248 else if (type() == t_eph::L3OC) {
1249 if ( readDbl(line, pos[0], fieldLen, _z_pos)
1250 || readDbl(line, pos[1], fieldLen, _z_vel)
1251 || readDbl(line, pos[2], fieldLen, _z_acc)
1252 || readDbl(line, pos[3], fieldLen, _TGD_L3OCp)) {
1253 _checkState = bad;
1254 return;
1255 }
1256 }
1257 }
1258 // =====================
1259 // BROADCAST ORBIT - 4
1260 // =====================
1261 else if (iLine == 4) {
1262 if (type() == t_eph::FDMA ||
1263 type() == t_eph::FDMA_M) {
1264 if (readDbl(line, pos[0], fieldLen, statusflags)) {
1265 //status flags BLK, do nothing
1266 _statusflags_unknown = true;
1267 } else {
1268 _statusflags_unknown = false;
1269 // status flags
1270 // ============
1271 // bit 0-1
1272 _M_P = double(bitExtracted(unsigned(statusflags), 2, 0));
1273 // bit 2-3
1274 _P1 = double(bitExtracted(unsigned(statusflags), 2, 2));
1275 // bit 4
1276 _P2 = double(bitExtracted(unsigned(statusflags), 1, 4));
1277 // bit 5
1278 _P3 = double(bitExtracted(unsigned(statusflags), 1, 5));
1279 // bit 6
1280 _M_P4 = double(bitExtracted(unsigned(statusflags), 1, 6));
1281 // bit 7-8
1282 _M_M = double(bitExtracted(unsigned(statusflags), 2, 7));
1283 /// GLO M/K exclusive flags/values only valid if flag M is set to '01'
1284 if (!_M_M) {
1285 _M_P = 0.0;
1286 _M_l3 = 0.0;
1287 _M_P4 = 0.0;
1288 _M_FE = 0.0;
1289 _M_FT = 0.0;
1290 _M_NA = 0.0;
1291 _M_NT = 0.0;
1292 _M_N4 = 0.0;
1293 _M_l5 = 0.0;
1294 _M_tau_GPS = 0.0;
1295 _M_delta_tau = 0.0;
1296 _type = t_eph::FDMA;
1297 }
1298 else {
1299 _type = t_eph::FDMA_M;
1300 }
1301 }
1302 if ( readDbl(line, pos[1], fieldLen, _M_delta_tau)
1303 || readDbl(line, pos[2], fieldLen, _M_FT)) {
1304 _checkState = bad;
1305 return;
1306 }
1307 if (readDbl(line, pos[3], fieldLen, healthflags)) {
1308 // health flags BLK
1309 _healthflags_unknown = true;
1310 } else {
1311 _healthflags_unknown = false;
1312 // health flags
1313 // ============
1314 // bit 0 (is to be ignored, if bit 1 is zero)
1315 _almanac_health = double(bitExtracted(unsigned(healthflags), 1, 0));
1316 // bit 1
1317 _almanac_health_availablility_indicator =
1318 double(bitExtracted(unsigned(healthflags), 1, 1));
1319 // bit 2; GLO-M/K only, health bit of string 3
1320 _M_l3 = double(bitExtracted(unsigned(healthflags), 1, 2));
1321 }
1322 }
1323 else if (type() == t_eph::L1OC ||
1324 type() == t_eph::L3OC) {
1325 if ( readDbl(line, pos[0], fieldLen, _sat_type)
1326 || readDbl(line, pos[1], fieldLen, sourceflags)
1327 || readDbl(line, pos[2], fieldLen, _EE)
1328 || readDbl(line, pos[3], fieldLen, _ET)) {
1329 _checkState = bad;
1330 return;
1331 }
1332 // sourceflags:
1333 // ============
1334 // bit 0-1
1335 _RT = double(bitExtracted(unsigned(sourceflags), 2, 0));
1336 // bit 2-3:
1337 _RE = double(bitExtracted(unsigned(sourceflags), 2, 2));
1338 }
1339 }
1340 // =====================
1341 // BROADCAST ORBIT - 5
1342 // =====================
1343 else if (iLine == 5) {
1344 if (type() == t_eph::L1OC ||
1345 type() == t_eph::L3OC) {
1346 if ( readDbl(line, pos[0], fieldLen, _attitude_P2)
1347 || readDbl(line, pos[1], fieldLen, _Tin)
1348 || readDbl(line, pos[2], fieldLen, _tau1)
1349 || readDbl(line, pos[3], fieldLen, _tau2)) {
1350 _checkState = bad;
1351 return;
1352 }
1353 }
1354 }
1355 // =====================
1356 // BROADCAST ORBIT - 6
1357 // =====================
1358 else if (iLine == 6) {
1359 if (type() == t_eph::L1OC ||
1360 type() == t_eph::L3OC) {
1361 if ( readDbl(line, pos[0], fieldLen, _yaw)
1362 || readDbl(line, pos[1], fieldLen, _sn)
1363 || readDbl(line, pos[2], fieldLen, _angular_rate)
1364 || readDbl(line, pos[3], fieldLen, _angular_acc)) {
1365 _checkState = bad;
1366 return;
1367 }
1368 }
1369 }
1370 // =====================
1371 // BROADCAST ORBIT - 7
1372 // =====================
1373 else if (iLine == 7) {
1374 if (type() == t_eph::L1OC ||
1375 type() == t_eph::L3OC) {
1376 if ( readDbl(line, pos[0], fieldLen, _angular_rate_max)
1377 || readDbl(line, pos[1], fieldLen, _X_PC)
1378 || readDbl(line, pos[2], fieldLen, _Y_PC)
1379 || readDbl(line, pos[3], fieldLen, _Z_PC)) {
1380 _checkState = bad;
1381 return;
1382 }
1383 }
1384 }
1385 // =====================
1386 // BROADCAST ORBIT - 8
1387 // =====================
1388 else if (iLine == 8) {
1389 if (type() == t_eph::L1OC ||
1390 type() == t_eph::L3OC) {
1391 if ( readDbl(line, pos[0], fieldLen, _M_FE)
1392 || readDbl(line, pos[1], fieldLen, _M_FT)
1393 || readDbl(line, pos[3], fieldLen, _TOT)) {
1394 _checkState = bad;
1395 return;
1396 }
1397 }
1398 }
1399 }
1400
1401 _prn.setFlag(type());
1402
1403 // Initialize status vector
1404 // ------------------------
1405 _tt = _TOC;
1406 _xv.ReSize(6);
1407 _xv = 0.0;
1408 _xv(1) = _x_pos * 1.e3;
1409 _xv(2) = _y_pos * 1.e3;
1410 _xv(3) = _z_pos * 1.e3;
1411 _xv(4) = _x_vel * 1.e3;
1412 _xv(5) = _y_vel * 1.e3;
1413 _xv(6) = _z_vel * 1.e3;
1414}
1415
1416// Compute Glonass Satellite Position (virtual)
1417////////////////////////////////////////////////////////////////////////////
1418t_irc t_ephGlo::position(int GPSweek, double GPSweeks, double *xc,
1419 double *vv) const {
1420
1421 static const double nominalStep = 10.0;
1422
1423 memset(xc, 0, 6 * sizeof(double));
1424 memset(vv, 0, 3 * sizeof(double));
1425
1426 double dtPos = bncTime(GPSweek, GPSweeks) - _tt;
1427
1428 if (fabs(dtPos) > 24 * 3600.0) {
1429 return failure;
1430 }
1431
1432 int nSteps = int(fabs(dtPos) / nominalStep) + 1;
1433 double step = dtPos / nSteps;
1434
1435 double acc[3];
1436 acc[0] = _x_acc * 1.e3;
1437 acc[1] = _y_acc * 1.e3;
1438 acc[2] = _z_acc * 1.e3;
1439
1440 for (int ii = 1; ii <= nSteps; ii++) {
1441 _xv = rungeKutta4(_tt.gpssec(), _xv, step, acc, glo_deriv);
1442 _tt = _tt + step;
1443 }
1444
1445 // Position and Velocity
1446 // ---------------------
1447 xc[0] = _xv(1);
1448 xc[1] = _xv(2);
1449 xc[2] = _xv(3);
1450
1451 vv[0] = _xv(4);
1452 vv[1] = _xv(5);
1453 vv[2] = _xv(6);
1454
1455 // Clock Correction
1456 // ----------------
1457 double dtClk = bncTime(GPSweek, GPSweeks) - _TOC;
1458 xc[3] = -_tau + _gamma * dtClk;
1459
1460 xc[4] = _gamma;
1461 xc[5] = 0.0;
1462
1463 return success;
1464}
1465
1466// RINEX Format String
1467//////////////////////////////////////////////////////////////////////////////
1468QString t_ephGlo::toString(double version) const {
1469
1470 if (version < 4.0 &&
1471 (type() == t_eph::L1OC ||
1472 type() == t_eph::L3OC )) {
1473 return "";
1474 }
1475
1476 QString ephStr = typeStr(_type, _prn, version);
1477 QString rnxStr = ephStr + rinexDateStr(_TOC - _gps_utc, _prn, version);
1478 int nd = int((_TOC - _gps_utc).gpssec()) / (24.0 * 60.0 * 60.0);
1479 QTextStream out(&rnxStr);
1480
1481 out
1482 << QString("%1%2%3\n")
1483 .arg(-_tau, 19, 'e', 12)
1484 .arg(_gamma, 19, 'e', 12)
1485 .arg(_tki + nd * 86400.0, 19, 'e', 12);
1486
1487 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1488 // =====================
1489 // BROADCAST ORBIT - 1
1490 // =====================
1491 out
1492 << QString(fmt)
1493 .arg(_x_pos, 19, 'e', 12)
1494 .arg(_x_vel, 19, 'e', 12)
1495 .arg(_x_acc, 19, 'e', 12)
1496 .arg(_health, 19, 'e', 12);
1497
1498 // =====================
1499 // BROADCAST ORBIT - 2
1500 // =====================
1501 if (type() == t_eph::FDMA ||
1502 type() == t_eph::FDMA_M) {
1503 out
1504 << QString(fmt)
1505 .arg(_y_pos, 19, 'e', 12)
1506 .arg(_y_vel, 19, 'e', 12)
1507 .arg(_y_acc, 19, 'e', 12)
1508 .arg(_frq_num, 19, 'e', 12);
1509 }
1510 else { //L1OC, L3OC
1511 out
1512 << QString(fmt)
1513 .arg(_y_pos, 19, 'e', 12)
1514 .arg(_y_vel, 19, 'e', 12)
1515 .arg(_y_acc, 19, 'e', 12)
1516 .arg(double(_data_validity), 19, 'e', 12);
1517 }
1518 // =====================
1519 // BROADCAST ORBIT - 3
1520 // =====================
1521 if (type() == t_eph::FDMA ||
1522 type() == t_eph::FDMA_M) {
1523 out
1524 << QString(fmt)
1525 .arg(_z_pos, 19, 'e', 12)
1526 .arg(_z_vel, 19, 'e', 12)
1527 .arg(_z_acc, 19, 'e', 12)
1528 .arg(_E, 19, 'e', 12);
1529 }
1530 else if (type() == t_eph::L1OC) {
1531 out
1532 << QString(fmt)
1533 .arg(_z_pos, 19, 'e', 12)
1534 .arg(_z_vel, 19, 'e', 12)
1535 .arg(_z_acc, 19, 'e', 12)
1536 .arg(_TGD_L2OCp, 19, 'e', 12);
1537 }
1538 else if (type() == t_eph::L3OC) {
1539 out
1540 << QString(fmt)
1541 .arg(_z_pos, 19, 'e', 12)
1542 .arg(_z_vel, 19, 'e', 12)
1543 .arg(_z_acc, 19, 'e', 12)
1544 .arg(_TGD_L3OCp, 19, 'e', 12);
1545 }
1546 if (version >= 3.05) {
1547 // =====================
1548 // BROADCAST ORBIT - 4
1549 // =====================
1550 if (type() == t_eph::FDMA ||
1551 type() == t_eph::FDMA_M){
1552 int statusflags = 0;
1553 int healthflags = 0;
1554 if (!_statusflags_unknown ) {
1555 // bit 0-1
1556 if (_M_P == 1.0) {statusflags |= (1 << 0);}
1557 else if (_M_P == 2.0) {statusflags |= (1 << 1);}
1558 else if (_M_P == 3.0) {statusflags |= (1 << 0); statusflags |= (1 << 1);}
1559 // bit 2-3
1560 if (_P1 == 1.0) {statusflags |= (1 << 2);}
1561 else if (_P1 == 2.0) {statusflags |= (1 << 3);}
1562 else if (_P1 == 3.0) {statusflags |= (1 << 2); statusflags |= (1 << 3);}
1563 // bit 4
1564 if (_P2) {statusflags |= (1 << 4);}
1565 // bit 5
1566 if (_P3) {statusflags |= (1 << 5);}
1567 // bit 6
1568 if (_M_P4) {statusflags |= (1 << 6);}
1569 // bit 7-8
1570 if (_M_M == 1.0) {statusflags |= (1 << 7);}
1571 }
1572 if (!_healthflags_unknown) {
1573 // bit 0 (is to be ignored, if bit 1 is zero)
1574 if (_almanac_health) {healthflags |= (1 << 0);}
1575 // bit 1
1576 if (_almanac_health_availablility_indicator) {healthflags |= (1 << 1);}
1577 // bit 2
1578 if (_M_l3) {healthflags |= (1 << 2);}
1579 }
1580
1581 if (_statusflags_unknown && _healthflags_unknown) {
1582 out
1583 << QString(fmt)
1584 .arg("", 19, QChar(' ')) // status-flags BNK (unknown)
1585 .arg(_M_delta_tau, 19, 'e', 12)
1586 .arg(_M_FT, 19, 'e', 12)
1587 .arg("", 19, QChar(' '));// health-flags BNK (unknown)
1588 }
1589 else if (!_statusflags_unknown && _healthflags_unknown) {
1590 out
1591 << QString(fmt)
1592 .arg(double(statusflags), 19, 'e', 12)
1593 .arg(_M_delta_tau, 19, 'e', 12)
1594 .arg(_M_FT, 19, 'e', 12)
1595 .arg("", 19, QChar(' '));// health-flags BNK (unknown)
1596 }
1597 else if (_statusflags_unknown && !_healthflags_unknown) {
1598 out
1599 << QString(fmt)
1600 .arg("", 19, QChar(' ')) // status-flags BNK (unknown)
1601 .arg(_M_delta_tau, 19, 'e', 12)
1602 .arg(_M_FT, 19, 'e', 12)
1603 .arg(double(healthflags), 19, 'e', 12);
1604 }
1605 else if (!_statusflags_unknown && !_healthflags_unknown) {
1606 out
1607 << QString(fmt)
1608 .arg(double(statusflags), 19, 'e', 12)
1609 .arg(_M_delta_tau, 19, 'e', 12)
1610 .arg(_M_FT, 19, 'e', 12)
1611 .arg(double(healthflags), 19, 'e', 12);
1612 }
1613 }
1614 else if (type() == t_eph::L1OC ||
1615 type() == t_eph::L3OC) {
1616 int sourceflags = 0;
1617 // bit 0-1
1618 if (_RT == 1.0) {sourceflags |= (1 << 0);}
1619 else if (_RT == 2.0) {sourceflags |= (1 << 1);}
1620 else if (_RT == 3.0) {sourceflags |= (1 << 0); sourceflags |= (1 << 1);}
1621 // bit 2-3
1622 if (_RE == 1.0) {sourceflags |= (1 << 2);}
1623 else if (_RE == 2.0) {sourceflags |= (1 << 3);}
1624 else if (_RE == 3.0) {sourceflags |= (1 << 2); sourceflags |= (1 << 3);}
1625 out
1626 << QString(fmt)
1627 .arg(_sat_type , 19, 'e', 12)
1628 .arg(double(sourceflags), 19, 'e', 12)
1629 .arg(_ET, 19, 'e', 12)
1630 .arg(_EE, 19, 'e', 12);
1631 }
1632 // =====================
1633 // BROADCAST ORBIT - 5
1634 // =====================
1635 if (type() == t_eph::L1OC ||
1636 type() == t_eph::L3OC) {
1637 out
1638 << QString(fmt)
1639 .arg(_attitude_P2, 19, 'e', 12)
1640 .arg(_Tin, 19, 'e', 12)
1641 .arg(_tau1, 19, 'e', 12)
1642 .arg(_tau2, 19, 'e', 12);
1643 }
1644 // =====================
1645 // BROADCAST ORBIT - 6
1646 // =====================
1647 if (type() == t_eph::L1OC ||
1648 type() == t_eph::L3OC) {
1649 out
1650 << QString(fmt)
1651 .arg(_yaw, 19, 'e', 12)
1652 .arg(_sn, 19, 'e', 12)
1653 .arg(_angular_rate, 19, 'e', 12)
1654 .arg(_angular_acc, 19, 'e', 12);
1655 }
1656 // =====================
1657 // BROADCAST ORBIT - 7
1658 // =====================
1659 if (type() == t_eph::L1OC ||
1660 type() == t_eph::L3OC) {
1661 out
1662 << QString(fmt)
1663 .arg(_angular_rate_max, 19, 'e', 12)
1664 .arg(_X_PC, 19, 'e', 12)
1665 .arg(_Y_PC, 19, 'e', 12)
1666 .arg(_Z_PC, 19, 'e', 12);
1667 }
1668 // =====================
1669 // BROADCAST ORBIT - 8
1670 // =====================
1671 if (type() == t_eph::L1OC ||
1672 type() == t_eph::L3OC) {
1673 out
1674 << QString(fmt)
1675 .arg(_M_FE, 19, 'e', 12)
1676 .arg(_M_FT, 19, 'e', 12)
1677 .arg("", 19, QChar(' '))
1678 .arg(_TOT, 19, 'e', 12);
1679 }
1680 }
1681 return rnxStr;
1682}
1683
1684// Derivative of the state vector using a simple force model (static)
1685////////////////////////////////////////////////////////////////////////////
1686ColumnVector t_ephGlo::glo_deriv(double /* tt */, const ColumnVector &xv,
1687 double *acc) {
1688
1689 // State vector components
1690 // -----------------------
1691 ColumnVector rr = xv.rows(1, 3);
1692 ColumnVector vv = xv.rows(4, 6);
1693
1694 // Acceleration
1695 // ------------
1696 static const double gmWGS = 398.60044e12;
1697 static const double AE = 6378136.0;
1698 static const double OMEGA = 7292115.e-11;
1699 static const double C20 = -1082.6257e-6;
1700
1701 double rho = rr.NormFrobenius();
1702 double t1 = -gmWGS / (rho * rho * rho);
1703 double t2 = 3.0 / 2.0 * C20 * (gmWGS * AE * AE)
1704 / (rho * rho * rho * rho * rho);
1705 double t3 = OMEGA * OMEGA;
1706 double t4 = 2.0 * OMEGA;
1707 double z2 = rr(3) * rr(3);
1708
1709 // Vector of derivatives
1710 // ---------------------
1711 ColumnVector va(6);
1712 va(1) = vv(1);
1713 va(2) = vv(2);
1714 va(3) = vv(3);
1715 va(4) = (t1 + t2 * (1.0 - 5.0 * z2 / (rho * rho)) + t3) * rr(1) + t4 * vv(2)
1716 + acc[0];
1717 va(5) = (t1 + t2 * (1.0 - 5.0 * z2 / (rho * rho)) + t3) * rr(2) - t4 * vv(1)
1718 + acc[1];
1719 va(6) = (t1 + t2 * (3.0 - 5.0 * z2 / (rho * rho))) * rr(3) + acc[2];
1720
1721 return va;
1722}
1723
1724// IOD of Glonass Ephemeris (virtual)
1725////////////////////////////////////////////////////////////////////////////
1726unsigned int t_ephGlo::IOD() const {
1727 bncTime tMoscow = _TOC - _gps_utc + 3 * 3600.0;
1728 return (unsigned long) tMoscow.daysec() / 900;
1729}
1730
1731// Health status of Glonass Ephemeris (virtual)
1732////////////////////////////////////////////////////////////////////////////
1733unsigned int t_ephGlo::isUnhealthy() const {
1734
1735 if (_almanac_health_availablility_indicator) {
1736 if ((_health == 0 && _almanac_health == 0)
1737 || (_health == 1 && _almanac_health == 0)
1738 || (_health == 1 && _almanac_health == 1)) {
1739 return 1;
1740 }
1741 } else if (!_almanac_health_availablility_indicator) {
1742 if (_health) {
1743 return 1;
1744 }
1745 }
1746 return 0; /* (_health == 0 && _almanac_health == 1) or (_health == 0) */
1747}
1748
1749// Constructor
1750//////////////////////////////////////////////////////////////////////////////
1751t_ephGal::t_ephGal(double rnxVersion, const QStringList &lines, const QString typeStr) {
1752
1753 setType(typeStr);
1754
1755 const int nLines = 8;
1756
1757 if (lines.size() != nLines) {
1758 _checkState = bad;
1759 return;
1760 }
1761
1762 // RINEX Format
1763 // ------------
1764 int fieldLen = 19;
1765 double SVhealth = 0.0;
1766 double datasource = 0.0;
1767
1768 int pos[4];
1769 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
1770 pos[1] = pos[0] + fieldLen;
1771 pos[2] = pos[1] + fieldLen;
1772 pos[3] = pos[2] + fieldLen;
1773
1774 // Read eight lines
1775 // ----------------
1776 for (int iLine = 0; iLine < nLines; iLine++) {
1777 QString line = lines[iLine];
1778
1779 if (iLine == 0) {
1780 QTextStream in(line.left(pos[1]).toLatin1());
1781
1782 int year, month, day, hour, min;
1783 double sec;
1784
1785 QString prnStr, n;
1786 in >> prnStr;
1787 if (prnStr.size() == 1 && prnStr[0] == 'E') {
1788 in >> n;
1789 prnStr.append(n);
1790 }
1791 if (prnStr.at(0) == 'E') {
1792 _prn.set('E', prnStr.mid(1).toInt());
1793 } else {
1794 _prn.set('E', prnStr.toInt());
1795 }
1796
1797 in >> year >> month >> day >> hour >> min >> sec;
1798 if (year < 80) {
1799 year += 2000;
1800 } else if (year < 100) {
1801 year += 1900;
1802 }
1803
1804 _TOC.set(year, month, day, hour, min, sec);
1805
1806 if ( readDbl(line, pos[1], fieldLen, _clock_bias)
1807 || readDbl(line, pos[2], fieldLen, _clock_drift)
1808 || readDbl(line, pos[3], fieldLen, _clock_driftrate)) {
1809 _checkState = bad;
1810 return;
1811 }
1812 }
1813 // =====================
1814 // BROADCAST ORBIT - 1
1815 // =====================
1816 else if (iLine == 1) {
1817 if ( readDbl(line, pos[0], fieldLen, _IODnav)
1818 || readDbl(line, pos[1], fieldLen, _Crs)
1819 || readDbl(line, pos[2], fieldLen, _Delta_n)
1820 || readDbl(line, pos[3], fieldLen, _M0)) {
1821 _checkState = bad;
1822 return;
1823 }
1824 }
1825 // =====================
1826 // BROADCAST ORBIT - 2
1827 // =====================
1828 else if (iLine == 2) {
1829 if ( readDbl(line, pos[0], fieldLen, _Cuc)
1830 || readDbl(line, pos[1], fieldLen, _e)
1831 || readDbl(line, pos[2], fieldLen, _Cus)
1832 || readDbl(line, pos[3], fieldLen, _sqrt_A)) {
1833 _checkState = bad;
1834 return;
1835 }
1836 }
1837 // =====================
1838 // BROADCAST ORBIT - 3
1839 // =====================
1840 else if (iLine == 3) {
1841 if ( readDbl(line, pos[0], fieldLen, _TOEsec)
1842 || readDbl(line, pos[1], fieldLen, _Cic)
1843 || readDbl(line, pos[2], fieldLen, _OMEGA0)
1844 || readDbl(line, pos[3], fieldLen, _Cis)) {
1845 _checkState = bad;
1846 return;
1847 }
1848 }
1849 // =====================
1850 // BROADCAST ORBIT - 4
1851 // =====================
1852 else if (iLine == 4) {
1853 if ( readDbl(line, pos[0], fieldLen, _i0)
1854 || readDbl(line, pos[1], fieldLen, _Crc)
1855 || readDbl(line, pos[2], fieldLen, _omega)
1856 || readDbl(line, pos[3], fieldLen, _OMEGADOT)) {
1857 _checkState = bad;
1858 return;
1859 }
1860 }
1861 // =====================
1862 // BROADCAST ORBIT - 5
1863 // =====================
1864 else if (iLine == 5) {
1865 if ( readDbl(line, pos[0], fieldLen, _IDOT)
1866 || readDbl(line, pos[1], fieldLen, datasource)
1867 || readDbl(line, pos[2], fieldLen, _TOEweek)) {
1868 _checkState = bad;
1869 return;
1870 }
1871 else {
1872 if (bitExtracted(unsigned(datasource), 1, 8)) {
1873 _fnav = true;
1874 _type = t_eph::FNAV;
1875 _inav = false;
1876 /* set unused I/NAV values */
1877 _E5b_HS = 0.0;
1878 _E1B_HS = 0.0;
1879 _E1B_DataInvalid = false;
1880 _E5b_DataInvalid = false;
1881 }
1882 if (bitExtracted(unsigned(datasource), 1, 9)) {
1883 _fnav = false;
1884 _inav = true;
1885 _type = t_eph::INAV;
1886 /* set unused F/NAV values */
1887 _E5a_HS = 0.0;
1888 _E5a_DataInvalid = false;
1889 }
1890 // GAL week # in RINEX is aligned/identical to continuous GPS week # used in RINEX
1891 // but GST week # started at the first GPS roll-over (continuous GPS week 1024)
1892 _TOEweek -= 1024.0;
1893 }
1894 }
1895 // =====================
1896 // BROADCAST ORBIT - 6
1897 // =====================
1898 else if (iLine == 6) {
1899 if ( readDbl(line, pos[0], fieldLen, _SISA)
1900 || readDbl(line, pos[1], fieldLen, SVhealth)
1901 || readDbl(line, pos[2], fieldLen, _BGD_1_5A)
1902 || readDbl(line, pos[3], fieldLen, _BGD_1_5B)) {
1903 _checkState = bad;
1904 return;
1905 } else {
1906 // Bit 0
1907 _E1B_DataInvalid = bitExtracted(unsigned(SVhealth), 1, 0);
1908 // Bit 1-2
1909 _E1B_HS = double(bitExtracted(unsigned(SVhealth), 2, 1));
1910 // Bit 3
1911 _E5a_DataInvalid = bitExtracted(unsigned(SVhealth), 1, 3);
1912 // Bit 4-5
1913 _E5a_HS = double(bitExtracted(unsigned(SVhealth), 2, 4));
1914 // Bit 6
1915 _E5b_DataInvalid = bitExtracted(unsigned(SVhealth), 1, 6);
1916 // Bit 7-8
1917 _E5b_HS = double(bitExtracted(unsigned(SVhealth), 2, 7));
1918 if (_fnav) {
1919 _BGD_1_5B = 0.0;
1920 }
1921 }
1922 }
1923 // =====================
1924 // BROADCAST ORBIT - 7
1925 // =====================
1926 else if (iLine == 7) {
1927 if (readDbl(line, pos[0], fieldLen, _TOT)) {
1928 _checkState = bad;
1929 return;
1930 }
1931 }
1932 }
1933 _prn.setFlag(type());
1934}
1935
1936// Compute Galileo Satellite Position (virtual)
1937////////////////////////////////////////////////////////////////////////////
1938t_irc t_ephGal::position(int GPSweek, double GPSweeks, double *xc,
1939 double *vv) const {
1940
1941 static const double omegaEarth = 7292115.1467e-11;
1942 static const double gmWGS = 398.6004418e12;
1943
1944 memset(xc, 0, 6 * sizeof(double));
1945 memset(vv, 0, 3 * sizeof(double));
1946
1947 double a0 = _sqrt_A * _sqrt_A;
1948 if (a0 == 0) {
1949 return failure;
1950 }
1951
1952 double n0 = sqrt(gmWGS / (a0 * a0 * a0));
1953
1954 bncTime tt(GPSweek, GPSweeks);
1955 double tk = tt - bncTime(_TOC.gpsw(), _TOEsec);
1956
1957 double n = n0 + _Delta_n;
1958 double M = _M0 + n * tk;
1959 double E = M;
1960 double E_last;
1961 int nLoop = 0;
1962 do {
1963 E_last = E;
1964 E = M + _e * sin(E);
1965
1966 if (++nLoop == 100) {
1967 return failure;
1968 }
1969 } while (fabs(E - E_last) * a0 > 0.001);
1970 double v = 2.0 * atan(sqrt((1.0 + _e) / (1.0 - _e)) * tan(E / 2));
1971 double u0 = v + _omega;
1972 double sin2u0 = sin(2 * u0);
1973 double cos2u0 = cos(2 * u0);
1974 double r = a0 * (1 - _e * cos(E)) + _Crc * cos2u0 + _Crs * sin2u0;
1975 double i = _i0 + _IDOT * tk + _Cic * cos2u0 + _Cis * sin2u0;
1976 double u = u0 + _Cuc * cos2u0 + _Cus * sin2u0;
1977 double xp = r * cos(u);
1978 double yp = r * sin(u);
1979 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth) * tk - omegaEarth * _TOEsec;
1980
1981 double sinom = sin(OM);
1982 double cosom = cos(OM);
1983 double sini = sin(i);
1984 double cosi = cos(i);
1985 xc[0] = xp * cosom - yp * cosi * sinom;
1986 xc[1] = xp * sinom + yp * cosi * cosom;
1987 xc[2] = yp * sini;
1988
1989 double tc = tt - _TOC;
1990 xc[3] = _clock_bias + _clock_drift * tc + _clock_driftrate * tc * tc;
1991
1992 // Velocity
1993 // --------
1994 double tanv2 = tan(v / 2);
1995 double dEdM = 1 / (1 - _e * cos(E));
1996 double dotv = sqrt((1.0 + _e) / (1.0 - _e)) / cos(E / 2) / cos(E / 2)
1997 / (1 + tanv2 * tanv2) * dEdM * n;
1998 double dotu = dotv + (-_Cuc * sin2u0 + _Cus * cos2u0) * 2 * dotv;
1999 double dotom = _OMEGADOT - omegaEarth;
2000 double doti = _IDOT + (-_Cic * sin2u0 + _Cis * cos2u0) * 2 * dotv;
2001 double dotr = a0 * _e * sin(E) * dEdM * n
2002 + (-_Crc * sin2u0 + _Crs * cos2u0) * 2 * dotv;
2003 double dotx = dotr * cos(u) - r * sin(u) * dotu;
2004 double doty = dotr * sin(u) + r * cos(u) * dotu;
2005
2006 vv[0] = cosom * dotx - cosi * sinom * doty // dX / dr
2007 - xp * sinom * dotom - yp * cosi * cosom * dotom // dX / dOMEGA
2008 + yp * sini * sinom * doti; // dX / di
2009
2010 vv[1] = sinom * dotx + cosi * cosom * doty + xp * cosom * dotom
2011 - yp * cosi * sinom * dotom - yp * sini * cosom * doti;
2012
2013 vv[2] = sini * doty + yp * cosi * doti;
2014
2015 // Relativistic Correction
2016 // -----------------------
2017 xc[3] -= 4.442807309e-10 * _e * sqrt(a0) * sin(E);
2018
2019 xc[4] = _clock_drift + _clock_driftrate * tc;
2020 xc[5] = _clock_driftrate;
2021
2022 return success;
2023}
2024
2025// Health status of Galileo Ephemeris (virtual)
2026////////////////////////////////////////////////////////////////////////////
2027unsigned int t_ephGal::isUnhealthy() const {
2028 // SHS; 1 = Out of Service, 3 = In Test, 0 = Signal Ok, 2 = Extended Operations Mode
2029 if (_E5a_HS == 1 || _E5a_HS == 3 ||
2030 _E5b_HS == 1 || _E5b_HS == 3 ||
2031 _E1B_HS == 1 || _E1B_HS == 3) {
2032 return 1;
2033 }
2034 if (_E5a_DataInvalid ||
2035 _E5b_DataInvalid ||
2036 _E1B_DataInvalid) {
2037 return 1;
2038 }
2039 if (_SISA == 255.0) { // NAPA: No Accuracy Prediction Available
2040 return 1;
2041 }
2042 /*
2043 * SDD v1.3: SHS=2 leads to a newly-defined "EOM" status.
2044 * It also means that the satellite signal may be used for PNT.
2045 if (_E5aHS == 2 ||
2046 _E5bHS == 2 ||
2047 _E1_bHS == 2 ) {
2048 return 1;
2049 }
2050 */
2051 return 0;
2052
2053}
2054
2055// RINEX Format String
2056//////////////////////////////////////////////////////////////////////////////
2057QString t_ephGal::toString(double version) const {
2058
2059 QString ephStr = typeStr(_type, _prn, version);
2060 QString rnxStr = ephStr + rinexDateStr(_TOC, _prn, version);
2061
2062 QTextStream out(&rnxStr);
2063
2064 out
2065 << QString("%1%2%3\n").arg(_clock_bias, 19, 'e', 12).arg(_clock_drift, 19,
2066 'e', 12).arg(_clock_driftrate, 19, 'e', 12);
2067
2068 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
2069 // =====================
2070 // BROADCAST ORBIT - 1
2071 // =====================
2072 out
2073 << QString(fmt)
2074 .arg(_IODnav, 19, 'e', 12)
2075 .arg(_Crs, 19, 'e', 12)
2076 .arg(_Delta_n, 19, 'e', 12)
2077 .arg(_M0, 19, 'e', 12);
2078 // =====================
2079 // BROADCAST ORBIT - 2
2080 // =====================
2081 out
2082 << QString(fmt)
2083 .arg(_Cuc, 19, 'e', 12).
2084 arg(_e, 19, 'e', 12)
2085 .arg(_Cus, 19, 'e', 12)
2086 .arg(_sqrt_A, 19, 'e', 12);
2087 // =====================
2088 // BROADCAST ORBIT - 3
2089 // =====================
2090 out
2091 << QString(fmt).
2092 arg(_TOEsec, 19, 'e', 12)
2093 .arg(_Cic, 19, 'e', 12)
2094 .arg(_OMEGA0, 19, 'e', 12)
2095 .arg(_Cis, 19, 'e', 12);
2096 // =====================
2097 // BROADCAST ORBIT - 4
2098 // =====================
2099 out
2100 << QString(fmt)
2101 .arg(_i0, 19, 'e', 12)
2102 .arg(_Crc, 19, 'e', 12)
2103 .arg(_omega, 19, 'e', 12)
2104 .arg(_OMEGADOT, 19, 'e', 12);
2105 // =====================
2106 // BROADCAST ORBIT - 5/6
2107 // =====================
2108 int dataSource = 0;
2109 int SVhealth = 0;
2110 double BGD_1_5A = _BGD_1_5A;
2111 double BGD_1_5B = _BGD_1_5B;
2112 if (_fnav) {
2113 dataSource |= (1 << 1);
2114 dataSource |= (1 << 8);
2115 BGD_1_5B = 0.0;
2116 // SVhealth
2117 // Bit 3 : E5a DVS
2118 if (_E5a_DataInvalid) {
2119 SVhealth |= (1 << 3);
2120 }
2121 // Bit 4-5: E5a HS
2122 if (_E5a_HS == 1.0) {
2123 SVhealth |= (1 << 4);
2124 }
2125 else if (_E5a_HS == 2.0) {
2126 SVhealth |= (1 << 5);
2127 }
2128 else if (_E5a_HS == 3.0) {
2129 SVhealth |= (1 << 4);
2130 SVhealth |= (1 << 5);
2131 }
2132 } else if (_inav) {
2133 // Bit 2 and 0 are set because from MT1046 the data source cannot be determined
2134 // and RNXv3.03 says both can be set if the navigation messages were merged
2135 dataSource |= (1 << 0);
2136 dataSource |= (1 << 2);
2137 dataSource |= (1 << 9);
2138 // SVhealth
2139 // Bit 0 : E1-B DVS
2140 if (_E1B_DataInvalid) {
2141 SVhealth |= (1 << 0);
2142 }
2143 // Bit 1-2: E1-B HS
2144 if (_E1B_HS == 1.0) {
2145 SVhealth |= (1 << 1);
2146 }
2147 else if (_E1B_HS == 2.0) {
2148 SVhealth |= (1 << 2);
2149 }
2150 else if (_E1B_HS == 3.0) {
2151 SVhealth |= (1 << 1);
2152 SVhealth |= (1 << 2);
2153 }
2154 // Bit 6 : E5b DVS
2155 if (_E5b_DataInvalid) {
2156 SVhealth |= (1 << 6);
2157 }
2158 // Bit 7-8: E5b HS
2159 if (_E5b_HS == 1.0) {
2160 SVhealth |= (1 << 7);
2161 }
2162 else if (_E5b_HS == 2.0) {
2163 SVhealth |= (1 << 8);
2164 }
2165 else if (_E5b_HS == 3.0) {
2166 SVhealth |= (1 << 7);
2167 SVhealth |= (1 << 8);
2168 }
2169 }
2170 // =====================
2171 // BROADCAST ORBIT - 5
2172 // =====================
2173 out
2174 << QString(fmt)
2175 .arg(_IDOT, 19, 'e', 12)
2176 .arg(double(dataSource), 19, 'e', 12)
2177 .arg(_TOEweek + 1024.0, 19, 'e', 12)
2178 .arg(0.0, 19, 'e', 12);
2179 // =====================
2180 // BROADCAST ORBIT - 6
2181 // =====================
2182 out
2183 << QString(fmt)
2184 .arg(_SISA, 19, 'e', 12)
2185 .arg(double(SVhealth), 19, 'e', 12)
2186 .arg(BGD_1_5A, 19, 'e', 12)
2187 .arg(BGD_1_5B, 19, 'e', 12);
2188 // =====================
2189 // BROADCAST ORBIT - 7
2190 // =====================
2191 double tot = _TOT;
2192 if (tot == 0.9999e9 && version < 3.0) {
2193 tot = 0.0;
2194 }
2195 out
2196 << QString(fmt)
2197 .arg(tot, 19, 'e', 12)
2198 .arg("", 19, QChar(' '))
2199 .arg("", 19, QChar(' '))
2200 .arg("", 19, QChar(' '));
2201
2202 return rnxStr;
2203}
2204
2205// Constructor
2206//////////////////////////////////////////////////////////////////////////////
2207t_ephSBAS::t_ephSBAS(double rnxVersion, const QStringList &lines, const QString typeStr) {
2208
2209 setType(typeStr);
2210
2211 const int nLines = 4;
2212
2213 // Source RINEX version < 4
2214 if (type() == t_eph::undefined) {
2215 _type = t_eph::SBASL1;
2216 }
2217
2218 if (lines.size() != nLines) {
2219 _checkState = bad;
2220 return;
2221 }
2222
2223 // RINEX Format
2224 // ------------
2225 int fieldLen = 19;
2226
2227 int pos[4];
2228 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
2229 pos[1] = pos[0] + fieldLen;
2230 pos[2] = pos[1] + fieldLen;
2231 pos[3] = pos[2] + fieldLen;
2232
2233 // Read four lines
2234 // ---------------
2235 for (int iLine = 0; iLine < nLines; iLine++) {
2236 QString line = lines[iLine];
2237
2238 if (iLine == 0) {
2239 QTextStream in(line.left(pos[1]).toLatin1());
2240
2241 int year, month, day, hour, min;
2242 double sec;
2243
2244 QString prnStr, n;
2245 in >> prnStr;
2246 if (prnStr.size() == 1 && prnStr[0] == 'S') {
2247 in >> n;
2248 prnStr.append(n);
2249 }
2250 if (prnStr.at(0) == 'S') {
2251 _prn.set('S', prnStr.mid(1).toInt());
2252 } else {
2253 _prn.set('S', prnStr.toInt());
2254 }
2255 _prn.setFlag(type());
2256
2257 in >> year >> month >> day >> hour >> min >> sec;
2258
2259 if (year < 80) {
2260 year += 2000;
2261 } else if (year < 100) {
2262 year += 1900;
2263 }
2264
2265 _TOC.set(year, month, day, hour, min, sec);
2266
2267 if ( readDbl(line, pos[1], fieldLen, _agf0)
2268 || readDbl(line, pos[2], fieldLen, _agf1)
2269 || readDbl(line, pos[3], fieldLen, _TOT)) {
2270 _checkState = bad;
2271 return;
2272 }
2273 }
2274 // =====================
2275 // BROADCAST ORBIT - 1
2276 // =====================
2277 else if (iLine == 1) {
2278 if ( readDbl(line, pos[0], fieldLen, _x_pos)
2279 || readDbl(line, pos[1], fieldLen, _x_vel)
2280 || readDbl(line, pos[2], fieldLen, _x_acc)
2281 || readDbl(line, pos[3], fieldLen, _health)) {
2282 _checkState = bad;
2283 return;
2284 }
2285 }
2286 // =====================
2287 // BROADCAST ORBIT - 2
2288 // =====================
2289 else if (iLine == 2) {
2290 if ( readDbl(line, pos[0], fieldLen, _y_pos)
2291 || readDbl(line, pos[1], fieldLen, _y_vel)
2292 || readDbl(line, pos[2], fieldLen, _y_acc)
2293 || readDbl(line, pos[3], fieldLen, _ura)) {
2294 _checkState = bad;
2295 return;
2296 }
2297 }
2298 // =====================
2299 // BROADCAST ORBIT - 3
2300 // =====================
2301 else if (iLine == 3) {
2302 double iodn;
2303 if ( readDbl(line, pos[0], fieldLen, _z_pos)
2304 || readDbl(line, pos[1], fieldLen, _z_vel)
2305 || readDbl(line, pos[2], fieldLen, _z_acc)
2306 || readDbl(line, pos[3], fieldLen, iodn)) {
2307 _checkState = bad;
2308 return;
2309 } else {
2310 _IODN = int(iodn);
2311 }
2312 }
2313 }
2314 _x_pos *= 1.e3;
2315 _y_pos *= 1.e3;
2316 _z_pos *= 1.e3;
2317 _x_vel *= 1.e3;
2318 _y_vel *= 1.e3;
2319 _z_vel *= 1.e3;
2320 _x_acc *= 1.e3;
2321 _y_acc *= 1.e3;
2322 _z_acc *= 1.e3;
2323}
2324
2325// IOD of SBAS Ephemeris (virtual)
2326////////////////////////////////////////////////////////////////////////////
2327
2328unsigned int t_ephSBAS::IOD() const {
2329 unsigned char buffer[80];
2330 int size = 0;
2331 int numbits = 0;
2332 long long bitbuffer = 0;
2333 unsigned char *startbuffer = buffer;
2334
2335 SBASADDBITSFLOAT(30, this->_x_pos, 0.08)
2336 SBASADDBITSFLOAT(30, this->_y_pos, 0.08)
2337 SBASADDBITSFLOAT(25, this->_z_pos, 0.4)
2338 SBASADDBITSFLOAT(17, this->_x_vel, 0.000625)
2339 SBASADDBITSFLOAT(17, this->_y_vel, 0.000625)
2340 SBASADDBITSFLOAT(18, this->_z_vel, 0.004)
2341 SBASADDBITSFLOAT(10, this->_x_acc, 0.0000125)
2342 SBASADDBITSFLOAT(10, this->_y_acc, 0.0000125)
2343 SBASADDBITSFLOAT(10, this->_z_acc, 0.0000625)
2344 SBASADDBITSFLOAT(12, this->_agf0,
2345 1.0 / static_cast<double>(1 << 30) / static_cast<double>(1 << 1))
2346 SBASADDBITSFLOAT(8, this->_agf1,
2347 1.0 / static_cast<double>(1 << 30) / static_cast<double>(1 << 10))
2348 SBASADDBITS(5, 0); // the last byte is filled by 0-bits to obtain a length of an integer multiple of 8
2349
2350 return CRC24(size, startbuffer);
2351}
2352
2353// Compute SBAS Satellite Position (virtual)
2354////////////////////////////////////////////////////////////////////////////
2355t_irc t_ephSBAS::position(int GPSweek, double GPSweeks, double *xc,
2356 double *vv) const {
2357
2358 bncTime tt(GPSweek, GPSweeks);
2359 double dt = tt - _TOC;
2360
2361 xc[0] = _x_pos + _x_vel * dt + _x_acc * dt * dt / 2.0;
2362 xc[1] = _y_pos + _y_vel * dt + _y_acc * dt * dt / 2.0;
2363 xc[2] = _z_pos + _z_vel * dt + _z_acc * dt * dt / 2.0;
2364
2365 vv[0] = _x_vel + _x_acc * dt;
2366 vv[1] = _y_vel + _y_acc * dt;
2367 vv[2] = _z_vel + _z_acc * dt;
2368
2369 xc[3] = _agf0 + _agf1 * dt;
2370
2371 xc[4] = _agf1;
2372 xc[5] = 0.0;
2373
2374 return success;
2375}
2376
2377// Health status of SBAS Ephemeris (virtual)
2378////////////////////////////////////////////////////////////////////////////
2379unsigned int t_ephSBAS::isUnhealthy() const {
2380
2381// Bit 5
2382 bool URAindexIs15 = bitExtracted(unsigned(_health), 1, 5);
2383 if (URAindexIs15) {
2384 // in this case it is recommended
2385 // to set the bits 0,1,2,3 to 1 (MT17health = 15)
2386 return 1;
2387 }
2388
2389 // Bit 4
2390 bool MT17HealthIsUnavailable = bitExtracted(unsigned(_health), 1, 4);
2391 if (MT17HealthIsUnavailable) {
2392 return 0;
2393 }
2394
2395 // Bit 0-3
2396 int MT17health = bitExtracted(unsigned(_health), 4, 0);
2397 if (MT17health) {
2398 return 1;
2399 }
2400
2401 return 0;
2402}
2403
2404// RINEX Format String
2405//////////////////////////////////////////////////////////////////////////////
2406QString t_ephSBAS::toString(double version) const {
2407
2408 QString ephStr = typeStr(_type, _prn, version);
2409 QString rnxStr = ephStr + rinexDateStr(_TOC, _prn, version);
2410
2411 QTextStream out(&rnxStr);
2412
2413 out
2414 << QString("%1%2%3\n")
2415 .arg(_agf0, 19, 'e', 12)
2416 .arg(_agf1, 19, 'e', 12)
2417 .arg(_TOT, 19, 'e', 12);
2418
2419 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
2420 // =====================
2421 // BROADCAST ORBIT - 1
2422 // =====================
2423 out
2424 << QString(fmt)
2425 .arg(1.e-3 * _x_pos, 19, 'e', 12)
2426 .arg(1.e-3 * _x_vel, 19, 'e', 12)
2427 .arg(1.e-3 * _x_acc, 19, 'e', 12)
2428 .arg(_health, 19, 'e', 12);
2429 // =====================
2430 // BROADCAST ORBIT - 2
2431 // =====================
2432 out
2433 << QString(fmt)
2434 .arg(1.e-3 * _y_pos, 19, 'e', 12)
2435 .arg(1.e-3 * _y_vel, 19, 'e', 12)
2436 .arg(1.e-3 * _y_acc, 19, 'e', 12)
2437 .arg(_ura, 19, 'e', 12);
2438 // =====================
2439 // BROADCAST ORBIT - 3
2440 // =====================
2441 out
2442 << QString(fmt)
2443 .arg(1.e-3 * _z_pos, 19, 'e', 12)
2444 .arg(1.e-3 * _z_vel, 19, 'e', 12)
2445 .arg(1.e-3 * _z_acc, 19, 'e', 12)
2446 .arg(double(_IODN), 19, 'e', 12);
2447
2448 return rnxStr;
2449}
2450
2451// Constructor
2452//////////////////////////////////////////////////////////////////////////////
2453t_ephBDS::t_ephBDS(double rnxVersion, const QStringList &lines, const QString typeStr) {
2454
2455 setType(typeStr);
2456
2457 int nLines = 8;
2458
2459 if (type() == t_eph::CNV1 ||
2460 type() == t_eph::CNV2) {
2461 nLines += 2;
2462 }
2463 if (type() == t_eph::CNV3) {
2464 nLines += 1;
2465 }
2466
2467 if (lines.size() != nLines) {
2468 _checkState = bad;
2469 return;
2470 }
2471
2472 // RINEX Format
2473 // ------------
2474 int fieldLen = 19;
2475
2476 int pos[4];
2477 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
2478 pos[1] = pos[0] + fieldLen;
2479 pos[2] = pos[1] + fieldLen;
2480 pos[3] = pos[2] + fieldLen;
2481
2482 // Read eight lines
2483 // ----------------
2484 for (int iLine = 0; iLine < nLines; iLine++) {
2485 QString line = lines[iLine];
2486
2487 if (iLine == 0) {
2488 QTextStream in(line.left(pos[1]).toLatin1());
2489
2490 int year, month, day, hour, min;
2491 double sec;
2492
2493 QString prnStr, n;
2494 in >> prnStr;
2495 if (prnStr.size() == 1 && prnStr[0] == 'C') {
2496 in >> n;
2497 prnStr.append(n);
2498 }
2499 if (prnStr.at(0) == 'C') {
2500 _prn.set('C', prnStr.mid(1).toInt());
2501 } else {
2502 _prn.set('C', prnStr.toInt());
2503 }
2504
2505 in >> year >> month >> day >> hour >> min >> sec;
2506 if (year < 80) {
2507 year += 2000;
2508 } else if (year < 100) {
2509 year += 1900;
2510 }
2511
2512 _TOC.setBDS(year, month, day, hour, min, sec);
2513
2514 if ( readDbl(line, pos[1], fieldLen, _clock_bias)
2515 || readDbl(line, pos[2], fieldLen, _clock_drift)
2516 || readDbl(line, pos[3], fieldLen, _clock_driftrate)) {
2517 _checkState = bad;
2518 return;
2519 }
2520 }
2521 // =====================
2522 // BROADCAST ORBIT - 1
2523 // =====================
2524 else if (iLine == 1) {
2525 if (type() == t_eph::D1 ||
2526 type() == t_eph::D2 ||
2527 type() == t_eph::undefined) {
2528 double aode;
2529 if ( readDbl(line, pos[0], fieldLen, aode)
2530 || readDbl(line, pos[1], fieldLen, _Crs)
2531 || readDbl(line, pos[2], fieldLen, _Delta_n)
2532 || readDbl(line, pos[3], fieldLen, _M0)) {
2533 _checkState = bad;
2534 return;
2535 }
2536 _AODE = int(aode);
2537 }
2538 else { //CNV1, CNV2, CNV3
2539 if ( readDbl(line, pos[0], fieldLen, _ADOT)
2540 || readDbl(line, pos[1], fieldLen, _Crs)
2541 || readDbl(line, pos[2], fieldLen, _Delta_n)
2542 || readDbl(line, pos[3], fieldLen, _M0)) {
2543 _checkState = bad;
2544 return;
2545 }
2546 }
2547 }
2548 // =====================
2549 // BROADCAST ORBIT - 2
2550 // =====================
2551 else if (iLine == 2) {
2552 if ( readDbl(line, pos[0], fieldLen, _Cuc)
2553 || readDbl(line, pos[1], fieldLen, _e)
2554 || readDbl(line, pos[2], fieldLen, _Cus)
2555 || readDbl(line, pos[3], fieldLen, _sqrt_A)) {
2556 _checkState = bad;
2557 return;
2558 }
2559 }
2560 // =====================
2561 // BROADCAST ORBIT - 3
2562 // =====================
2563 else if (iLine == 3) {
2564 if ( readDbl(line, pos[0], fieldLen, _TOEsec)
2565 || readDbl(line, pos[1], fieldLen, _Cic)
2566 || readDbl(line, pos[2], fieldLen, _OMEGA0)
2567 || readDbl(line, pos[3], fieldLen, _Cis)) {
2568 _checkState = bad;
2569 return;
2570 }
2571 }
2572 // =====================
2573 // BROADCAST ORBIT - 4
2574 // =====================
2575 else if (iLine == 4) {
2576 if ( readDbl(line, pos[0], fieldLen, _i0)
2577 || readDbl(line, pos[1], fieldLen, _Crc)
2578 || readDbl(line, pos[2], fieldLen, _omega)
2579 || readDbl(line, pos[3], fieldLen, _OMEGADOT)) {
2580 _checkState = bad;
2581 return;
2582 }
2583 else {
2584 // Source RINEX version < 4
2585 if (type() == t_eph::undefined) {
2586 const double iMaxGEO = 10.0 / 180.0 * M_PI;
2587 if (_i0 > iMaxGEO) {
2588 _type = t_eph::D1;
2589 }
2590 else {
2591 _type = t_eph::D2;
2592 }
2593 }
2594 }
2595 }
2596 // =====================
2597 // BROADCAST ORBIT - 5
2598 // =====================
2599 else if (iLine == 5) {
2600 if (type() == t_eph::CNV1 ||
2601 type() == t_eph::CNV2 ||
2602 type() == t_eph::CNV3) {
2603 if ( readDbl(line, pos[0], fieldLen, _IDOT)
2604 || readDbl(line, pos[1], fieldLen, _Delta_n_dot)
2605 || readDbl(line, pos[2], fieldLen, _satType)
2606 || readDbl(line, pos[3], fieldLen, _top)) {
2607 _checkState = bad;
2608 return;
2609 }
2610 }
2611 else { // D1, D2
2612 if ( readDbl(line, pos[0], fieldLen, _IDOT)
2613 || readDbl(line, pos[2], fieldLen, _BDTweek)) {
2614 _checkState = bad;
2615 return;
2616 }
2617 }
2618 }
2619 // =====================
2620 // BROADCAST ORBIT - 6
2621 // =====================
2622 else if (iLine == 6) {
2623 if (type() == t_eph::CNV1 ||
2624 type() == t_eph::CNV2 ||
2625 type() == t_eph::CNV3) {
2626 if ( readDbl(line, pos[0], fieldLen, _SISAI_oe)
2627 || readDbl(line, pos[1], fieldLen, _SISAI_ocb)
2628 || readDbl(line, pos[2], fieldLen, _SISAI_oc1)
2629 || readDbl(line, pos[3], fieldLen, _SISAI_oc2)) {
2630 _checkState = bad;
2631 return;
2632 }
2633 }
2634 else { // D1, D2
2635 double SatH1;
2636 if ( readDbl(line, pos[0], fieldLen, _ura)
2637 || readDbl(line, pos[1], fieldLen, SatH1)
2638 || readDbl(line, pos[2], fieldLen, _TGD1)
2639 || readDbl(line, pos[3], fieldLen, _TGD2)) {
2640 _checkState = bad;
2641 return;
2642 }
2643 _SatH1 = int(SatH1);
2644 }
2645 }
2646 // =====================
2647 // BROADCAST ORBIT - 7
2648 // =====================
2649 else if (iLine == 7) {
2650 if (type() == t_eph::CNV1) {
2651 if ( readDbl(line, pos[0], fieldLen, _ISC_B1Cd)
2652 || readDbl(line, pos[2], fieldLen, _TGD_B1Cp)
2653 || readDbl(line, pos[3], fieldLen, _TGD_B2ap)) {
2654 _checkState = bad;
2655 return;
2656 }
2657 }
2658 else if (type() == t_eph::CNV2) {
2659 if ( readDbl(line, pos[1], fieldLen, _ISC_B2ad)
2660 || readDbl(line, pos[2], fieldLen, _TGD_B1Cp)
2661 || readDbl(line, pos[3], fieldLen, _TGD_B2ap)) {
2662 _checkState = bad;
2663 return;
2664 }
2665 }
2666 else if (type() == t_eph::CNV3) {
2667 double health;
2668 if ( readDbl(line, pos[0], fieldLen, _SISMAI)
2669 || readDbl(line, pos[1], fieldLen, health)
2670 || readDbl(line, pos[2], fieldLen, _INTEGRITYF_B2b)
2671 || readDbl(line, pos[3], fieldLen, _TGD_B2bI)) {
2672 _checkState = bad;
2673 return;
2674 }
2675 _health = int(health);
2676 }
2677 else { // D1, D2
2678 double aodc;
2679 if ( readDbl(line, pos[0], fieldLen, _TOT)
2680 || readDbl(line, pos[1], fieldLen, aodc)) {
2681 _checkState = bad;
2682 return;
2683 }
2684 if (_TOT == 0.9999e9) { // 0.9999e9 means not known (RINEX standard)
2685 _TOT = _TOEsec;
2686 }
2687 _AODC = int(aodc);
2688 }
2689 }
2690 // =====================
2691 // BROADCAST ORBIT - 8
2692 // =====================
2693 else if (iLine == 8) {
2694 double health;
2695 if (type() == t_eph::CNV1) {
2696 if ( readDbl(line, pos[0], fieldLen, _SISMAI)
2697 || readDbl(line, pos[1], fieldLen, health)
2698 || readDbl(line, pos[2], fieldLen, _INTEGRITYF_B1C)
2699 || readDbl(line, pos[3], fieldLen, _IODC)) {
2700 _checkState = bad;
2701 return;
2702 }
2703 _health = int(health);
2704 }
2705 else if (type() == t_eph::CNV2) {
2706 if ( readDbl(line, pos[0], fieldLen, _SISMAI)
2707 || readDbl(line, pos[1], fieldLen, health)
2708 || readDbl(line, pos[2], fieldLen, _INTEGRITYF_B2aB1C)
2709 || readDbl(line, pos[3], fieldLen, _IODC)) {
2710 _checkState = bad;
2711 return;
2712 }
2713 _health = int(health);
2714 }
2715 else if (type() == t_eph::CNV3) {
2716 if (readDbl(line, pos[0], fieldLen, _TOT)) {
2717 _checkState = bad;
2718 return;
2719 }
2720 }
2721 }
2722 // =====================
2723 // BROADCAST ORBIT - 9
2724 // =====================
2725 else if (iLine == 9) {
2726 if (type() == t_eph::CNV1 ||
2727 type() == t_eph::CNV2) {
2728 if ( readDbl(line, pos[0], fieldLen, _TOT)
2729 || readDbl(line, pos[3], fieldLen, _IODE)) {
2730 _checkState = bad;
2731 return;
2732 }
2733 }
2734 }
2735 }
2736 _prn.setFlag(type());
2737
2738 _TOE.setBDS(int(_BDTweek), _TOEsec);
2739 // remark: actually should be computed from second_tot
2740 // but it seems to be unreliable in RINEX files
2741 //_TOT = _TOC.bdssec();
2742}
2743
2744// IOD of BDS Ephemeris (virtual)
2745////////////////////////////////////////////////////////////////////////////
2746unsigned int t_ephBDS::IOD() const {
2747 if (type() == t_eph::D1 ||
2748 type() == t_eph::D2) {
2749 return (int(_TOC.gpssec()) / 720) % 240;
2750 }
2751 else {
2752 return _IODE;
2753 }
2754}
2755
2756// Compute BDS Satellite Position (virtual)
2757//////////////////////////////////////////////////////////////////////////////
2758t_irc t_ephBDS::position(int GPSweek, double GPSweeks, double *xc,
2759 double *vv) const {
2760
2761 static const double gmBDS = 398.6004418e12;
2762 static const double omegaBDS = 7292115.0000e-11;
2763
2764 xc[0] = xc[1] = xc[2] = xc[3] = 0.0;
2765 vv[0] = vv[1] = vv[2] = 0.0;
2766
2767 bncTime tt(GPSweek, GPSweeks);
2768
2769 if (_sqrt_A == 0) {
2770 return failure;
2771 }
2772 double a0 = _sqrt_A * _sqrt_A;
2773
2774 double n0 = sqrt(gmBDS / (a0 * a0 * a0));
2775 double tk = tt - _TOE;
2776 double n = n0 + _Delta_n;
2777 double M = _M0 + n * tk;
2778 double E = M;
2779 double E_last;
2780 int nLoop = 0;
2781 do {
2782 E_last = E;
2783 E = M + _e * sin(E);
2784
2785 if (++nLoop == 100) {
2786 return failure;
2787 }
2788 } while (fabs(E - E_last) * a0 > 0.001);
2789
2790 double v = atan2(sqrt(1 - _e * _e) * sin(E), cos(E) - _e);
2791 double u0 = v + _omega;
2792 double sin2u0 = sin(2 * u0);
2793 double cos2u0 = cos(2 * u0);
2794 double r = a0 * (1 - _e * cos(E)) + _Crc * cos2u0 + _Crs * sin2u0;
2795 double i = _i0 + _IDOT * tk + _Cic * cos2u0 + _Cis * sin2u0;
2796 double u = u0 + _Cuc * cos2u0 + _Cus * sin2u0;
2797 double xp = r * cos(u);
2798 double yp = r * sin(u);
2799 double toesec = (_TOE.gpssec() - 14.0);
2800 double sinom = 0;
2801 double cosom = 0;
2802 double sini = 0;
2803 double cosi = 0;
2804
2805 // Velocity
2806 // --------
2807 double tanv2 = tan(v / 2);
2808 double dEdM = 1 / (1 - _e * cos(E));
2809 double dotv = sqrt((1.0 + _e) / (1.0 - _e)) / cos(E / 2) / cos(E / 2)
2810 / (1 + tanv2 * tanv2) * dEdM * n;
2811 double dotu = dotv + (-_Cuc * sin2u0 + _Cus * cos2u0) * 2 * dotv;
2812 double doti = _IDOT + (-_Cic * sin2u0 + _Cis * cos2u0) * 2 * dotv;
2813 double dotr = a0 * _e * sin(E) * dEdM * n
2814 + (-_Crc * sin2u0 + _Crs * cos2u0) * 2 * dotv;
2815
2816 double dotx = dotr * cos(u) - r * sin(u) * dotu;
2817 double doty = dotr * sin(u) + r * cos(u) * dotu;
2818
2819 const double iMaxGEO = 10.0 / 180.0 * M_PI;
2820
2821 // MEO/IGSO satellite
2822 // ------------------
2823 if (_i0 > iMaxGEO) {
2824 double OM = _OMEGA0 + (_OMEGADOT - omegaBDS) * tk - omegaBDS * toesec;
2825
2826 sinom = sin(OM);
2827 cosom = cos(OM);
2828 sini = sin(i);
2829 cosi = cos(i);
2830
2831 xc[0] = xp * cosom - yp * cosi * sinom;
2832 xc[1] = xp * sinom + yp * cosi * cosom;
2833 xc[2] = yp * sini;
2834
2835 // Velocity
2836 // --------
2837
2838 double dotom = _OMEGADOT - t_CST::omega;
2839
2840 vv[0] = cosom * dotx - cosi * sinom * doty // dX / dr
2841 - xp * sinom * dotom - yp * cosi * cosom * dotom // dX / dOMEGA
2842 + yp * sini * sinom * doti; // dX / di
2843
2844 vv[1] = sinom * dotx + cosi * cosom * doty + xp * cosom * dotom
2845 - yp * cosi * sinom * dotom - yp * sini * cosom * doti;
2846
2847 vv[2] = sini * doty + yp * cosi * doti;
2848
2849 }
2850
2851 // GEO satellite
2852 // -------------
2853 else {
2854 double OM = _OMEGA0 + _OMEGADOT * tk - omegaBDS * toesec;
2855 double ll = omegaBDS * tk;
2856
2857 sinom = sin(OM);
2858 cosom = cos(OM);
2859 sini = sin(i);
2860 cosi = cos(i);
2861
2862 double xx = xp * cosom - yp * cosi * sinom;
2863 double yy = xp * sinom + yp * cosi * cosom;
2864 double zz = yp * sini;
2865
2866 Matrix RX = BNC_PPP::t_astro::rotX(-5.0 / 180.0 * M_PI);
2867 Matrix RZ = BNC_PPP::t_astro::rotZ(ll);
2868
2869 ColumnVector X1(3);
2870 X1 << xx << yy << zz;
2871 ColumnVector X2 = RZ * RX * X1;
2872
2873 xc[0] = X2(1);
2874 xc[1] = X2(2);
2875 xc[2] = X2(3);
2876
2877 double dotom = _OMEGADOT;
2878
2879 double vx = cosom * dotx - cosi * sinom * doty - xp * sinom * dotom
2880 - yp * cosi * cosom * dotom + yp * sini * sinom * doti;
2881
2882 double vy = sinom * dotx + cosi * cosom * doty + xp * cosom * dotom
2883 - yp * cosi * sinom * dotom - yp * sini * cosom * doti;
2884
2885 double vz = sini * doty + yp * cosi * doti;
2886
2887 ColumnVector V(3);
2888 V << vx << vy << vz;
2889
2890 Matrix RdotZ(3, 3);
2891 double C = cos(ll);
2892 double S = sin(ll);
2893 Matrix UU(3, 3);
2894 UU[0][0] = -S;
2895 UU[0][1] = +C;
2896 UU[0][2] = 0.0;
2897 UU[1][0] = -C;
2898 UU[1][1] = -S;
2899 UU[1][2] = 0.0;
2900 UU[2][0] = 0.0;
2901 UU[2][1] = 0.0;
2902 UU[2][2] = 0.0;
2903 RdotZ = omegaBDS * UU;
2904
2905 ColumnVector VV(3);
2906 VV = RZ * RX * V + RdotZ * RX * X1;
2907
2908 vv[0] = VV(1);
2909 vv[1] = VV(2);
2910 vv[2] = VV(3);
2911 }
2912
2913 double tc = tt - _TOC;
2914 xc[3] = _clock_bias + _clock_drift * tc + _clock_driftrate * tc * tc;
2915
2916// dotC = _clock_drift + _clock_driftrate*tc
2917// - 4.442807309e-10*_e * sqrt(a0) * cos(E) * dEdM * n;
2918
2919 // Relativistic Correction
2920 // -----------------------
2921 xc[3] -= 4.442807309e-10 * _e * sqrt(a0) * sin(E);
2922
2923 xc[4] = _clock_drift + _clock_driftrate * tc;
2924 xc[5] = _clock_driftrate;
2925
2926 return success;
2927}
2928
2929// Health status of SBAS Ephemeris (virtual)
2930////////////////////////////////////////////////////////////////////////////
2931unsigned int t_ephBDS::isUnhealthy() const {
2932
2933 if (type() == t_eph::CNV1 ||
2934 type() == t_eph::CNV2 ||
2935 type() == t_eph::CNV3) {
2936 return static_cast<unsigned int>(_health);
2937 }
2938
2939 return static_cast<unsigned int>(_SatH1);
2940
2941}
2942
2943// RINEX Format String
2944//////////////////////////////////////////////////////////////////////////////
2945QString t_ephBDS::toString(double version) const {
2946
2947 if (version < 4.0 &&
2948 (type() == t_eph::CNV1 ||
2949 type() == t_eph::CNV2 ||
2950 type() == t_eph::CNV3 )) {
2951 return "";
2952 }
2953
2954 QString ephStr = typeStr(_type, _prn, version);
2955 QString rnxStr = ephStr + rinexDateStr(_TOC - 14.0, _prn, version);
2956
2957 QTextStream out(&rnxStr);
2958
2959 out
2960 << QString("%1%2%3\n")
2961 .arg(_clock_bias, 19, 'e', 12)
2962 .arg(_clock_drift, 19, 'e', 12)
2963 .arg(_clock_driftrate, 19, 'e', 12);
2964
2965 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
2966 // =====================
2967 // BROADCAST ORBIT - 1
2968 // =====================
2969 if (type() == t_eph::D1 ||
2970 type() == t_eph::D2 ||
2971 type() == t_eph::undefined) {
2972 out
2973 << QString(fmt)
2974 .arg(double(_AODE), 19, 'e', 12)
2975 .arg(_Crs, 19, 'e', 12)
2976 .arg(_Delta_n, 19, 'e', 12)
2977 .arg(_M0, 19, 'e', 12);
2978 }
2979 else { //CNV1, CNV2, CNV3
2980 out
2981 << QString(fmt)
2982 .arg(_ADOT, 19, 'e', 12)
2983 .arg(_Crs, 19, 'e', 12)
2984 .arg(_Delta_n, 19, 'e', 12)
2985 .arg(_M0, 19, 'e', 12);
2986 }
2987
2988 // =====================
2989 // BROADCAST ORBIT - 2
2990 // =====================
2991 out
2992 << QString(fmt)
2993 .arg(_Cuc, 19, 'e', 12)
2994 .arg(_e, 19, 'e', 12)
2995 .arg(_Cus, 19, 'e', 12)
2996 .arg(_sqrt_A, 19, 'e', 12);
2997
2998 // =====================
2999 // BROADCAST ORBIT - 3
3000 // =====================
3001 out
3002 << QString(fmt)
3003 .arg(_TOEsec, 19, 'e', 12)
3004 .arg(_Cic, 19, 'e', 12)
3005 .arg(_OMEGA0, 19, 'e', 12)
3006 .arg(_Cis, 19, 'e', 12);
3007 // =====================
3008 // BROADCAST ORBIT - 4
3009 // =====================
3010 out
3011 << QString(fmt)
3012 .arg(_i0, 19, 'e', 12)
3013 .arg(_Crc, 19, 'e', 12)
3014 .arg(_omega, 19, 'e', 12)
3015 .arg(_OMEGADOT, 19, 'e', 12);
3016 // =====================
3017 // BROADCAST ORBIT - 5
3018 // =====================
3019 if (type() == t_eph::CNV1 ||
3020 type() == t_eph::CNV2 ||
3021 type() == t_eph::CNV3) {
3022 out
3023 << QString(fmt)
3024 .arg(_IDOT, 19, 'e', 12)
3025 .arg(_Delta_n_dot, 19, 'e', 12)
3026 .arg(_satType, 19, 'e', 12)
3027 .arg(_top, 19, 'e', 12);
3028 }
3029 else { // D1, D2,
3030 out
3031 << QString(fmt)
3032 .arg(_IDOT, 19, 'e', 12)
3033 .arg("", 19, QChar(' '))
3034 .arg(_BDTweek, 19, 'e', 12)
3035 .arg("", 19, QChar(' '));
3036 }
3037 // =====================
3038 // BROADCAST ORBIT - 6
3039 // =====================
3040 if (type() == t_eph::CNV1 ||
3041 type() == t_eph::CNV2 ||
3042 type() == t_eph::CNV3) {
3043 out
3044 << QString(fmt)
3045 .arg(_SISAI_oe, 19, 'e', 12)
3046 .arg(_SISAI_ocb, 19, 'e', 12)
3047 .arg(_SISAI_oc1, 19, 'e', 12)
3048 .arg(_SISAI_oc2, 19, 'e', 12);
3049 }
3050 else { // D1, D2, undefined
3051 out
3052 << QString(fmt)
3053 .arg(_ura, 19, 'e', 12)
3054 .arg(double(_SatH1), 19, 'e', 12)
3055 .arg(_TGD1, 19, 'e', 12)
3056 .arg(_TGD2, 19, 'e', 12);
3057 }
3058 // =====================
3059 // BROADCAST ORBIT - 7
3060 // =====================
3061 if (type() == t_eph::CNV1) {
3062 out
3063 << QString(fmt)
3064 .arg(_ISC_B1Cd, 19, 'e', 12)
3065 .arg("", 19, QChar(' '))
3066 .arg(_TGD_B1Cp, 19, 'e', 12)
3067 .arg(_TGD_B2ap, 19, 'e', 12);
3068 }
3069 else if (type() == t_eph::CNV2) {
3070 out
3071 << QString(fmt)
3072 .arg("", 19, QChar(' '))
3073 .arg(_ISC_B2ad, 19, 'e', 12)
3074 .arg(_TGD_B1Cp, 19, 'e', 12)
3075 .arg(_TGD_B2ap, 19, 'e', 12);
3076 }
3077 else if (type() == t_eph::CNV3) {
3078 out
3079 << QString(fmt)
3080 .arg(_SISMAI, 19, 'e', 12)
3081 .arg(double(_health), 19, 'e', 12)
3082 .arg(_INTEGRITYF_B2b, 19, 'e', 12)
3083 .arg(_TGD_B2bI, 19, 'e', 12);
3084 }
3085 else { // D1, D2, undefined
3086 double tots = 0.0;
3087 if (_receptDateTime.isValid()) { // RTCM stream input
3088 tots = _TOE.bdssec();
3089 } else { // RINEX input
3090 tots = _TOT;
3091 }
3092 out
3093 << QString(fmt)
3094 .arg(tots, 19, 'e', 12)
3095 .arg(double(_AODC), 19, 'e', 12)
3096 .arg("", 19, QChar(' '))
3097 .arg("", 19, QChar(' '));
3098 }
3099
3100 // =====================
3101 // BROADCAST ORBIT - 8
3102 // =====================
3103 if (type() == t_eph::CNV1) {
3104 out
3105 << QString(fmt)
3106 .arg(_SISMAI, 19, 'e', 12)
3107 .arg(double(_health), 19, 'e', 12)
3108 .arg(_INTEGRITYF_B1C, 19, 'e', 12)
3109 .arg(_IODC, 19, 'e', 12);
3110 }
3111 else if (type() == t_eph::CNV2) {
3112 out
3113 << QString(fmt)
3114 .arg(_SISMAI, 19, 'e', 12)
3115 .arg(double(_health), 19, 'e', 12)
3116 .arg(_INTEGRITYF_B2aB1C, 19, 'e', 12)
3117 .arg(_IODC, 19, 'e', 12);
3118 }
3119 else if (type() == t_eph::CNV3) {
3120 out
3121 << QString(fmt)
3122 .arg(_TOT, 19, 'e', 12)
3123 .arg("", 19, QChar(' '))
3124 .arg("", 19, QChar(' '))
3125 .arg("", 19, QChar(' '));
3126 }
3127
3128 // =====================
3129 // BROADCAST ORBIT - 9
3130 // =====================
3131 if (type() == t_eph::CNV1 ||
3132 type() == t_eph::CNV2) {
3133 out
3134 << QString(fmt)
3135 .arg(_TOT, 19, 'e', 12)
3136 .arg("", 19, QChar(' '))
3137 .arg("", 19, QChar(' '))
3138 .arg(_IODE, 19, 'e', 12);
3139 }
3140
3141 return rnxStr;
3142}
Note: See TracBrowser for help on using the repository browser.