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

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

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