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

Last change on this file since 9126 was 9126, checked in by stuerze, 4 years ago

minor changes
File size: 47.8 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
17using namespace std;
18
19// Constructor
20////////////////////////////////////////////////////////////////////////////
21t_eph::t_eph() {
22 _checkState = unchecked;
23 _orbCorr = 0;
24 _clkCorr = 0;
25}
26// Destructor
27////////////////////////////////////////////////////////////////////////////
28t_eph::~t_eph() {
29 if (_orbCorr)
30 delete _orbCorr;
31 if (_clkCorr)
32 delete _clkCorr;
33}
34
35//
36////////////////////////////////////////////////////////////////////////////
37void t_eph::setOrbCorr(const t_orbCorr* orbCorr) {
38 if (_orbCorr) {
39 delete _orbCorr;
40 _orbCorr = 0;
41 }
42 _orbCorr = new t_orbCorr(*orbCorr);
43}
44
45//
46////////////////////////////////////////////////////////////////////////////
47void t_eph::setClkCorr(const t_clkCorr* clkCorr) {
48 if (_clkCorr) {
49 delete _clkCorr;
50 _clkCorr = 0;
51 }
52 _clkCorr = new t_clkCorr(*clkCorr);
53}
54
55//
56////////////////////////////////////////////////////////////////////////////
57t_irc t_eph::getCrd(const bncTime& tt, ColumnVector& xc, ColumnVector& vv, bool useCorr) const {
58
59 if (_checkState == bad ||
60 _checkState == unhealthy ||
61 _checkState == outdated) {
62 return failure;
63 }
64
65 xc.ReSize(6);
66 vv.ReSize(3);
67 if (position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data()) != success) {
68 return failure;
69 }
70 if (useCorr) {
71 if (_orbCorr && _clkCorr) {
72 double dtO = tt - _orbCorr->_time;
73 if (_orbCorr->_updateInt) {
74 dtO -= (0.5 * ssrUpdateInt[_orbCorr->_updateInt]);
75 }
76 ColumnVector dx(3);
77 dx[0] = _orbCorr->_xr[0] + _orbCorr->_dotXr[0] * dtO;
78 dx[1] = _orbCorr->_xr[1] + _orbCorr->_dotXr[1] * dtO;
79 dx[2] = _orbCorr->_xr[2] + _orbCorr->_dotXr[2] * dtO;
80
81 RSW_to_XYZ(xc.Rows(1,3), vv.Rows(1,3), dx, dx);
82
83 xc[0] -= dx[0];
84 xc[1] -= dx[1];
85 xc[2] -= dx[2];
86
87 ColumnVector dv(3);
88 RSW_to_XYZ(xc.Rows(1,3), vv.Rows(1,3), _orbCorr->_dotXr, dv);
89
90 vv[0] -= dv[0];
91 vv[1] -= dv[1];
92 vv[2] -= dv[2];
93
94 double dtC = tt - _clkCorr->_time;
95 if (_clkCorr->_updateInt) {
96 dtC -= (0.5 * ssrUpdateInt[_clkCorr->_updateInt]);
97 }
98 xc[3] += _clkCorr->_dClk + _clkCorr->_dotDClk * dtC + _clkCorr->_dotDotDClk * dtC * dtC;
99 }
100 else {
101 return failure;
102 }
103 }
104 return success;
105}
106
107//
108//////////////////////////////////////////////////////////////////////////////
109QString t_eph::rinexDateStr(const bncTime& tt, const t_prn& prn, double version) {
110 QString prnStr(prn.toString().c_str());
111 return rinexDateStr(tt, prnStr, version);
112}
113
114//
115//////////////////////////////////////////////////////////////////////////////
116QString t_eph::rinexDateStr(const bncTime& tt, const QString& prnStr, double version) {
117
118 QString datStr;
119
120 unsigned year, month, day, hour, min;
121 double sec;
122 tt.civil_date(year, month, day);
123 tt.civil_time(hour, min, sec);
124
125 QTextStream out(&datStr);
126
127 if (version < 3.0) {
128 QString prnHlp = prnStr.mid(1,2); if (prnHlp[0] == '0') prnHlp[0] = ' ';
129 out << prnHlp << QString(" %1 %2 %3 %4 %5%6")
130 .arg(year % 100, 2, 10, QChar('0'))
131 .arg(month, 2)
132 .arg(day, 2)
133 .arg(hour, 2)
134 .arg(min, 2)
135 .arg(sec, 5, 'f',1);
136 }
137 else {
138 out << prnStr << QString(" %1 %2 %3 %4 %5 %6")
139 .arg(year, 4)
140 .arg(month, 2, 10, QChar('0'))
141 .arg(day, 2, 10, QChar('0'))
142 .arg(hour, 2, 10, QChar('0'))
143 .arg(min, 2, 10, QChar('0'))
144 .arg(int(sec), 2, 10, QChar('0'));
145 }
146
147 return datStr;
148}
149
150// Constructor
151//////////////////////////////////////////////////////////////////////////////
152t_ephGPS::t_ephGPS(float rnxVersion, const QStringList& lines) {
153
154 const int nLines = 8;
155
156 if (lines.size() != nLines) {
157 _checkState = bad;
158 return;
159 }
160
161 // RINEX Format
162 // ------------
163 int fieldLen = 19;
164
165 int pos[4];
166 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
167 pos[1] = pos[0] + fieldLen;
168 pos[2] = pos[1] + fieldLen;
169 pos[3] = pos[2] + fieldLen;
170
171 // Read eight lines
172 // ----------------
173 for (int iLine = 0; iLine < nLines; iLine++) {
174 QString line = lines[iLine];
175
176 if ( iLine == 0 ) {
177 QTextStream in(line.left(pos[1]).toLatin1());
178 int year, month, day, hour, min;
179 double sec;
180
181 QString prnStr, n;
182 in >> prnStr;
183
184 if (prnStr.size() == 1 &&
185 (prnStr[0] == 'G' ||
186 prnStr[0] == 'J' ||
187 prnStr[0] == 'I')) {
188 in >> n;
189 prnStr.append(n);
190 }
191
192 in >> year >> month >> day >> hour >> min >> sec;
193 if (prnStr.at(0) == 'G') {
194 _prn.set('G', prnStr.mid(1).toInt());
195 }
196 else if (prnStr.at(0) == 'J') {
197 _prn.set('J', prnStr.mid(1).toInt());
198 }
199 else if (prnStr.at(0) == 'I') {
200 _prn.set('I', prnStr.mid(1).toInt());
201 }
202 else {
203 _prn.set('G', prnStr.toInt());
204 }
205
206 if (year < 80) {
207 year += 2000;
208 }
209 else if (year < 100) {
210 year += 1900;
211 }
212
213 _TOC.set(year, month, day, hour, min, sec);
214
215 if ( readDbl(line, pos[1], fieldLen, _clock_bias ) ||
216 readDbl(line, pos[2], fieldLen, _clock_drift ) ||
217 readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
218 _checkState = bad;
219 return;
220 }
221 }
222
223 else if ( iLine == 1 ) {
224 if ( readDbl(line, pos[0], fieldLen, _IODE ) ||
225 readDbl(line, pos[1], fieldLen, _Crs ) ||
226 readDbl(line, pos[2], fieldLen, _Delta_n) ||
227 readDbl(line, pos[3], fieldLen, _M0 ) ) {
228 _checkState = bad;
229 return;
230 }
231 }
232
233 else if ( iLine == 2 ) {
234 if ( readDbl(line, pos[0], fieldLen, _Cuc ) ||
235 readDbl(line, pos[1], fieldLen, _e ) ||
236 readDbl(line, pos[2], fieldLen, _Cus ) ||
237 readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
238 _checkState = bad;
239 return;
240 }
241 }
242
243 else if ( iLine == 3 ) {
244 if ( readDbl(line, pos[0], fieldLen, _TOEsec) ||
245 readDbl(line, pos[1], fieldLen, _Cic ) ||
246 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
247 readDbl(line, pos[3], fieldLen, _Cis ) ) {
248 _checkState = bad;
249 return;
250 }
251 }
252
253 else if ( iLine == 4 ) {
254 if ( readDbl(line, pos[0], fieldLen, _i0 ) ||
255 readDbl(line, pos[1], fieldLen, _Crc ) ||
256 readDbl(line, pos[2], fieldLen, _omega ) ||
257 readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
258 _checkState = bad;
259 return;
260 }
261 }
262
263 else if ( iLine == 5 && type() != t_eph::IRNSS) {
264 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
265 readDbl(line, pos[1], fieldLen, _L2Codes) ||
266 readDbl(line, pos[2], fieldLen, _TOEweek ) ||
267 readDbl(line, pos[3], fieldLen, _L2PFlag) ) {
268 _checkState = bad;
269 return;
270 }
271 }
272 else if ( iLine == 5 && type() == t_eph::IRNSS) {
273 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
274 readDbl(line, pos[2], fieldLen, _TOEweek) ) {
275 _checkState = bad;
276 return;
277 }
278 }
279
280 else if ( iLine == 6 && type() != t_eph::IRNSS) {
281 if ( readDbl(line, pos[0], fieldLen, _ura ) ||
282 readDbl(line, pos[1], fieldLen, _health) ||
283 readDbl(line, pos[2], fieldLen, _TGD ) ||
284 readDbl(line, pos[3], fieldLen, _IODC ) ) {
285 _checkState = bad;
286 return;
287 }
288 }
289 else if ( iLine == 6 && type() == t_eph::IRNSS) {
290 if ( readDbl(line, pos[0], fieldLen, _ura ) ||
291 readDbl(line, pos[1], fieldLen, _health) ||
292 readDbl(line, pos[2], fieldLen, _TGD ) ) {
293 _checkState = bad;
294 return;
295 }
296 }
297 else if ( iLine == 7 ) {
298 if ( readDbl(line, pos[0], fieldLen, _TOT) ) {
299 _checkState = bad;
300 return;
301 }
302 // fitInterval is not valid for IRNSS
303 if (type() != t_eph::IRNSS) {
304 double fitIntervalRnx;
305 readDbl(line, pos[1], fieldLen, fitIntervalRnx);
306 if (type() == t_eph::GPS) { // in RINEX specified allways as time period for GPS
307 _fitInterval = fitIntervalRnx;
308 } else if (type() == t_eph::QZSS) { // specified as flag for QZSS
309 if (rnxVersion == 3.02) {
310 _fitInterval = fitIntervalRnx; // specified as time period
311 }
312 else {
313 _fitInterval = fitIntervalFromFlag(fitIntervalRnx, _IODC, t_eph::QZSS);
314 }
315 }
316 }
317 }
318 }
319}
320
321// Compute GPS Satellite Position (virtual)
322////////////////////////////////////////////////////////////////////////////
323t_irc t_ephGPS::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
324
325 static const double omegaEarth = 7292115.1467e-11;
326 static const double gmGRS = 398.6005e12;
327
328 memset(xc, 0, 6*sizeof(double));
329 memset(vv, 0, 3*sizeof(double));
330
331 double a0 = _sqrt_A * _sqrt_A;
332 if (a0 == 0) {
333 return failure;
334 }
335
336 double n0 = sqrt(gmGRS/(a0*a0*a0));
337
338 bncTime tt(GPSweek, GPSweeks);
339 double tk = tt - bncTime(int(_TOEweek), _TOEsec);
340
341 double n = n0 + _Delta_n;
342 double M = _M0 + n*tk;
343 double E = M;
344 double E_last;
345 int nLoop = 0;
346 do {
347 E_last = E;
348 E = M + _e*sin(E);
349
350 if (++nLoop == 100) {
351 return failure;
352 }
353 } while ( fabs(E-E_last)*a0 > 0.001);
354 double v = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) );
355 double u0 = v + _omega;
356 double sin2u0 = sin(2*u0);
357 double cos2u0 = cos(2*u0);
358 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
359 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
360 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
361 double xp = r*cos(u);
362 double yp = r*sin(u);
363 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
364 omegaEarth*_TOEsec;
365
366 double sinom = sin(OM);
367 double cosom = cos(OM);
368 double sini = sin(i);
369 double cosi = cos(i);
370 xc[0] = xp*cosom - yp*cosi*sinom;
371 xc[1] = xp*sinom + yp*cosi*cosom;
372 xc[2] = yp*sini;
373
374 double tc = tt - _TOC;
375 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
376
377 // Velocity
378 // --------
379 double tanv2 = tan(v/2);
380 double dEdM = 1 / (1 - _e*cos(E));
381 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
382 * dEdM * n;
383 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
384 double dotom = _OMEGADOT - omegaEarth;
385 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
386 double dotr = a0 * _e*sin(E) * dEdM * n
387 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
388 double dotx = dotr*cos(u) - r*sin(u)*dotu;
389 double doty = dotr*sin(u) + r*cos(u)*dotu;
390
391 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
392 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
393 + yp*sini*sinom*doti; // dX / di
394
395 vv[1] = sinom *dotx + cosi*cosom *doty
396 + xp*cosom*dotom - yp*cosi*sinom*dotom
397 - yp*sini*cosom*doti;
398
399 vv[2] = sini *doty + yp*cosi *doti;
400
401 // Relativistic Correction
402 // -----------------------
403 // correspondent to IGS convention and GPS ICD (and SSR standard)
404 // xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c;
405 xc[3] -= 4.442807633e-10 * _e * sqrt(a0) *sin(E);
406 // correspondent to IGS-SSR
407 xc[4] = _clock_drift + _clock_driftrate*tc;
408 xc[5] = _clock_driftrate;
409
410 return success;
411}
412
413// RINEX Format String
414//////////////////////////////////////////////////////////////////////////////
415QString t_ephGPS::toString(double version) const {
416
417 QString rnxStr = rinexDateStr(_TOC, _prn, version);
418
419 QTextStream out(&rnxStr);
420
421 out << QString("%1%2%3\n")
422 .arg(_clock_bias, 19, 'e', 12)
423 .arg(_clock_drift, 19, 'e', 12)
424 .arg(_clock_driftrate, 19, 'e', 12);
425
426 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
427
428 out << QString(fmt)
429 .arg(_IODE, 19, 'e', 12)
430 .arg(_Crs, 19, 'e', 12)
431 .arg(_Delta_n, 19, 'e', 12)
432 .arg(_M0, 19, 'e', 12);
433
434 out << QString(fmt)
435 .arg(_Cuc, 19, 'e', 12)
436 .arg(_e, 19, 'e', 12)
437 .arg(_Cus, 19, 'e', 12)
438 .arg(_sqrt_A, 19, 'e', 12);
439
440 out << QString(fmt)
441 .arg(_TOEsec, 19, 'e', 12)
442 .arg(_Cic, 19, 'e', 12)
443 .arg(_OMEGA0, 19, 'e', 12)
444 .arg(_Cis, 19, 'e', 12);
445
446 out << QString(fmt)
447 .arg(_i0, 19, 'e', 12)
448 .arg(_Crc, 19, 'e', 12)
449 .arg(_omega, 19, 'e', 12)
450 .arg(_OMEGADOT, 19, 'e', 12);
451
452 if (type() == t_eph::IRNSS) {
453 out << QString(fmt)
454 .arg(_IDOT, 19, 'e', 12)
455 .arg(0.0, 19, 'e', 12)
456 .arg(_TOEweek, 19, 'e', 12)
457 .arg(0.0, 19, 'e', 12);
458 }
459 else {
460 out << QString(fmt)
461 .arg(_IDOT, 19, 'e', 12)
462 .arg(_L2Codes, 19, 'e', 12)
463 .arg(_TOEweek, 19, 'e', 12)
464 .arg(_L2PFlag, 19, 'e', 12);
465 }
466
467 if (type() == t_eph::IRNSS) {
468 out << QString(fmt)
469 .arg(_ura, 19, 'e', 12)
470 .arg(_health, 19, 'e', 12)
471 .arg(_TGD, 19, 'e', 12)
472 .arg(0.0, 19, 'e', 12);
473 }
474 else {
475 out << QString(fmt)
476 .arg(_ura, 19, 'e', 12)
477 .arg(_health, 19, 'e', 12)
478 .arg(_TGD, 19, 'e', 12)
479 .arg(_IODC, 19, 'e', 12);
480 }
481
482 double tot = _TOT;
483 if (tot == 0.9999e9 && version < 3.0) {
484 tot = 0.0;
485 }
486 // fitInterval
487 if (type() == t_eph::IRNSS) {// not valid for IRNSS
488 out << QString(fmt)
489 .arg(tot, 19, 'e', 12)
490 .arg(0.0, 19, 'e', 12)
491 .arg("", 19, QChar(' '))
492 .arg("", 19, QChar(' '));
493 }
494 else {
495 // for GPS and QZSS in version 3.02 specified in hours
496 double fitIntervalRnx = _fitInterval;
497 // otherwise specified as flag
498 if (type() == t_eph::QZSS && version != 3.02) {
499 (_fitInterval == 2.0) ? fitIntervalRnx = 0.0 : fitIntervalRnx = 1.0;
500 }
501 out << QString(fmt)
502 .arg(tot, 19, 'e', 12)
503 .arg(fitIntervalRnx, 19, 'e', 12)
504 .arg("", 19, QChar(' '))
505 .arg("", 19, QChar(' '));
506 }
507 return rnxStr;
508}
509
510// Constructor
511//////////////////////////////////////////////////////////////////////////////
512t_ephGlo::t_ephGlo(float rnxVersion, const QStringList& lines) {
513
514 const int nLines = 4;
515
516 if (lines.size() != nLines) {
517 _checkState = bad;
518 return;
519 }
520
521 // RINEX Format
522 // ------------
523 int fieldLen = 19;
524
525 int pos[4];
526 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
527 pos[1] = pos[0] + fieldLen;
528 pos[2] = pos[1] + fieldLen;
529 pos[3] = pos[2] + fieldLen;
530
531 // Read four lines
532 // ---------------
533 for (int iLine = 0; iLine < nLines; iLine++) {
534 QString line = lines[iLine];
535
536 if ( iLine == 0 ) {
537 QTextStream in(line.left(pos[1]).toLatin1());
538
539 int year, month, day, hour, min;
540 double sec;
541
542 QString prnStr, n;
543 in >> prnStr;
544 if (prnStr.size() == 1 && prnStr[0] == 'R') {
545 in >> n;
546 prnStr.append(n);
547 }
548 in >> year >> month >> day >> hour >> min >> sec;
549 if (prnStr.at(0) == 'R') {
550 _prn.set('R', prnStr.mid(1).toInt());
551 }
552 else {
553 _prn.set('R', prnStr.toInt());
554 }
555
556 if (year < 80) {
557 year += 2000;
558 }
559 else if (year < 100) {
560 year += 1900;
561 }
562
563 _gps_utc = gnumleap(year, month, day);
564
565 _TOC.set(year, month, day, hour, min, sec);
566 _TOC = _TOC + _gps_utc;
567 int nd = int((_TOC.gpssec())) / (24.0*60.0*60.0);
568 if ( readDbl(line, pos[1], fieldLen, _tau ) ||
569 readDbl(line, pos[2], fieldLen, _gamma) ||
570 readDbl(line, pos[3], fieldLen, _tki ) ) {
571 _checkState = bad;
572 return;
573 }
574 _tki -= nd * 86400.0;
575 _tau = -_tau;
576 }
577
578 else if ( iLine == 1 ) {
579 if ( readDbl(line, pos[0], fieldLen, _x_pos ) ||
580 readDbl(line, pos[1], fieldLen, _x_velocity ) ||
581 readDbl(line, pos[2], fieldLen, _x_acceleration) ||
582 readDbl(line, pos[3], fieldLen, _health ) ) {
583 _checkState = bad;
584 return;
585 }
586 }
587
588 else if ( iLine == 2 ) {
589 if ( readDbl(line, pos[0], fieldLen, _y_pos ) ||
590 readDbl(line, pos[1], fieldLen, _y_velocity ) ||
591 readDbl(line, pos[2], fieldLen, _y_acceleration ) ||
592 readDbl(line, pos[3], fieldLen, _frequency_number) ) {
593 _checkState = bad;
594 return;
595 }
596 }
597
598 else if ( iLine == 3 ) {
599 if ( readDbl(line, pos[0], fieldLen, _z_pos ) ||
600 readDbl(line, pos[1], fieldLen, _z_velocity ) ||
601 readDbl(line, pos[2], fieldLen, _z_acceleration) ||
602 readDbl(line, pos[3], fieldLen, _E ) ) {
603 _checkState = bad;
604 return;
605 }
606 }
607 }
608
609 // Initialize status vector
610 // ------------------------
611 _tt = _TOC;
612 _xv.ReSize(6); _xv = 0.0;
613 _xv(1) = _x_pos * 1.e3;
614 _xv(2) = _y_pos * 1.e3;
615 _xv(3) = _z_pos * 1.e3;
616 _xv(4) = _x_velocity * 1.e3;
617 _xv(5) = _y_velocity * 1.e3;
618 _xv(6) = _z_velocity * 1.e3;
619}
620
621// Compute Glonass Satellite Position (virtual)
622////////////////////////////////////////////////////////////////////////////
623t_irc t_ephGlo::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
624
625 static const double nominalStep = 10.0;
626
627 memset(xc, 0, 6*sizeof(double));
628 memset(vv, 0, 3*sizeof(double));
629
630 double dtPos = bncTime(GPSweek, GPSweeks) - _tt;
631
632 if (fabs(dtPos) > 24 * 3600.0) {
633 return failure;
634 }
635
636 int nSteps = int(fabs(dtPos) / nominalStep) + 1;
637 double step = dtPos / nSteps;
638
639 double acc[3];
640 acc[0] = _x_acceleration * 1.e3;
641 acc[1] = _y_acceleration * 1.e3;
642 acc[2] = _z_acceleration * 1.e3;
643
644 for (int ii = 1; ii <= nSteps; ii++) {
645 _xv = rungeKutta4(_tt.gpssec(), _xv, step, acc, glo_deriv);
646 _tt = _tt + step;
647 }
648
649 // Position and Velocity
650 // ---------------------
651 xc[0] = _xv(1);
652 xc[1] = _xv(2);
653 xc[2] = _xv(3);
654
655 vv[0] = _xv(4);
656 vv[1] = _xv(5);
657 vv[2] = _xv(6);
658
659 // Clock Correction
660 // ----------------
661 double dtClk = bncTime(GPSweek, GPSweeks) - _TOC;
662 xc[3] = -_tau + _gamma * dtClk;
663
664 xc[4] = _gamma;
665 xc[5] = 0.0;
666
667 return success;
668}
669
670// RINEX Format String
671//////////////////////////////////////////////////////////////////////////////
672QString t_ephGlo::toString(double version) const {
673
674 QString rnxStr = rinexDateStr(_TOC -_gps_utc, _prn, version);
675 int nd = int((_TOC - _gps_utc).gpssec()) / (24.0*60.0*60.0);
676 QTextStream out(&rnxStr);
677
678 out << QString("%1%2%3\n")
679 .arg(-_tau, 19, 'e', 12)
680 .arg(_gamma, 19, 'e', 12)
681 .arg(_tki+nd*86400.0, 19, 'e', 12);
682
683 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
684
685 out << QString(fmt)
686 .arg(_x_pos, 19, 'e', 12)
687 .arg(_x_velocity, 19, 'e', 12)
688 .arg(_x_acceleration, 19, 'e', 12)
689 .arg(_health, 19, 'e', 12);
690
691 out << QString(fmt)
692 .arg(_y_pos, 19, 'e', 12)
693 .arg(_y_velocity, 19, 'e', 12)
694 .arg(_y_acceleration, 19, 'e', 12)
695 .arg(_frequency_number, 19, 'e', 12);
696
697 out << QString(fmt)
698 .arg(_z_pos, 19, 'e', 12)
699 .arg(_z_velocity, 19, 'e', 12)
700 .arg(_z_acceleration, 19, 'e', 12)
701 .arg(_E, 19, 'e', 12);
702
703 return rnxStr;
704}
705
706// Derivative of the state vector using a simple force model (static)
707////////////////////////////////////////////////////////////////////////////
708ColumnVector t_ephGlo::glo_deriv(double /* tt */, const ColumnVector& xv,
709 double* acc) {
710
711 // State vector components
712 // -----------------------
713 ColumnVector rr = xv.rows(1,3);
714 ColumnVector vv = xv.rows(4,6);
715
716 // Acceleration
717 // ------------
718 static const double gmWGS = 398.60044e12;
719 static const double AE = 6378136.0;
720 static const double OMEGA = 7292115.e-11;
721 static const double C20 = -1082.6257e-6;
722
723 double rho = rr.NormFrobenius();
724 double t1 = -gmWGS/(rho*rho*rho);
725 double t2 = 3.0/2.0 * C20 * (gmWGS*AE*AE) / (rho*rho*rho*rho*rho);
726 double t3 = OMEGA * OMEGA;
727 double t4 = 2.0 * OMEGA;
728 double z2 = rr(3) * rr(3);
729
730 // Vector of derivatives
731 // ---------------------
732 ColumnVector va(6);
733 va(1) = vv(1);
734 va(2) = vv(2);
735 va(3) = vv(3);
736 va(4) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2) + acc[0];
737 va(5) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1) + acc[1];
738 va(6) = (t1 + t2*(3.0-5.0*z2/(rho*rho)) ) * rr(3) + acc[2];
739
740 return va;
741}
742
743// IOD of Glonass Ephemeris (virtual)
744////////////////////////////////////////////////////////////////////////////
745unsigned int t_ephGlo::IOD() const {
746 bncTime tMoscow = _TOC - _gps_utc + 3 * 3600.0;
747 return (unsigned long)tMoscow.daysec() / 900;
748}
749
750// Health status of Glonass Ephemeris (virtual)
751////////////////////////////////////////////////////////////////////////////
752unsigned int t_ephGlo::isUnhealthy() const {
753
754 if (_almanac_health_availablility_indicator) {
755 if ((_health == 0 && _almanac_health == 0) ||
756 (_health == 1 && _almanac_health == 0) ||
757 (_health == 1 && _almanac_health == 1)) {
758 return 1;
759 }
760 }
761 else if (!_almanac_health_availablility_indicator) {
762 if (_health) {
763 return 1;
764 }
765 }
766 return 0; /* (_health == 0 && _almanac_health == 1) or (_health == 0) */
767}
768
769// Constructor
770//////////////////////////////////////////////////////////////////////////////
771t_ephGal::t_ephGal(float rnxVersion, const QStringList& lines) {
772 int year, month, day, hour, min;
773 double sec;
774 QString prnStr;
775 const int nLines = 8;
776 if (lines.size() != nLines) {
777 _checkState = bad;
778 return;
779 }
780
781 // RINEX Format
782 // ------------
783 int fieldLen = 19;
784 double SVhealth = 0.0;
785 double datasource = 0.0;
786
787 int pos[4];
788 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
789 pos[1] = pos[0] + fieldLen;
790 pos[2] = pos[1] + fieldLen;
791 pos[3] = pos[2] + fieldLen;
792
793 // Read eight lines
794 // ----------------
795 for (int iLine = 0; iLine < nLines; iLine++) {
796 QString line = lines[iLine];
797
798 if ( iLine == 0 ) {
799 QTextStream in(line.left(pos[1]).toLatin1());
800 QString n;
801 in >> prnStr;
802 if (prnStr.size() == 1 && prnStr[0] == 'E') {
803 in >> n;
804 prnStr.append(n);
805 }
806 in >> year >> month >> day >> hour >> min >> sec;
807 if (year < 80) {
808 year += 2000;
809 }
810 else if (year < 100) {
811 year += 1900;
812 }
813
814 _TOC.set(year, month, day, hour, min, sec);
815
816 if ( readDbl(line, pos[1], fieldLen, _clock_bias ) ||
817 readDbl(line, pos[2], fieldLen, _clock_drift ) ||
818 readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
819 _checkState = bad;
820 return;
821 }
822 }
823
824 else if ( iLine == 1 ) {
825 if ( readDbl(line, pos[0], fieldLen, _IODnav ) ||
826 readDbl(line, pos[1], fieldLen, _Crs ) ||
827 readDbl(line, pos[2], fieldLen, _Delta_n) ||
828 readDbl(line, pos[3], fieldLen, _M0 ) ) {
829 _checkState = bad;
830 return;
831 }
832 }
833
834 else if ( iLine == 2 ) {
835 if ( readDbl(line, pos[0], fieldLen, _Cuc ) ||
836 readDbl(line, pos[1], fieldLen, _e ) ||
837 readDbl(line, pos[2], fieldLen, _Cus ) ||
838 readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
839 _checkState = bad;
840 return;
841 }
842 }
843
844 else if ( iLine == 3 ) {
845 if ( readDbl(line, pos[0], fieldLen, _TOEsec) ||
846 readDbl(line, pos[1], fieldLen, _Cic ) ||
847 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
848 readDbl(line, pos[3], fieldLen, _Cis ) ) {
849 _checkState = bad;
850 return;
851 }
852 }
853
854 else if ( iLine == 4 ) {
855 if ( readDbl(line, pos[0], fieldLen, _i0 ) ||
856 readDbl(line, pos[1], fieldLen, _Crc ) ||
857 readDbl(line, pos[2], fieldLen, _omega ) ||
858 readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
859 _checkState = bad;
860 return;
861 }
862 }
863
864 else if ( iLine == 5 ) {
865 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
866 readDbl(line, pos[1], fieldLen, datasource) ||
867 readDbl(line, pos[2], fieldLen, _TOEweek ) ) {
868 _checkState = bad;
869 return;
870 } else {
871 if (int(datasource) & (1<<8)) {
872 _fnav = true;
873 _inav = false;
874 } else if (int(datasource) & (1<<9)) {
875 _fnav = false;
876 _inav = true;
877 }
878 _TOEweek -= 1024.0;
879 }
880 }
881
882 else if ( iLine == 6 ) {
883 if ( readDbl(line, pos[0], fieldLen, _SISA ) ||
884 readDbl(line, pos[1], fieldLen, SVhealth) ||
885 readDbl(line, pos[2], fieldLen, _BGD_1_5A) ||
886 readDbl(line, pos[3], fieldLen, _BGD_1_5B) ) {
887 _checkState = bad;
888 return;
889 } else {
890 // Bit 0
891 _e1DataInValid = (int(SVhealth) & (1<<0));
892 // Bit 1-2
893 _E1_bHS = double((int(SVhealth) >> 1) & 0x3);
894 // Bit 3
895 _e5aDataInValid = (int(SVhealth) & (1<<3));
896 // Bit 4-5
897 _E5aHS = double((int(SVhealth) >> 4) & 0x3);
898 // Bit 6
899 _e5bDataInValid = (int(SVhealth) & (1<<6));
900 // Bit 7-8
901 _E5bHS = double((int(SVhealth) >> 7) & 0x3);
902
903 if (prnStr.at(0) == 'E') {
904 _prn.set('E', prnStr.mid(1).toInt(), _inav ? 1 : 0);
905 }
906 }
907 }
908
909 else if ( iLine == 7 ) {
910 if ( readDbl(line, pos[0], fieldLen, _TOT) ) {
911 _checkState = bad;
912 return;
913 }
914 }
915 }
916}
917
918// Compute Galileo Satellite Position (virtual)
919////////////////////////////////////////////////////////////////////////////
920t_irc t_ephGal::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
921
922 static const double omegaEarth = 7292115.1467e-11;
923 static const double gmWGS = 398.6004418e12;
924
925 memset(xc, 0, 6*sizeof(double));
926 memset(vv, 0, 3*sizeof(double));
927
928 double a0 = _sqrt_A * _sqrt_A;
929 if (a0 == 0) {
930 return failure;
931 }
932
933 double n0 = sqrt(gmWGS/(a0*a0*a0));
934
935 bncTime tt(GPSweek, GPSweeks);
936 double tk = tt - bncTime(_TOC.gpsw(), _TOEsec);
937
938 double n = n0 + _Delta_n;
939 double M = _M0 + n*tk;
940 double E = M;
941 double E_last;
942 int nLoop = 0;
943 do {
944 E_last = E;
945 E = M + _e*sin(E);
946
947 if (++nLoop == 100) {
948 return failure;
949 }
950 } while ( fabs(E-E_last)*a0 > 0.001 );
951 double v = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) );
952 double u0 = v + _omega;
953 double sin2u0 = sin(2*u0);
954 double cos2u0 = cos(2*u0);
955 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
956 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
957 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
958 double xp = r*cos(u);
959 double yp = r*sin(u);
960 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
961 omegaEarth*_TOEsec;
962
963 double sinom = sin(OM);
964 double cosom = cos(OM);
965 double sini = sin(i);
966 double cosi = cos(i);
967 xc[0] = xp*cosom - yp*cosi*sinom;
968 xc[1] = xp*sinom + yp*cosi*cosom;
969 xc[2] = yp*sini;
970
971 double tc = tt - _TOC;
972 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
973
974 // Velocity
975 // --------
976 double tanv2 = tan(v/2);
977 double dEdM = 1 / (1 - _e*cos(E));
978 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
979 * dEdM * n;
980 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
981 double dotom = _OMEGADOT - omegaEarth;
982 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
983 double dotr = a0 * _e*sin(E) * dEdM * n
984 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
985 double dotx = dotr*cos(u) - r*sin(u)*dotu;
986 double doty = dotr*sin(u) + r*cos(u)*dotu;
987
988 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
989 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
990 + yp*sini*sinom*doti; // dX / di
991
992 vv[1] = sinom *dotx + cosi*cosom *doty
993 + xp*cosom*dotom - yp*cosi*sinom*dotom
994 - yp*sini*cosom*doti;
995
996 vv[2] = sini *doty + yp*cosi *doti;
997
998 // Relativistic Correction
999 // -----------------------
1000 // correspondent to Galileo ICD and to SSR standard
1001 xc[3] -= 4.442807633e-10 * _e * sqrt(a0) *sin(E);
1002 // correspondent to IGS convention
1003 //xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c;
1004
1005 xc[4] = _clock_drift + _clock_driftrate*tc;
1006 xc[5] = _clock_driftrate;
1007
1008 return success;
1009}
1010
1011// Health status of Galileo Ephemeris (virtual)
1012////////////////////////////////////////////////////////////////////////////
1013unsigned int t_ephGal::isUnhealthy() const {
1014 if (_E5aHS && _E5bHS && _E1_bHS) {
1015 return 1;
1016 }
1017 return 0;
1018}
1019
1020// RINEX Format String
1021//////////////////////////////////////////////////////////////////////////////
1022QString t_ephGal::toString(double version) const {
1023
1024 QString rnxStr = rinexDateStr(_TOC, _prn, version);
1025
1026 QTextStream out(&rnxStr);
1027
1028 out << QString("%1%2%3\n")
1029 .arg(_clock_bias, 19, 'e', 12)
1030 .arg(_clock_drift, 19, 'e', 12)
1031 .arg(_clock_driftrate, 19, 'e', 12);
1032
1033 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1034
1035 out << QString(fmt)
1036 .arg(_IODnav, 19, 'e', 12)
1037 .arg(_Crs, 19, 'e', 12)
1038 .arg(_Delta_n, 19, 'e', 12)
1039 .arg(_M0, 19, 'e', 12);
1040
1041 out << QString(fmt)
1042 .arg(_Cuc, 19, 'e', 12)
1043 .arg(_e, 19, 'e', 12)
1044 .arg(_Cus, 19, 'e', 12)
1045 .arg(_sqrt_A, 19, 'e', 12);
1046
1047 out << QString(fmt)
1048 .arg(_TOEsec, 19, 'e', 12)
1049 .arg(_Cic, 19, 'e', 12)
1050 .arg(_OMEGA0, 19, 'e', 12)
1051 .arg(_Cis, 19, 'e', 12);
1052
1053 out << QString(fmt)
1054 .arg(_i0, 19, 'e', 12)
1055 .arg(_Crc, 19, 'e', 12)
1056 .arg(_omega, 19, 'e', 12)
1057 .arg(_OMEGADOT, 19, 'e', 12);
1058
1059 int dataSource = 0;
1060 int SVhealth = 0;
1061 double BGD_1_5A = _BGD_1_5A;
1062 double BGD_1_5B = _BGD_1_5B;
1063 if (_fnav) {
1064 dataSource |= (1<<1);
1065 dataSource |= (1<<8);
1066 BGD_1_5B = 0.0;
1067 // SVhealth
1068 // Bit 3 : E5a DVS
1069 if (_e5aDataInValid) {
1070 SVhealth |= (1<<3);
1071 }
1072 // Bit 4-5: E5a HS
1073 if (_E5aHS == 1.0) {
1074 SVhealth |= (1<<4);
1075 }
1076 else if (_E5aHS == 2.0) {
1077 SVhealth |= (1<<5);
1078 }
1079 else if (_E5aHS == 3.0) {
1080 SVhealth |= (1<<4);
1081 SVhealth |= (1<<5);
1082 }
1083 }
1084 else if(_inav) {
1085 // Bit 2 and 0 are set because from MT1046 the data source cannot be determined
1086 // and RNXv3.03 says both can be set if the navigation messages were merged
1087 dataSource |= (1<<0);
1088 dataSource |= (1<<2);
1089 dataSource |= (1<<9);
1090 // SVhealth
1091 // Bit 0 : E1-B DVS
1092 if (_e1DataInValid) {
1093 SVhealth |= (1<<0);
1094 }
1095 // Bit 1-2: E1-B HS
1096 if (_E1_bHS == 1.0) {
1097 SVhealth |= (1<<1);
1098 }
1099 else if (_E1_bHS == 2.0) {
1100 SVhealth |= (1<<2);
1101 }
1102 else if (_E1_bHS == 3.0) {
1103 SVhealth |= (1<<1);
1104 SVhealth |= (1<<2);
1105 }
1106 // Bit 3 : E5a DVS
1107 if (_e5aDataInValid) {
1108 SVhealth |= (1<<3);
1109 }
1110 // Bit 4-5: E5a HS
1111 if (_E5aHS == 1.0) {
1112 SVhealth |= (1<<4);
1113 }
1114 else if (_E5aHS == 2.0) {
1115 SVhealth |= (1<<5);
1116 }
1117 else if (_E5aHS == 3.0) {
1118 SVhealth |= (1<<4);
1119 SVhealth |= (1<<5);
1120 }
1121 // Bit 6 : E5b DVS
1122 if (_e5bDataInValid) {
1123 SVhealth |= (1<<6);
1124 }
1125 // Bit 7-8: E5b HS
1126 if (_E5bHS == 1.0) {
1127 SVhealth |= (1<<7);
1128 }
1129 else if (_E5bHS == 2.0) {
1130 SVhealth |= (1<<8);
1131 }
1132 else if (_E5bHS == 3.0) {
1133 SVhealth |= (1<<7);
1134 SVhealth |= (1<<8);
1135 }
1136 }
1137
1138 out << QString(fmt)
1139 .arg(_IDOT, 19, 'e', 12)
1140 .arg(double(dataSource), 19, 'e', 12)
1141 .arg(_TOEweek + 1024.0, 19, 'e', 12)
1142 .arg(0.0, 19, 'e', 12);
1143
1144 out << QString(fmt)
1145 .arg(_SISA, 19, 'e', 12)
1146 .arg(double(SVhealth), 19, 'e', 12)
1147 .arg(BGD_1_5A, 19, 'e', 12)
1148 .arg(BGD_1_5B, 19, 'e', 12);
1149
1150 double tot = _TOT;
1151 if (tot == 0.9999e9 && version < 3.0) {
1152 tot = 0.0;
1153 }
1154 out << QString(fmt)
1155 .arg(tot, 19, 'e', 12)
1156 .arg("", 19, QChar(' '))
1157 .arg("", 19, QChar(' '))
1158 .arg("", 19, QChar(' '));
1159
1160 return rnxStr;
1161}
1162
1163// Constructor
1164//////////////////////////////////////////////////////////////////////////////
1165t_ephSBAS::t_ephSBAS(float rnxVersion, const QStringList& lines) {
1166
1167 const int nLines = 4;
1168
1169 if (lines.size() != nLines) {
1170 _checkState = bad;
1171 return;
1172 }
1173
1174 // RINEX Format
1175 // ------------
1176 int fieldLen = 19;
1177
1178 int pos[4];
1179 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
1180 pos[1] = pos[0] + fieldLen;
1181 pos[2] = pos[1] + fieldLen;
1182 pos[3] = pos[2] + fieldLen;
1183
1184 // Read four lines
1185 // ---------------
1186 for (int iLine = 0; iLine < nLines; iLine++) {
1187 QString line = lines[iLine];
1188
1189 if ( iLine == 0 ) {
1190 QTextStream in(line.left(pos[1]).toLatin1());
1191
1192 int year, month, day, hour, min;
1193 double sec;
1194
1195 QString prnStr, n;
1196 in >> prnStr;
1197 if (prnStr.size() == 1 && prnStr[0] == 'S') {
1198 in >> n;
1199 prnStr.append(n);
1200 }
1201 in >> year >> month >> day >> hour >> min >> sec;
1202 if (prnStr.at(0) == 'S') {
1203 _prn.set('S', prnStr.mid(1).toInt());
1204 }
1205 else {
1206 _prn.set('S', prnStr.toInt());
1207 }
1208
1209 if (year < 80) {
1210 year += 2000;
1211 }
1212 else if (year < 100) {
1213 year += 1900;
1214 }
1215
1216 _TOC.set(year, month, day, hour, min, sec);
1217
1218 if ( readDbl(line, pos[1], fieldLen, _agf0 ) ||
1219 readDbl(line, pos[2], fieldLen, _agf1 ) ||
1220 readDbl(line, pos[3], fieldLen, _TOT ) ) {
1221 _checkState = bad;
1222 return;
1223 }
1224 }
1225
1226 else if ( iLine == 1 ) {
1227 if ( readDbl(line, pos[0], fieldLen, _x_pos ) ||
1228 readDbl(line, pos[1], fieldLen, _x_velocity ) ||
1229 readDbl(line, pos[2], fieldLen, _x_acceleration) ||
1230 readDbl(line, pos[3], fieldLen, _health ) ) {
1231 _checkState = bad;
1232 return;
1233 }
1234 }
1235
1236 else if ( iLine == 2 ) {
1237 if ( readDbl(line, pos[0], fieldLen, _y_pos ) ||
1238 readDbl(line, pos[1], fieldLen, _y_velocity ) ||
1239 readDbl(line, pos[2], fieldLen, _y_acceleration ) ||
1240 readDbl(line, pos[3], fieldLen, _ura ) ) {
1241 _checkState = bad;
1242 return;
1243 }
1244 }
1245
1246 else if ( iLine == 3 ) {
1247 double iodn;
1248 if ( readDbl(line, pos[0], fieldLen, _z_pos ) ||
1249 readDbl(line, pos[1], fieldLen, _z_velocity ) ||
1250 readDbl(line, pos[2], fieldLen, _z_acceleration) ||
1251 readDbl(line, pos[3], fieldLen, iodn ) ) {
1252 _checkState = bad;
1253 return;
1254 } else {
1255 _IODN = int(iodn);
1256 }
1257 }
1258 }
1259
1260 _x_pos *= 1.e3;
1261 _y_pos *= 1.e3;
1262 _z_pos *= 1.e3;
1263 _x_velocity *= 1.e3;
1264 _y_velocity *= 1.e3;
1265 _z_velocity *= 1.e3;
1266 _x_acceleration *= 1.e3;
1267 _y_acceleration *= 1.e3;
1268 _z_acceleration *= 1.e3;
1269}
1270
1271// IOD of SBAS Ephemeris (virtual)
1272////////////////////////////////////////////////////////////////////////////
1273
1274unsigned int t_ephSBAS::IOD() const {
1275 unsigned char buffer[80];
1276 int size = 0;
1277 int numbits = 0;
1278 long long bitbuffer = 0;
1279 unsigned char *startbuffer = buffer;
1280
1281 SBASADDBITSFLOAT(30, this->_x_pos, 0.08)
1282 SBASADDBITSFLOAT(30, this->_y_pos, 0.08)
1283 SBASADDBITSFLOAT(25, this->_z_pos, 0.4)
1284 SBASADDBITSFLOAT(17, this->_x_velocity, 0.000625)
1285 SBASADDBITSFLOAT(17, this->_y_velocity, 0.000625)
1286 SBASADDBITSFLOAT(18, this->_z_velocity, 0.004)
1287 SBASADDBITSFLOAT(10, this->_x_acceleration, 0.0000125)
1288 SBASADDBITSFLOAT(10, this->_y_acceleration, 0.0000125)
1289 SBASADDBITSFLOAT(10, this->_z_acceleration, 0.0000625)
1290 SBASADDBITSFLOAT(12, this->_agf0, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
1291 SBASADDBITSFLOAT(8, this->_agf1, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<10))
1292 SBASADDBITS(5,0); // the last byte is filled by 0-bits to obtain a length of an integer multiple of 8
1293
1294 return CRC24(size, startbuffer);
1295}
1296
1297// Compute SBAS Satellite Position (virtual)
1298////////////////////////////////////////////////////////////////////////////
1299t_irc t_ephSBAS::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
1300
1301 bncTime tt(GPSweek, GPSweeks);
1302 double dt = tt - _TOC;
1303
1304 xc[0] = _x_pos + _x_velocity * dt + _x_acceleration * dt * dt / 2.0;
1305 xc[1] = _y_pos + _y_velocity * dt + _y_acceleration * dt * dt / 2.0;
1306 xc[2] = _z_pos + _z_velocity * dt + _z_acceleration * dt * dt / 2.0;
1307
1308 vv[0] = _x_velocity + _x_acceleration * dt;
1309 vv[1] = _y_velocity + _y_acceleration * dt;
1310 vv[2] = _z_velocity + _z_acceleration * dt;
1311
1312 xc[3] = _agf0 + _agf1 * dt;
1313
1314 xc[4] = _agf1;
1315 xc[5] = 0.0;
1316
1317 return success;
1318}
1319
1320// RINEX Format String
1321//////////////////////////////////////////////////////////////////////////////
1322QString t_ephSBAS::toString(double version) const {
1323
1324 QString rnxStr = rinexDateStr(_TOC, _prn, version);
1325
1326 QTextStream out(&rnxStr);
1327
1328 out << QString("%1%2%3\n")
1329 .arg(_agf0, 19, 'e', 12)
1330 .arg(_agf1, 19, 'e', 12)
1331 .arg(_TOT, 19, 'e', 12);
1332
1333 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1334
1335 out << QString(fmt)
1336 .arg(1.e-3*_x_pos, 19, 'e', 12)
1337 .arg(1.e-3*_x_velocity, 19, 'e', 12)
1338 .arg(1.e-3*_x_acceleration, 19, 'e', 12)
1339 .arg(_health, 19, 'e', 12);
1340
1341 out << QString(fmt)
1342 .arg(1.e-3*_y_pos, 19, 'e', 12)
1343 .arg(1.e-3*_y_velocity, 19, 'e', 12)
1344 .arg(1.e-3*_y_acceleration, 19, 'e', 12)
1345 .arg(_ura, 19, 'e', 12);
1346
1347 out << QString(fmt)
1348 .arg(1.e-3*_z_pos, 19, 'e', 12)
1349 .arg(1.e-3*_z_velocity, 19, 'e', 12)
1350 .arg(1.e-3*_z_acceleration, 19, 'e', 12)
1351 .arg(double(_IODN), 19, 'e', 12);
1352
1353 return rnxStr;
1354}
1355
1356// Constructor
1357//////////////////////////////////////////////////////////////////////////////
1358t_ephBDS::t_ephBDS(float rnxVersion, const QStringList& lines) {
1359
1360 const int nLines = 8;
1361
1362 if (lines.size() != nLines) {
1363 _checkState = bad;
1364 return;
1365 }
1366
1367 // RINEX Format
1368 // ------------
1369 int fieldLen = 19;
1370
1371 int pos[4];
1372 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
1373 pos[1] = pos[0] + fieldLen;
1374 pos[2] = pos[1] + fieldLen;
1375 pos[3] = pos[2] + fieldLen;
1376
1377 // Read eight lines
1378 // ----------------
1379 for (int iLine = 0; iLine < nLines; iLine++) {
1380 QString line = lines[iLine];
1381
1382 if ( iLine == 0 ) {
1383 QTextStream in(line.left(pos[1]).toLatin1());
1384
1385 int year, month, day, hour, min;
1386 double sec;
1387
1388 QString prnStr, n;
1389 in >> prnStr;
1390 if (prnStr.size() == 1 && prnStr[0] == 'C') {
1391 in >> n;
1392 prnStr.append(n);
1393 }
1394 in >> year >> month >> day >> hour >> min >> sec;
1395 if (prnStr.at(0) == 'C') {
1396 _prn.set('C', prnStr.mid(1).toInt());
1397 }
1398 else {
1399 _prn.set('C', prnStr.toInt());
1400 }
1401
1402 if (year < 80) {
1403 year += 2000;
1404 }
1405 else if (year < 100) {
1406 year += 1900;
1407 }
1408
1409 _TOC.setBDS(year, month, day, hour, min, sec);
1410
1411 if ( readDbl(line, pos[1], fieldLen, _clock_bias ) ||
1412 readDbl(line, pos[2], fieldLen, _clock_drift ) ||
1413 readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
1414 _checkState = bad;
1415 return;
1416 }
1417 }
1418
1419 else if ( iLine == 1 ) {
1420 double aode;
1421 if ( readDbl(line, pos[0], fieldLen, aode ) ||
1422 readDbl(line, pos[1], fieldLen, _Crs ) ||
1423 readDbl(line, pos[2], fieldLen, _Delta_n) ||
1424 readDbl(line, pos[3], fieldLen, _M0 ) ) {
1425 _checkState = bad;
1426 return;
1427 }
1428 _AODE = int(aode);
1429 }
1430
1431 else if ( iLine == 2 ) {
1432 if ( readDbl(line, pos[0], fieldLen, _Cuc ) ||
1433 readDbl(line, pos[1], fieldLen, _e ) ||
1434 readDbl(line, pos[2], fieldLen, _Cus ) ||
1435 readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
1436 _checkState = bad;
1437 return;
1438 }
1439 }
1440
1441 else if ( iLine == 3 ) {
1442 if ( readDbl(line, pos[0], fieldLen, _TOEsec ) ||
1443 readDbl(line, pos[1], fieldLen, _Cic ) ||
1444 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
1445 readDbl(line, pos[3], fieldLen, _Cis ) ) {
1446 _checkState = bad;
1447 return;
1448 }
1449 }
1450
1451 else if ( iLine == 4 ) {
1452 if ( readDbl(line, pos[0], fieldLen, _i0 ) ||
1453 readDbl(line, pos[1], fieldLen, _Crc ) ||
1454 readDbl(line, pos[2], fieldLen, _omega ) ||
1455 readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
1456 _checkState = bad;
1457 return;
1458 }
1459 }
1460
1461 else if ( iLine == 5 ) {
1462 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
1463 readDbl(line, pos[2], fieldLen, _TOEweek)) {
1464 _checkState = bad;
1465 return;
1466 }
1467 }
1468
1469 else if ( iLine == 6 ) {
1470 double SatH1;
1471 if ( readDbl(line, pos[0], fieldLen, _URA ) ||
1472 readDbl(line, pos[1], fieldLen, SatH1) ||
1473 readDbl(line, pos[2], fieldLen, _TGD1) ||
1474 readDbl(line, pos[3], fieldLen, _TGD2) ) {
1475 _checkState = bad;
1476 return;
1477 }
1478 _SatH1 = int(SatH1);
1479 }
1480
1481 else if ( iLine == 7 ) {
1482 double aodc;
1483 if ( readDbl(line, pos[0], fieldLen, _TOT) ||
1484 readDbl(line, pos[1], fieldLen, aodc) ) {
1485 _checkState = bad;
1486 return;
1487 }
1488 if (_TOT == 0.9999e9) { // 0.9999e9 means not known (RINEX standard)
1489 _TOT = _TOEsec;
1490 }
1491 _AODC = int(aodc);
1492 }
1493 }
1494
1495 _TOE.setBDS(int(_TOEweek), _TOEsec);
1496
1497 // remark: actually should be computed from second_tot
1498 // but it seems to be unreliable in RINEX files
1499 //_TOT = _TOC.bdssec();
1500}
1501
1502// IOD of BDS Ephemeris (virtual)
1503////////////////////////////////////////////////////////////////////////////
1504unsigned int t_ephBDS::IOD() const {
1505 return (int(_TOEsec)/720) % 240;
1506}
1507
1508// Compute BDS Satellite Position (virtual)
1509//////////////////////////////////////////////////////////////////////////////
1510t_irc t_ephBDS::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
1511
1512 static const double gmBDS = 398.6004418e12;
1513 static const double omegaBDS = 7292115.0000e-11;
1514
1515 xc[0] = xc[1] = xc[2] = xc[3] = 0.0;
1516 vv[0] = vv[1] = vv[2] = 0.0;
1517
1518 bncTime tt(GPSweek, GPSweeks);
1519
1520 if (_sqrt_A == 0) {
1521 return failure;
1522 }
1523 double a0 = _sqrt_A * _sqrt_A;
1524
1525 double n0 = sqrt(gmBDS/(a0*a0*a0));
1526 double tk = tt - _TOE;
1527 double n = n0 + _Delta_n;
1528 double M = _M0 + n*tk;
1529 double E = M;
1530 double E_last;
1531 int nLoop = 0;
1532 do {
1533 E_last = E;
1534 E = M + _e*sin(E);
1535
1536 if (++nLoop == 100) {
1537 return failure;
1538 }
1539 } while ( fabs(E-E_last)*a0 > 0.001 );
1540
1541 double v = atan2(sqrt(1-_e*_e) * sin(E), cos(E) - _e);
1542 double u0 = v + _omega;
1543 double sin2u0 = sin(2*u0);
1544 double cos2u0 = cos(2*u0);
1545 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
1546 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
1547 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
1548 double xp = r*cos(u);
1549 double yp = r*sin(u);
1550 double toesec = (_TOE.gpssec() - 14.0);
1551 double sinom = 0;
1552 double cosom = 0;
1553 double sini = 0;
1554 double cosi = 0;
1555
1556 // Velocity
1557 // --------
1558 double tanv2 = tan(v/2);
1559 double dEdM = 1 / (1 - _e*cos(E));
1560 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2)
1561 / (1 + tanv2*tanv2) * dEdM * n;
1562 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
1563 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
1564 double dotr = a0 * _e*sin(E) * dEdM * n
1565 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
1566
1567 double dotx = dotr*cos(u) - r*sin(u)*dotu;
1568 double doty = dotr*sin(u) + r*cos(u)*dotu;
1569
1570 const double iMaxGEO = 10.0 / 180.0 * M_PI;
1571
1572 // MEO/IGSO satellite
1573 // ------------------
1574 if (_i0 > iMaxGEO) {
1575 double OM = _OMEGA0 + (_OMEGADOT - omegaBDS)*tk - omegaBDS*toesec;
1576
1577 sinom = sin(OM);
1578 cosom = cos(OM);
1579 sini = sin(i);
1580 cosi = cos(i);
1581
1582 xc[0] = xp*cosom - yp*cosi*sinom;
1583 xc[1] = xp*sinom + yp*cosi*cosom;
1584 xc[2] = yp*sini;
1585
1586 // Velocity
1587 // --------
1588
1589 double dotom = _OMEGADOT - t_CST::omega;
1590
1591 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
1592 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
1593 + yp*sini*sinom*doti; // dX / di
1594
1595 vv[1] = sinom *dotx + cosi*cosom *doty
1596 + xp*cosom*dotom - yp*cosi*sinom*dotom
1597 - yp*sini*cosom*doti;
1598
1599 vv[2] = sini *doty + yp*cosi *doti;
1600
1601 }
1602
1603 // GEO satellite
1604 // -------------
1605 else {
1606 double OM = _OMEGA0 + _OMEGADOT*tk - omegaBDS*toesec;
1607 double ll = omegaBDS*tk;
1608
1609 sinom = sin(OM);
1610 cosom = cos(OM);
1611 sini = sin(i);
1612 cosi = cos(i);
1613
1614 double xx = xp*cosom - yp*cosi*sinom;
1615 double yy = xp*sinom + yp*cosi*cosom;
1616 double zz = yp*sini;
1617
1618 Matrix RX = BNC_PPP::t_astro::rotX(-5.0 / 180.0 * M_PI);
1619 Matrix RZ = BNC_PPP::t_astro::rotZ(ll);
1620
1621 ColumnVector X1(3); X1 << xx << yy << zz;
1622 ColumnVector X2 = RZ*RX*X1;
1623
1624 xc[0] = X2(1);
1625 xc[1] = X2(2);
1626 xc[2] = X2(3);
1627
1628 double dotom = _OMEGADOT;
1629
1630 double vx = cosom *dotx - cosi*sinom *doty
1631 - xp*sinom*dotom - yp*cosi*cosom*dotom
1632 + yp*sini*sinom*doti;
1633
1634 double vy = sinom *dotx + cosi*cosom *doty
1635 + xp*cosom*dotom - yp*cosi*sinom*dotom
1636 - yp*sini*cosom*doti;
1637
1638 double vz = sini *doty + yp*cosi *doti;
1639
1640 ColumnVector V(3); V << vx << vy << vz;
1641
1642 Matrix RdotZ(3,3);
1643 double C = cos(ll);
1644 double S = sin(ll);
1645 Matrix UU(3,3);
1646 UU[0][0] = -S; UU[0][1] = +C; UU[0][2] = 0.0;
1647 UU[1][0] = -C; UU[1][1] = -S; UU[1][2] = 0.0;
1648 UU[2][0] = 0.0; UU[2][1] = 0.0; UU[2][2] = 0.0;
1649 RdotZ = omegaBDS * UU;
1650
1651 ColumnVector VV(3);
1652 VV = RZ*RX*V + RdotZ*RX*X1;
1653
1654 vv[0] = VV(1);
1655 vv[1] = VV(2);
1656 vv[2] = VV(3);
1657 }
1658
1659 double tc = tt - _TOC;
1660 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
1661
1662 // dotC = _clock_drift + _clock_driftrate*tc
1663 // - 4.442807633e-10*_e*sqrt(a0)*cos(E) * dEdM * n;
1664
1665 // Relativistic Correction
1666 // -----------------------
1667 // correspondent to BDS ICD and to SSR standard
1668 xc[3] -= 4.442807633e-10 * _e * sqrt(a0) *sin(E);
1669 // correspondent to IGS convention
1670 // xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c;
1671
1672 xc[4] = _clock_drift + _clock_driftrate*tc;
1673 xc[5] = _clock_driftrate;
1674
1675 return success;
1676}
1677
1678// RINEX Format String
1679//////////////////////////////////////////////////////////////////////////////
1680QString t_ephBDS::toString(double version) const {
1681
1682 QString rnxStr = rinexDateStr(_TOC-14.0, _prn, version);
1683
1684 QTextStream out(&rnxStr);
1685
1686 out << QString("%1%2%3\n")
1687 .arg(_clock_bias, 19, 'e', 12)
1688 .arg(_clock_drift, 19, 'e', 12)
1689 .arg(_clock_driftrate, 19, 'e', 12);
1690
1691 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1692
1693 out << QString(fmt)
1694 .arg(double(_AODE), 19, 'e', 12)
1695 .arg(_Crs, 19, 'e', 12)
1696 .arg(_Delta_n, 19, 'e', 12)
1697 .arg(_M0, 19, 'e', 12);
1698
1699 out << QString(fmt)
1700 .arg(_Cuc, 19, 'e', 12)
1701 .arg(_e, 19, 'e', 12)
1702 .arg(_Cus, 19, 'e', 12)
1703 .arg(_sqrt_A, 19, 'e', 12);
1704
1705 double toes = 0.0;
1706 if (_TOEweek > -1.0) {// RINEX input
1707 toes = _TOEsec;
1708 }
1709 else {// RTCM stream input
1710 toes = _TOE.bdssec();
1711 }
1712 out << QString(fmt)
1713 .arg(toes, 19, 'e', 12)
1714 .arg(_Cic, 19, 'e', 12)
1715 .arg(_OMEGA0, 19, 'e', 12)
1716 .arg(_Cis, 19, 'e', 12);
1717
1718 out << QString(fmt)
1719 .arg(_i0, 19, 'e', 12)
1720 .arg(_Crc, 19, 'e', 12)
1721 .arg(_omega, 19, 'e', 12)
1722 .arg(_OMEGADOT, 19, 'e', 12);
1723
1724 double toew = 0.0;
1725 if (_TOEweek > -1.0) {// RINEX input
1726 toew = _TOEweek;
1727 }
1728 else {// RTCM stream input
1729 toew = double(_TOE.bdsw());
1730 }
1731 out << QString(fmt)
1732 .arg(_IDOT, 19, 'e', 12)
1733 .arg(0.0, 19, 'e', 12)
1734 .arg(toew, 19, 'e', 12)
1735 .arg(0.0, 19, 'e', 12);
1736
1737 out << QString(fmt)
1738 .arg(_URA, 19, 'e', 12)
1739 .arg(double(_SatH1), 19, 'e', 12)
1740 .arg(_TGD1, 19, 'e', 12)
1741 .arg(_TGD2, 19, 'e', 12);
1742
1743 double tots = 0.0;
1744 if (_TOEweek > -1.0) {// RINEX input
1745 tots = _TOT;
1746 }
1747 else {// RTCM stream input
1748 tots = _TOE.bdssec();
1749 }
1750 out << QString(fmt)
1751 .arg(tots, 19, 'e', 12)
1752 .arg(double(_AODC), 19, 'e', 12)
1753 .arg("", 19, QChar(' '))
1754 .arg("", 19, QChar(' '));
1755 return rnxStr;
1756}
Note: See TracBrowser for help on using the repository browser.