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

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

GPS fit intervall: different specifications in differet RINEX versions are now considerred
File size: 47.9 KB
Line 
1#include <sstream>
2#include <iostream>
3#include <iomanip>
4#include <cstring>
5
6#include <newmatio.h>
7
8#include "ephemeris.h"
9#include "bncutils.h"
10#include "bnctime.h"
11#include "bnccore.h"
12#include "bncutils.h"
13#include "satObs.h"
14#include "pppInclude.h"
15#include "pppModel.h"
16
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) {
307 if (rnxVersion < 3.03) {// specified as flag for GPS
308 _fitInterval = fitIntervalFromFlag(fitIntervalRnx, _IODC, t_eph::GPS);
309 } else { // specified as time period for GPS since 3.03
310 _fitInterval = fitIntervalRnx;
311 }
312 } else if (type() == t_eph::QZSS) { // specified as flag for QZSS
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
406 xc[4] = _clock_drift + _clock_driftrate*tc;
407 xc[5] = _clock_driftrate;
408
409 return success;
410}
411
412// RINEX Format String
413//////////////////////////////////////////////////////////////////////////////
414QString t_ephGPS::toString(double version) const {
415
416 QString rnxStr = rinexDateStr(_TOC, _prn, version);
417
418 QTextStream out(&rnxStr);
419
420 out << QString("%1%2%3\n")
421 .arg(_clock_bias, 19, 'e', 12)
422 .arg(_clock_drift, 19, 'e', 12)
423 .arg(_clock_driftrate, 19, 'e', 12);
424
425 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
426
427 out << QString(fmt)
428 .arg(_IODE, 19, 'e', 12)
429 .arg(_Crs, 19, 'e', 12)
430 .arg(_Delta_n, 19, 'e', 12)
431 .arg(_M0, 19, 'e', 12);
432
433 out << QString(fmt)
434 .arg(_Cuc, 19, 'e', 12)
435 .arg(_e, 19, 'e', 12)
436 .arg(_Cus, 19, 'e', 12)
437 .arg(_sqrt_A, 19, 'e', 12);
438
439 out << QString(fmt)
440 .arg(_TOEsec, 19, 'e', 12)
441 .arg(_Cic, 19, 'e', 12)
442 .arg(_OMEGA0, 19, 'e', 12)
443 .arg(_Cis, 19, 'e', 12);
444
445 out << QString(fmt)
446 .arg(_i0, 19, 'e', 12)
447 .arg(_Crc, 19, 'e', 12)
448 .arg(_omega, 19, 'e', 12)
449 .arg(_OMEGADOT, 19, 'e', 12);
450
451 if (type() == t_eph::IRNSS) {
452 out << QString(fmt)
453 .arg(_IDOT, 19, 'e', 12)
454 .arg(0.0, 19, 'e', 12)
455 .arg(_TOEweek, 19, 'e', 12)
456 .arg(0.0, 19, 'e', 12);
457 }
458 else {
459 out << QString(fmt)
460 .arg(_IDOT, 19, 'e', 12)
461 .arg(_L2Codes, 19, 'e', 12)
462 .arg(_TOEweek, 19, 'e', 12)
463 .arg(_L2PFlag, 19, 'e', 12);
464 }
465
466 if (type() == t_eph::IRNSS) {
467 out << QString(fmt)
468 .arg(_ura, 19, 'e', 12)
469 .arg(_health, 19, 'e', 12)
470 .arg(_TGD, 19, 'e', 12)
471 .arg(0.0, 19, 'e', 12);
472 }
473 else {
474 out << QString(fmt)
475 .arg(_ura, 19, 'e', 12)
476 .arg(_health, 19, 'e', 12)
477 .arg(_TGD, 19, 'e', 12)
478 .arg(_IODC, 19, 'e', 12);
479 }
480
481 double tot = _TOT;
482 if (tot == 0.9999e9 && version < 3.0) {
483 tot = 0.0;
484 }
485 // fitInterval
486 if (type() == t_eph::IRNSS) { // not valid for IRNSS
487 out << QString(fmt)
488 .arg(tot, 19, 'e', 12)
489 .arg(0.0, 19, 'e', 12)
490 .arg("", 19, QChar(' '))
491 .arg("", 19, QChar(' '));
492 }
493 else {
494 double fitIntervalRnx;
495 if (type() == t_eph::GPS) {
496 if (version < 3.03) { // specified as flag)
497 (_fitInterval == 4.0) ? fitIntervalRnx = 0.0 : fitIntervalRnx = 1.0;
498 }
499 else {
500 fitIntervalRnx = _fitInterval; // specified as time period in hours
501 }
502 }
503 else if (type() == t_eph::QZSS) { // specified as flag
504 (_fitInterval == 2.0) ? fitIntervalRnx = 0.0 : fitIntervalRnx = 1.0;
505 }
506 out << QString(fmt)
507 .arg(tot, 19, 'e', 12)
508 .arg(fitIntervalRnx, 19, 'e', 12)
509 .arg("", 19, QChar(' '))
510 .arg("", 19, QChar(' '));
511 }
512 return rnxStr;
513}
514
515// Constructor
516//////////////////////////////////////////////////////////////////////////////
517t_ephGlo::t_ephGlo(float rnxVersion, const QStringList& lines) {
518
519 const int nLines = 4;
520
521 if (lines.size() != nLines) {
522 _checkState = bad;
523 return;
524 }
525
526 // RINEX Format
527 // ------------
528 int fieldLen = 19;
529
530 int pos[4];
531 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
532 pos[1] = pos[0] + fieldLen;
533 pos[2] = pos[1] + fieldLen;
534 pos[3] = pos[2] + fieldLen;
535
536 // Read four lines
537 // ---------------
538 for (int iLine = 0; iLine < nLines; iLine++) {
539 QString line = lines[iLine];
540
541 if ( iLine == 0 ) {
542 QTextStream in(line.left(pos[1]).toLatin1());
543
544 int year, month, day, hour, min;
545 double sec;
546
547 QString prnStr, n;
548 in >> prnStr;
549 if (prnStr.size() == 1 && prnStr[0] == 'R') {
550 in >> n;
551 prnStr.append(n);
552 }
553 in >> year >> month >> day >> hour >> min >> sec;
554 if (prnStr.at(0) == 'R') {
555 _prn.set('R', prnStr.mid(1).toInt());
556 }
557 else {
558 _prn.set('R', prnStr.toInt());
559 }
560
561 if (year < 80) {
562 year += 2000;
563 }
564 else if (year < 100) {
565 year += 1900;
566 }
567
568 _gps_utc = gnumleap(year, month, day);
569
570 _TOC.set(year, month, day, hour, min, sec);
571 _TOC = _TOC + _gps_utc;
572 int nd = int((_TOC.gpssec())) / (24.0*60.0*60.0);
573 if ( readDbl(line, pos[1], fieldLen, _tau ) ||
574 readDbl(line, pos[2], fieldLen, _gamma) ||
575 readDbl(line, pos[3], fieldLen, _tki ) ) {
576 _checkState = bad;
577 return;
578 }
579 _tki -= nd * 86400.0;
580 _tau = -_tau;
581 }
582
583 else if ( iLine == 1 ) {
584 if ( readDbl(line, pos[0], fieldLen, _x_pos ) ||
585 readDbl(line, pos[1], fieldLen, _x_velocity ) ||
586 readDbl(line, pos[2], fieldLen, _x_acceleration) ||
587 readDbl(line, pos[3], fieldLen, _health ) ) {
588 _checkState = bad;
589 return;
590 }
591 }
592
593 else if ( iLine == 2 ) {
594 if ( readDbl(line, pos[0], fieldLen, _y_pos ) ||
595 readDbl(line, pos[1], fieldLen, _y_velocity ) ||
596 readDbl(line, pos[2], fieldLen, _y_acceleration ) ||
597 readDbl(line, pos[3], fieldLen, _frequency_number) ) {
598 _checkState = bad;
599 return;
600 }
601 }
602
603 else if ( iLine == 3 ) {
604 if ( readDbl(line, pos[0], fieldLen, _z_pos ) ||
605 readDbl(line, pos[1], fieldLen, _z_velocity ) ||
606 readDbl(line, pos[2], fieldLen, _z_acceleration) ||
607 readDbl(line, pos[3], fieldLen, _E ) ) {
608 _checkState = bad;
609 return;
610 }
611 }
612 }
613
614 // Initialize status vector
615 // ------------------------
616 _tt = _TOC;
617 _xv.ReSize(6); _xv = 0.0;
618 _xv(1) = _x_pos * 1.e3;
619 _xv(2) = _y_pos * 1.e3;
620 _xv(3) = _z_pos * 1.e3;
621 _xv(4) = _x_velocity * 1.e3;
622 _xv(5) = _y_velocity * 1.e3;
623 _xv(6) = _z_velocity * 1.e3;
624}
625
626// Compute Glonass Satellite Position (virtual)
627////////////////////////////////////////////////////////////////////////////
628t_irc t_ephGlo::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
629
630 static const double nominalStep = 10.0;
631
632 memset(xc, 0, 6*sizeof(double));
633 memset(vv, 0, 3*sizeof(double));
634
635 double dtPos = bncTime(GPSweek, GPSweeks) - _tt;
636
637 if (fabs(dtPos) > 24 * 3600.0) {
638 return failure;
639 }
640
641 int nSteps = int(fabs(dtPos) / nominalStep) + 1;
642 double step = dtPos / nSteps;
643
644 double acc[3];
645 acc[0] = _x_acceleration * 1.e3;
646 acc[1] = _y_acceleration * 1.e3;
647 acc[2] = _z_acceleration * 1.e3;
648
649 for (int ii = 1; ii <= nSteps; ii++) {
650 _xv = rungeKutta4(_tt.gpssec(), _xv, step, acc, glo_deriv);
651 _tt = _tt + step;
652 }
653
654 // Position and Velocity
655 // ---------------------
656 xc[0] = _xv(1);
657 xc[1] = _xv(2);
658 xc[2] = _xv(3);
659
660 vv[0] = _xv(4);
661 vv[1] = _xv(5);
662 vv[2] = _xv(6);
663
664 // Clock Correction
665 // ----------------
666 double dtClk = bncTime(GPSweek, GPSweeks) - _TOC;
667 xc[3] = -_tau + _gamma * dtClk;
668
669 xc[4] = _gamma;
670 xc[5] = 0.0;
671
672 return success;
673}
674
675// RINEX Format String
676//////////////////////////////////////////////////////////////////////////////
677QString t_ephGlo::toString(double version) const {
678
679 QString rnxStr = rinexDateStr(_TOC -_gps_utc, _prn, version);
680 int nd = int((_TOC - _gps_utc).gpssec()) / (24.0*60.0*60.0);
681 QTextStream out(&rnxStr);
682
683 out << QString("%1%2%3\n")
684 .arg(-_tau, 19, 'e', 12)
685 .arg(_gamma, 19, 'e', 12)
686 .arg(_tki+nd*86400.0, 19, 'e', 12);
687
688 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
689
690 out << QString(fmt)
691 .arg(_x_pos, 19, 'e', 12)
692 .arg(_x_velocity, 19, 'e', 12)
693 .arg(_x_acceleration, 19, 'e', 12)
694 .arg(_health, 19, 'e', 12);
695
696 out << QString(fmt)
697 .arg(_y_pos, 19, 'e', 12)
698 .arg(_y_velocity, 19, 'e', 12)
699 .arg(_y_acceleration, 19, 'e', 12)
700 .arg(_frequency_number, 19, 'e', 12);
701
702 out << QString(fmt)
703 .arg(_z_pos, 19, 'e', 12)
704 .arg(_z_velocity, 19, 'e', 12)
705 .arg(_z_acceleration, 19, 'e', 12)
706 .arg(_E, 19, 'e', 12);
707
708 return rnxStr;
709}
710
711// Derivative of the state vector using a simple force model (static)
712////////////////////////////////////////////////////////////////////////////
713ColumnVector t_ephGlo::glo_deriv(double /* tt */, const ColumnVector& xv,
714 double* acc) {
715
716 // State vector components
717 // -----------------------
718 ColumnVector rr = xv.rows(1,3);
719 ColumnVector vv = xv.rows(4,6);
720
721 // Acceleration
722 // ------------
723 static const double gmWGS = 398.60044e12;
724 static const double AE = 6378136.0;
725 static const double OMEGA = 7292115.e-11;
726 static const double C20 = -1082.6257e-6;
727
728 double rho = rr.NormFrobenius();
729 double t1 = -gmWGS/(rho*rho*rho);
730 double t2 = 3.0/2.0 * C20 * (gmWGS*AE*AE) / (rho*rho*rho*rho*rho);
731 double t3 = OMEGA * OMEGA;
732 double t4 = 2.0 * OMEGA;
733 double z2 = rr(3) * rr(3);
734
735 // Vector of derivatives
736 // ---------------------
737 ColumnVector va(6);
738 va(1) = vv(1);
739 va(2) = vv(2);
740 va(3) = vv(3);
741 va(4) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2) + acc[0];
742 va(5) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1) + acc[1];
743 va(6) = (t1 + t2*(3.0-5.0*z2/(rho*rho)) ) * rr(3) + acc[2];
744
745 return va;
746}
747
748// IOD of Glonass Ephemeris (virtual)
749////////////////////////////////////////////////////////////////////////////
750unsigned int t_ephGlo::IOD() const {
751 bncTime tMoscow = _TOC - _gps_utc + 3 * 3600.0;
752 return (unsigned long)tMoscow.daysec() / 900;
753}
754
755// Health status of Glonass Ephemeris (virtual)
756////////////////////////////////////////////////////////////////////////////
757unsigned int t_ephGlo::isUnhealthy() const {
758
759 if (_almanac_health_availablility_indicator) {
760 if ((_health == 0 && _almanac_health == 0) ||
761 (_health == 1 && _almanac_health == 0) ||
762 (_health == 1 && _almanac_health == 1)) {
763 return 1;
764 }
765 }
766 else if (!_almanac_health_availablility_indicator) {
767 if (_health) {
768 return 1;
769 }
770 }
771 return 0; /* (_health == 0 && _almanac_health == 1) or (_health == 0) */
772}
773
774
775// Constructor
776//////////////////////////////////////////////////////////////////////////////
777t_ephGal::t_ephGal(float rnxVersion, const QStringList& lines) {
778 int year, month, day, hour, min;
779 double sec;
780 QString prnStr;
781 const int nLines = 8;
782 if (lines.size() != nLines) {
783 _checkState = bad;
784 return;
785 }
786
787 // RINEX Format
788 // ------------
789 int fieldLen = 19;
790 double SVhealth = 0.0;
791 double datasource = 0.0;
792
793 int pos[4];
794 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
795 pos[1] = pos[0] + fieldLen;
796 pos[2] = pos[1] + fieldLen;
797 pos[3] = pos[2] + fieldLen;
798
799 // Read eight lines
800 // ----------------
801 for (int iLine = 0; iLine < nLines; iLine++) {
802 QString line = lines[iLine];
803
804 if ( iLine == 0 ) {
805 QTextStream in(line.left(pos[1]).toLatin1());
806 QString n;
807 in >> prnStr;
808 if (prnStr.size() == 1 && prnStr[0] == 'E') {
809 in >> n;
810 prnStr.append(n);
811 }
812 in >> year >> month >> day >> hour >> min >> sec;
813 if (year < 80) {
814 year += 2000;
815 }
816 else if (year < 100) {
817 year += 1900;
818 }
819
820 _TOC.set(year, month, day, hour, min, sec);
821
822 if ( readDbl(line, pos[1], fieldLen, _clock_bias ) ||
823 readDbl(line, pos[2], fieldLen, _clock_drift ) ||
824 readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
825 _checkState = bad;
826 return;
827 }
828 }
829
830 else if ( iLine == 1 ) {
831 if ( readDbl(line, pos[0], fieldLen, _IODnav ) ||
832 readDbl(line, pos[1], fieldLen, _Crs ) ||
833 readDbl(line, pos[2], fieldLen, _Delta_n) ||
834 readDbl(line, pos[3], fieldLen, _M0 ) ) {
835 _checkState = bad;
836 return;
837 }
838 }
839
840 else if ( iLine == 2 ) {
841 if ( readDbl(line, pos[0], fieldLen, _Cuc ) ||
842 readDbl(line, pos[1], fieldLen, _e ) ||
843 readDbl(line, pos[2], fieldLen, _Cus ) ||
844 readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
845 _checkState = bad;
846 return;
847 }
848 }
849
850 else if ( iLine == 3 ) {
851 if ( readDbl(line, pos[0], fieldLen, _TOEsec) ||
852 readDbl(line, pos[1], fieldLen, _Cic ) ||
853 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
854 readDbl(line, pos[3], fieldLen, _Cis ) ) {
855 _checkState = bad;
856 return;
857 }
858 }
859
860 else if ( iLine == 4 ) {
861 if ( readDbl(line, pos[0], fieldLen, _i0 ) ||
862 readDbl(line, pos[1], fieldLen, _Crc ) ||
863 readDbl(line, pos[2], fieldLen, _omega ) ||
864 readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
865 _checkState = bad;
866 return;
867 }
868 }
869
870 else if ( iLine == 5 ) {
871 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
872 readDbl(line, pos[1], fieldLen, datasource) ||
873 readDbl(line, pos[2], fieldLen, _TOEweek ) ) {
874 _checkState = bad;
875 return;
876 } else {
877 if (int(datasource) & (1<<8)) {
878 _fnav = true;
879 _inav = false;
880 } else if (int(datasource) & (1<<9)) {
881 _fnav = false;
882 _inav = true;
883 }
884 _TOEweek -= 1024.0;
885 }
886 }
887
888 else if ( iLine == 6 ) {
889 if ( readDbl(line, pos[0], fieldLen, _SISA ) ||
890 readDbl(line, pos[1], fieldLen, SVhealth) ||
891 readDbl(line, pos[2], fieldLen, _BGD_1_5A) ||
892 readDbl(line, pos[3], fieldLen, _BGD_1_5B) ) {
893 _checkState = bad;
894 return;
895 } else {
896 // Bit 0
897 _e1DataInValid = (int(SVhealth) & (1<<0));
898 // Bit 1-2
899 _E1_bHS = double((int(SVhealth) >> 1) & 0x3);
900 // Bit 3
901 _e5aDataInValid = (int(SVhealth) & (1<<3));
902 // Bit 4-5
903 _E5aHS = double((int(SVhealth) >> 4) & 0x3);
904 // Bit 6
905 _e5bDataInValid = (int(SVhealth) & (1<<6));
906 // Bit 7-8
907 _E5bHS = double((int(SVhealth) >> 7) & 0x3);
908
909 if (prnStr.at(0) == 'E') {
910 _prn.set('E', prnStr.mid(1).toInt(), _inav ? 1 : 0);
911 }
912 }
913 }
914
915 else if ( iLine == 7 ) {
916 if ( readDbl(line, pos[0], fieldLen, _TOT) ) {
917 _checkState = bad;
918 return;
919 }
920 }
921 }
922}
923
924// Compute Galileo Satellite Position (virtual)
925////////////////////////////////////////////////////////////////////////////
926t_irc t_ephGal::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
927
928 static const double omegaEarth = 7292115.1467e-11;
929 static const double gmWGS = 398.6004418e12;
930
931 memset(xc, 0, 6*sizeof(double));
932 memset(vv, 0, 3*sizeof(double));
933
934 double a0 = _sqrt_A * _sqrt_A;
935 if (a0 == 0) {
936 return failure;
937 }
938
939 double n0 = sqrt(gmWGS/(a0*a0*a0));
940
941 bncTime tt(GPSweek, GPSweeks);
942 double tk = tt - bncTime(_TOC.gpsw(), _TOEsec);
943
944 double n = n0 + _Delta_n;
945 double M = _M0 + n*tk;
946 double E = M;
947 double E_last;
948 int nLoop = 0;
949 do {
950 E_last = E;
951 E = M + _e*sin(E);
952
953 if (++nLoop == 100) {
954 return failure;
955 }
956 } while ( fabs(E-E_last)*a0 > 0.001 );
957 double v = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) );
958 double u0 = v + _omega;
959 double sin2u0 = sin(2*u0);
960 double cos2u0 = cos(2*u0);
961 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
962 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
963 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
964 double xp = r*cos(u);
965 double yp = r*sin(u);
966 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
967 omegaEarth*_TOEsec;
968
969 double sinom = sin(OM);
970 double cosom = cos(OM);
971 double sini = sin(i);
972 double cosi = cos(i);
973 xc[0] = xp*cosom - yp*cosi*sinom;
974 xc[1] = xp*sinom + yp*cosi*cosom;
975 xc[2] = yp*sini;
976
977 double tc = tt - _TOC;
978 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
979
980 // Velocity
981 // --------
982 double tanv2 = tan(v/2);
983 double dEdM = 1 / (1 - _e*cos(E));
984 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
985 * dEdM * n;
986 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
987 double dotom = _OMEGADOT - omegaEarth;
988 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
989 double dotr = a0 * _e*sin(E) * dEdM * n
990 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
991 double dotx = dotr*cos(u) - r*sin(u)*dotu;
992 double doty = dotr*sin(u) + r*cos(u)*dotu;
993
994 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
995 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
996 + yp*sini*sinom*doti; // dX / di
997
998 vv[1] = sinom *dotx + cosi*cosom *doty
999 + xp*cosom*dotom - yp*cosi*sinom*dotom
1000 - yp*sini*cosom*doti;
1001
1002 vv[2] = sini *doty + yp*cosi *doti;
1003
1004 // Relativistic Correction
1005 // -----------------------
1006 // correspondent to Galileo ICD and to SSR standard
1007 xc[3] -= 4.442807633e-10 * _e * sqrt(a0) *sin(E);
1008 // correspondent to IGS convention
1009 //xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c;
1010
1011 xc[4] = _clock_drift + _clock_driftrate*tc;
1012 xc[5] = _clock_driftrate;
1013
1014 return success;
1015}
1016
1017// Health status of Galileo Ephemeris (virtual)
1018////////////////////////////////////////////////////////////////////////////
1019unsigned int t_ephGal::isUnhealthy() const {
1020 if (_E5aHS && _E5bHS && _E1_bHS) {
1021 return 1;
1022 }
1023 return 0;
1024}
1025
1026// RINEX Format String
1027//////////////////////////////////////////////////////////////////////////////
1028QString t_ephGal::toString(double version) const {
1029
1030 QString rnxStr = rinexDateStr(_TOC, _prn, version);
1031
1032 QTextStream out(&rnxStr);
1033
1034 out << QString("%1%2%3\n")
1035 .arg(_clock_bias, 19, 'e', 12)
1036 .arg(_clock_drift, 19, 'e', 12)
1037 .arg(_clock_driftrate, 19, 'e', 12);
1038
1039 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1040
1041 out << QString(fmt)
1042 .arg(_IODnav, 19, 'e', 12)
1043 .arg(_Crs, 19, 'e', 12)
1044 .arg(_Delta_n, 19, 'e', 12)
1045 .arg(_M0, 19, 'e', 12);
1046
1047 out << QString(fmt)
1048 .arg(_Cuc, 19, 'e', 12)
1049 .arg(_e, 19, 'e', 12)
1050 .arg(_Cus, 19, 'e', 12)
1051 .arg(_sqrt_A, 19, 'e', 12);
1052
1053 out << QString(fmt)
1054 .arg(_TOEsec, 19, 'e', 12)
1055 .arg(_Cic, 19, 'e', 12)
1056 .arg(_OMEGA0, 19, 'e', 12)
1057 .arg(_Cis, 19, 'e', 12);
1058
1059 out << QString(fmt)
1060 .arg(_i0, 19, 'e', 12)
1061 .arg(_Crc, 19, 'e', 12)
1062 .arg(_omega, 19, 'e', 12)
1063 .arg(_OMEGADOT, 19, 'e', 12);
1064
1065 int dataSource = 0;
1066 int SVhealth = 0;
1067 double BGD_1_5A = _BGD_1_5A;
1068 double BGD_1_5B = _BGD_1_5B;
1069 if (_fnav) {
1070 dataSource |= (1<<1);
1071 dataSource |= (1<<8);
1072 BGD_1_5B = 0.0;
1073 // SVhealth
1074 // Bit 3 : E5a DVS
1075 if (_e5aDataInValid) {
1076 SVhealth |= (1<<3);
1077 }
1078 // Bit 4-5: E5a HS
1079 if (_E5aHS == 1.0) {
1080 SVhealth |= (1<<4);
1081 }
1082 else if (_E5aHS == 2.0) {
1083 SVhealth |= (1<<5);
1084 }
1085 else if (_E5aHS == 3.0) {
1086 SVhealth |= (1<<4);
1087 SVhealth |= (1<<5);
1088 }
1089 }
1090 else if(_inav) {
1091 // Bit 2 and 0 are set because from MT1046 the data source cannot be determined
1092 // and RNXv3.03 says both can be set if the navigation messages were merged
1093 dataSource |= (1<<0);
1094 dataSource |= (1<<2);
1095 dataSource |= (1<<9);
1096 // SVhealth
1097 // Bit 0 : E1-B DVS
1098 if (_e1DataInValid) {
1099 SVhealth |= (1<<0);
1100 }
1101 // Bit 1-2: E1-B HS
1102 if (_E1_bHS == 1.0) {
1103 SVhealth |= (1<<1);
1104 }
1105 else if (_E1_bHS == 2.0) {
1106 SVhealth |= (1<<2);
1107 }
1108 else if (_E1_bHS == 3.0) {
1109 SVhealth |= (1<<1);
1110 SVhealth |= (1<<2);
1111 }
1112 // Bit 3 : E5a DVS
1113 if (_e5aDataInValid) {
1114 SVhealth |= (1<<3);
1115 }
1116 // Bit 4-5: E5a HS
1117 if (_E5aHS == 1.0) {
1118 SVhealth |= (1<<4);
1119 }
1120 else if (_E5aHS == 2.0) {
1121 SVhealth |= (1<<5);
1122 }
1123 else if (_E5aHS == 3.0) {
1124 SVhealth |= (1<<4);
1125 SVhealth |= (1<<5);
1126 }
1127 // Bit 6 : E5b DVS
1128 if (_e5bDataInValid) {
1129 SVhealth |= (1<<6);
1130 }
1131 // Bit 7-8: E5b HS
1132 if (_E5bHS == 1.0) {
1133 SVhealth |= (1<<7);
1134 }
1135 else if (_E5bHS == 2.0) {
1136 SVhealth |= (1<<8);
1137 }
1138 else if (_E5bHS == 3.0) {
1139 SVhealth |= (1<<7);
1140 SVhealth |= (1<<8);
1141 }
1142 }
1143
1144 out << QString(fmt)
1145 .arg(_IDOT, 19, 'e', 12)
1146 .arg(double(dataSource), 19, 'e', 12)
1147 .arg(_TOEweek + 1024.0, 19, 'e', 12)
1148 .arg(0.0, 19, 'e', 12);
1149
1150 out << QString(fmt)
1151 .arg(_SISA, 19, 'e', 12)
1152 .arg(double(SVhealth), 19, 'e', 12)
1153 .arg(BGD_1_5A, 19, 'e', 12)
1154 .arg(BGD_1_5B, 19, 'e', 12);
1155
1156
1157 double tot = _TOT;
1158 if (tot == 0.9999e9 && version < 3.0) {
1159 tot = 0.0;
1160 }
1161 out << QString(fmt)
1162 .arg(tot, 19, 'e', 12)
1163 .arg("", 19, QChar(' '))
1164 .arg("", 19, QChar(' '))
1165 .arg("", 19, QChar(' '));
1166
1167 return rnxStr;
1168}
1169
1170// Constructor
1171//////////////////////////////////////////////////////////////////////////////
1172t_ephSBAS::t_ephSBAS(float rnxVersion, const QStringList& lines) {
1173
1174 const int nLines = 4;
1175
1176 if (lines.size() != nLines) {
1177 _checkState = bad;
1178 return;
1179 }
1180
1181 // RINEX Format
1182 // ------------
1183 int fieldLen = 19;
1184
1185 int pos[4];
1186 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
1187 pos[1] = pos[0] + fieldLen;
1188 pos[2] = pos[1] + fieldLen;
1189 pos[3] = pos[2] + fieldLen;
1190
1191 // Read four lines
1192 // ---------------
1193 for (int iLine = 0; iLine < nLines; iLine++) {
1194 QString line = lines[iLine];
1195
1196 if ( iLine == 0 ) {
1197 QTextStream in(line.left(pos[1]).toLatin1());
1198
1199 int year, month, day, hour, min;
1200 double sec;
1201
1202 QString prnStr, n;
1203 in >> prnStr;
1204 if (prnStr.size() == 1 && prnStr[0] == 'S') {
1205 in >> n;
1206 prnStr.append(n);
1207 }
1208 in >> year >> month >> day >> hour >> min >> sec;
1209 if (prnStr.at(0) == 'S') {
1210 _prn.set('S', prnStr.mid(1).toInt());
1211 }
1212 else {
1213 _prn.set('S', prnStr.toInt());
1214 }
1215
1216 if (year < 80) {
1217 year += 2000;
1218 }
1219 else if (year < 100) {
1220 year += 1900;
1221 }
1222
1223 _TOC.set(year, month, day, hour, min, sec);
1224
1225 if ( readDbl(line, pos[1], fieldLen, _agf0 ) ||
1226 readDbl(line, pos[2], fieldLen, _agf1 ) ||
1227 readDbl(line, pos[3], fieldLen, _TOT ) ) {
1228 _checkState = bad;
1229 return;
1230 }
1231 }
1232
1233 else if ( iLine == 1 ) {
1234 if ( readDbl(line, pos[0], fieldLen, _x_pos ) ||
1235 readDbl(line, pos[1], fieldLen, _x_velocity ) ||
1236 readDbl(line, pos[2], fieldLen, _x_acceleration) ||
1237 readDbl(line, pos[3], fieldLen, _health ) ) {
1238 _checkState = bad;
1239 return;
1240 }
1241 }
1242
1243 else if ( iLine == 2 ) {
1244 if ( readDbl(line, pos[0], fieldLen, _y_pos ) ||
1245 readDbl(line, pos[1], fieldLen, _y_velocity ) ||
1246 readDbl(line, pos[2], fieldLen, _y_acceleration ) ||
1247 readDbl(line, pos[3], fieldLen, _ura ) ) {
1248 _checkState = bad;
1249 return;
1250 }
1251 }
1252
1253 else if ( iLine == 3 ) {
1254 double iodn;
1255 if ( readDbl(line, pos[0], fieldLen, _z_pos ) ||
1256 readDbl(line, pos[1], fieldLen, _z_velocity ) ||
1257 readDbl(line, pos[2], fieldLen, _z_acceleration) ||
1258 readDbl(line, pos[3], fieldLen, iodn ) ) {
1259 _checkState = bad;
1260 return;
1261 } else {
1262 _IODN = int(iodn);
1263 }
1264 }
1265 }
1266
1267 _x_pos *= 1.e3;
1268 _y_pos *= 1.e3;
1269 _z_pos *= 1.e3;
1270 _x_velocity *= 1.e3;
1271 _y_velocity *= 1.e3;
1272 _z_velocity *= 1.e3;
1273 _x_acceleration *= 1.e3;
1274 _y_acceleration *= 1.e3;
1275 _z_acceleration *= 1.e3;
1276}
1277
1278// IOD of SBAS Ephemeris (virtual)
1279////////////////////////////////////////////////////////////////////////////
1280
1281unsigned int t_ephSBAS::IOD() const {
1282 unsigned char buffer[80];
1283 int size = 0;
1284 int numbits = 0;
1285 long long bitbuffer = 0;
1286 unsigned char *startbuffer = buffer;
1287
1288 SBASADDBITSFLOAT(30, this->_x_pos, 0.08)
1289 SBASADDBITSFLOAT(30, this->_y_pos, 0.08)
1290 SBASADDBITSFLOAT(25, this->_z_pos, 0.4)
1291 SBASADDBITSFLOAT(17, this->_x_velocity, 0.000625)
1292 SBASADDBITSFLOAT(17, this->_y_velocity, 0.000625)
1293 SBASADDBITSFLOAT(18, this->_z_velocity, 0.004)
1294 SBASADDBITSFLOAT(10, this->_x_acceleration, 0.0000125)
1295 SBASADDBITSFLOAT(10, this->_y_acceleration, 0.0000125)
1296 SBASADDBITSFLOAT(10, this->_z_acceleration, 0.0000625)
1297 SBASADDBITSFLOAT(12, this->_agf0, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
1298 SBASADDBITSFLOAT(8, this->_agf1, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<10))
1299 SBASADDBITS(5,0); // the last byte is filled by 0-bits to obtain a length of an integer multiple of 8
1300
1301 return CRC24(size, startbuffer);
1302}
1303
1304// Compute SBAS Satellite Position (virtual)
1305////////////////////////////////////////////////////////////////////////////
1306t_irc t_ephSBAS::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
1307
1308 bncTime tt(GPSweek, GPSweeks);
1309 double dt = tt - _TOC;
1310
1311 xc[0] = _x_pos + _x_velocity * dt + _x_acceleration * dt * dt / 2.0;
1312 xc[1] = _y_pos + _y_velocity * dt + _y_acceleration * dt * dt / 2.0;
1313 xc[2] = _z_pos + _z_velocity * dt + _z_acceleration * dt * dt / 2.0;
1314
1315 vv[0] = _x_velocity + _x_acceleration * dt;
1316 vv[1] = _y_velocity + _y_acceleration * dt;
1317 vv[2] = _z_velocity + _z_acceleration * dt;
1318
1319 xc[3] = _agf0 + _agf1 * dt;
1320
1321 xc[4] = _agf1;
1322 xc[5] = 0.0;
1323
1324 return success;
1325}
1326
1327// RINEX Format String
1328//////////////////////////////////////////////////////////////////////////////
1329QString t_ephSBAS::toString(double version) const {
1330
1331 QString rnxStr = rinexDateStr(_TOC, _prn, version);
1332
1333 QTextStream out(&rnxStr);
1334
1335 out << QString("%1%2%3\n")
1336 .arg(_agf0, 19, 'e', 12)
1337 .arg(_agf1, 19, 'e', 12)
1338 .arg(_TOT, 19, 'e', 12);
1339
1340 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1341
1342 out << QString(fmt)
1343 .arg(1.e-3*_x_pos, 19, 'e', 12)
1344 .arg(1.e-3*_x_velocity, 19, 'e', 12)
1345 .arg(1.e-3*_x_acceleration, 19, 'e', 12)
1346 .arg(_health, 19, 'e', 12);
1347
1348 out << QString(fmt)
1349 .arg(1.e-3*_y_pos, 19, 'e', 12)
1350 .arg(1.e-3*_y_velocity, 19, 'e', 12)
1351 .arg(1.e-3*_y_acceleration, 19, 'e', 12)
1352 .arg(_ura, 19, 'e', 12);
1353
1354 out << QString(fmt)
1355 .arg(1.e-3*_z_pos, 19, 'e', 12)
1356 .arg(1.e-3*_z_velocity, 19, 'e', 12)
1357 .arg(1.e-3*_z_acceleration, 19, 'e', 12)
1358 .arg(double(_IODN), 19, 'e', 12);
1359
1360 return rnxStr;
1361}
1362
1363// Constructor
1364//////////////////////////////////////////////////////////////////////////////
1365t_ephBDS::t_ephBDS(float rnxVersion, const QStringList& lines) {
1366
1367 const int nLines = 8;
1368
1369 if (lines.size() != nLines) {
1370 _checkState = bad;
1371 return;
1372 }
1373
1374 // RINEX Format
1375 // ------------
1376 int fieldLen = 19;
1377
1378 int pos[4];
1379 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
1380 pos[1] = pos[0] + fieldLen;
1381 pos[2] = pos[1] + fieldLen;
1382 pos[3] = pos[2] + fieldLen;
1383
1384 // Read eight lines
1385 // ----------------
1386 for (int iLine = 0; iLine < nLines; iLine++) {
1387 QString line = lines[iLine];
1388
1389 if ( iLine == 0 ) {
1390 QTextStream in(line.left(pos[1]).toLatin1());
1391
1392 int year, month, day, hour, min;
1393 double sec;
1394
1395 QString prnStr, n;
1396 in >> prnStr;
1397 if (prnStr.size() == 1 && prnStr[0] == 'C') {
1398 in >> n;
1399 prnStr.append(n);
1400 }
1401 in >> year >> month >> day >> hour >> min >> sec;
1402 if (prnStr.at(0) == 'C') {
1403 _prn.set('C', prnStr.mid(1).toInt());
1404 }
1405 else {
1406 _prn.set('C', prnStr.toInt());
1407 }
1408
1409 if (year < 80) {
1410 year += 2000;
1411 }
1412 else if (year < 100) {
1413 year += 1900;
1414 }
1415
1416 _TOC.setBDS(year, month, day, hour, min, sec);
1417
1418 if ( readDbl(line, pos[1], fieldLen, _clock_bias ) ||
1419 readDbl(line, pos[2], fieldLen, _clock_drift ) ||
1420 readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
1421 _checkState = bad;
1422 return;
1423 }
1424 }
1425
1426 else if ( iLine == 1 ) {
1427 double aode;
1428 if ( readDbl(line, pos[0], fieldLen, aode ) ||
1429 readDbl(line, pos[1], fieldLen, _Crs ) ||
1430 readDbl(line, pos[2], fieldLen, _Delta_n) ||
1431 readDbl(line, pos[3], fieldLen, _M0 ) ) {
1432 _checkState = bad;
1433 return;
1434 }
1435 _AODE = int(aode);
1436 }
1437
1438 else if ( iLine == 2 ) {
1439 if ( readDbl(line, pos[0], fieldLen, _Cuc ) ||
1440 readDbl(line, pos[1], fieldLen, _e ) ||
1441 readDbl(line, pos[2], fieldLen, _Cus ) ||
1442 readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
1443 _checkState = bad;
1444 return;
1445 }
1446 }
1447
1448 else if ( iLine == 3 ) {
1449 if ( readDbl(line, pos[0], fieldLen, _TOEsec ) ||
1450 readDbl(line, pos[1], fieldLen, _Cic ) ||
1451 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
1452 readDbl(line, pos[3], fieldLen, _Cis ) ) {
1453 _checkState = bad;
1454 return;
1455 }
1456 }
1457
1458 else if ( iLine == 4 ) {
1459 if ( readDbl(line, pos[0], fieldLen, _i0 ) ||
1460 readDbl(line, pos[1], fieldLen, _Crc ) ||
1461 readDbl(line, pos[2], fieldLen, _omega ) ||
1462 readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
1463 _checkState = bad;
1464 return;
1465 }
1466 }
1467
1468 else if ( iLine == 5 ) {
1469 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
1470 readDbl(line, pos[2], fieldLen, _TOEweek)) {
1471 _checkState = bad;
1472 return;
1473 }
1474 }
1475
1476 else if ( iLine == 6 ) {
1477 double SatH1;
1478 if ( readDbl(line, pos[0], fieldLen, _URA ) ||
1479 readDbl(line, pos[1], fieldLen, SatH1) ||
1480 readDbl(line, pos[2], fieldLen, _TGD1) ||
1481 readDbl(line, pos[3], fieldLen, _TGD2) ) {
1482 _checkState = bad;
1483 return;
1484 }
1485 _SatH1 = int(SatH1);
1486 }
1487
1488 else if ( iLine == 7 ) {
1489 double aodc;
1490 if ( readDbl(line, pos[0], fieldLen, _TOT) ||
1491 readDbl(line, pos[1], fieldLen, aodc) ) {
1492 _checkState = bad;
1493 return;
1494 }
1495 if (_TOT == 0.9999e9) { // 0.9999e9 means not known (RINEX standard)
1496 _TOT = _TOEsec;
1497 }
1498 _AODC = int(aodc);
1499 }
1500 }
1501
1502 _TOE.setBDS(int(_TOEweek), _TOEsec);
1503
1504 // remark: actually should be computed from second_tot
1505 // but it seems to be unreliable in RINEX files
1506 //_TOT = _TOC.bdssec();
1507}
1508
1509// IOD of BDS Ephemeris (virtual)
1510////////////////////////////////////////////////////////////////////////////
1511unsigned int t_ephBDS::IOD() const {
1512 return (int(_TOEsec)/720) % 240;
1513}
1514
1515// Compute BDS Satellite Position (virtual)
1516//////////////////////////////////////////////////////////////////////////////
1517t_irc t_ephBDS::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
1518
1519 static const double gmBDS = 398.6004418e12;
1520 static const double omegaBDS = 7292115.0000e-11;
1521
1522 xc[0] = xc[1] = xc[2] = xc[3] = 0.0;
1523 vv[0] = vv[1] = vv[2] = 0.0;
1524
1525 bncTime tt(GPSweek, GPSweeks);
1526
1527 if (_sqrt_A == 0) {
1528 return failure;
1529 }
1530 double a0 = _sqrt_A * _sqrt_A;
1531
1532 double n0 = sqrt(gmBDS/(a0*a0*a0));
1533 double tk = tt - _TOE;
1534 double n = n0 + _Delta_n;
1535 double M = _M0 + n*tk;
1536 double E = M;
1537 double E_last;
1538 int nLoop = 0;
1539 do {
1540 E_last = E;
1541 E = M + _e*sin(E);
1542
1543 if (++nLoop == 100) {
1544 return failure;
1545 }
1546 } while ( fabs(E-E_last)*a0 > 0.001 );
1547
1548 double v = atan2(sqrt(1-_e*_e) * sin(E), cos(E) - _e);
1549 double u0 = v + _omega;
1550 double sin2u0 = sin(2*u0);
1551 double cos2u0 = cos(2*u0);
1552 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
1553 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
1554 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
1555 double xp = r*cos(u);
1556 double yp = r*sin(u);
1557 double toesec = (_TOE.gpssec() - 14.0);
1558 double sinom = 0;
1559 double cosom = 0;
1560 double sini = 0;
1561 double cosi = 0;
1562
1563 // Velocity
1564 // --------
1565 double tanv2 = tan(v/2);
1566 double dEdM = 1 / (1 - _e*cos(E));
1567 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2)
1568 / (1 + tanv2*tanv2) * dEdM * n;
1569 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
1570 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
1571 double dotr = a0 * _e*sin(E) * dEdM * n
1572 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
1573
1574 double dotx = dotr*cos(u) - r*sin(u)*dotu;
1575 double doty = dotr*sin(u) + r*cos(u)*dotu;
1576
1577 const double iMaxGEO = 10.0 / 180.0 * M_PI;
1578
1579 // MEO/IGSO satellite
1580 // ------------------
1581 if (_i0 > iMaxGEO) {
1582 double OM = _OMEGA0 + (_OMEGADOT - omegaBDS)*tk - omegaBDS*toesec;
1583
1584 sinom = sin(OM);
1585 cosom = cos(OM);
1586 sini = sin(i);
1587 cosi = cos(i);
1588
1589 xc[0] = xp*cosom - yp*cosi*sinom;
1590 xc[1] = xp*sinom + yp*cosi*cosom;
1591 xc[2] = yp*sini;
1592
1593 // Velocity
1594 // --------
1595
1596 double dotom = _OMEGADOT - t_CST::omega;
1597
1598 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
1599 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
1600 + yp*sini*sinom*doti; // dX / di
1601
1602 vv[1] = sinom *dotx + cosi*cosom *doty
1603 + xp*cosom*dotom - yp*cosi*sinom*dotom
1604 - yp*sini*cosom*doti;
1605
1606 vv[2] = sini *doty + yp*cosi *doti;
1607
1608 }
1609
1610 // GEO satellite
1611 // -------------
1612 else {
1613 double OM = _OMEGA0 + _OMEGADOT*tk - omegaBDS*toesec;
1614 double ll = omegaBDS*tk;
1615
1616 sinom = sin(OM);
1617 cosom = cos(OM);
1618 sini = sin(i);
1619 cosi = cos(i);
1620
1621 double xx = xp*cosom - yp*cosi*sinom;
1622 double yy = xp*sinom + yp*cosi*cosom;
1623 double zz = yp*sini;
1624
1625 Matrix RX = BNC_PPP::t_astro::rotX(-5.0 / 180.0 * M_PI);
1626 Matrix RZ = BNC_PPP::t_astro::rotZ(ll);
1627
1628 ColumnVector X1(3); X1 << xx << yy << zz;
1629 ColumnVector X2 = RZ*RX*X1;
1630
1631 xc[0] = X2(1);
1632 xc[1] = X2(2);
1633 xc[2] = X2(3);
1634
1635 double dotom = _OMEGADOT;
1636
1637 double vx = cosom *dotx - cosi*sinom *doty
1638 - xp*sinom*dotom - yp*cosi*cosom*dotom
1639 + yp*sini*sinom*doti;
1640
1641 double vy = sinom *dotx + cosi*cosom *doty
1642 + xp*cosom*dotom - yp*cosi*sinom*dotom
1643 - yp*sini*cosom*doti;
1644
1645 double vz = sini *doty + yp*cosi *doti;
1646
1647 ColumnVector V(3); V << vx << vy << vz;
1648
1649 Matrix RdotZ(3,3);
1650 double C = cos(ll);
1651 double S = sin(ll);
1652 Matrix UU(3,3);
1653 UU[0][0] = -S; UU[0][1] = +C; UU[0][2] = 0.0;
1654 UU[1][0] = -C; UU[1][1] = -S; UU[1][2] = 0.0;
1655 UU[2][0] = 0.0; UU[2][1] = 0.0; UU[2][2] = 0.0;
1656 RdotZ = omegaBDS * UU;
1657
1658 ColumnVector VV(3);
1659 VV = RZ*RX*V + RdotZ*RX*X1;
1660
1661 vv[0] = VV(1);
1662 vv[1] = VV(2);
1663 vv[2] = VV(3);
1664 }
1665
1666 double tc = tt - _TOC;
1667 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
1668
1669 // dotC = _clock_drift + _clock_driftrate*tc
1670 // - 4.442807633e-10*_e*sqrt(a0)*cos(E) * dEdM * n;
1671
1672 // Relativistic Correction
1673 // -----------------------
1674 // correspondent to BDS ICD and to SSR standard
1675 xc[3] -= 4.442807633e-10 * _e * sqrt(a0) *sin(E);
1676 // correspondent to IGS convention
1677 // xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c;
1678
1679 xc[4] = _clock_drift + _clock_driftrate*tc;
1680 xc[5] = _clock_driftrate;
1681 return success;
1682}
1683
1684// RINEX Format String
1685//////////////////////////////////////////////////////////////////////////////
1686QString t_ephBDS::toString(double version) const {
1687
1688 QString rnxStr = rinexDateStr(_TOC-14.0, _prn, version);
1689
1690 QTextStream out(&rnxStr);
1691
1692 out << QString("%1%2%3\n")
1693 .arg(_clock_bias, 19, 'e', 12)
1694 .arg(_clock_drift, 19, 'e', 12)
1695 .arg(_clock_driftrate, 19, 'e', 12);
1696
1697 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1698
1699 out << QString(fmt)
1700 .arg(double(_AODE), 19, 'e', 12)
1701 .arg(_Crs, 19, 'e', 12)
1702 .arg(_Delta_n, 19, 'e', 12)
1703 .arg(_M0, 19, 'e', 12);
1704
1705 out << QString(fmt)
1706 .arg(_Cuc, 19, 'e', 12)
1707 .arg(_e, 19, 'e', 12)
1708 .arg(_Cus, 19, 'e', 12)
1709 .arg(_sqrt_A, 19, 'e', 12);
1710
1711 double toes = 0.0;
1712 if (_TOEweek > -1.0) {// RINEX input
1713 toes = _TOEsec;
1714 }
1715 else {// RTCM stream input
1716 toes = _TOE.bdssec();
1717 }
1718 out << QString(fmt)
1719 .arg(toes, 19, 'e', 12)
1720 .arg(_Cic, 19, 'e', 12)
1721 .arg(_OMEGA0, 19, 'e', 12)
1722 .arg(_Cis, 19, 'e', 12);
1723
1724 out << QString(fmt)
1725 .arg(_i0, 19, 'e', 12)
1726 .arg(_Crc, 19, 'e', 12)
1727 .arg(_omega, 19, 'e', 12)
1728 .arg(_OMEGADOT, 19, 'e', 12);
1729
1730 double toew = 0.0;
1731 if (_TOEweek > -1.0) {// RINEX input
1732 toew = _TOEweek;
1733 }
1734 else {// RTCM stream input
1735 toew = double(_TOE.bdsw());
1736 }
1737 out << QString(fmt)
1738 .arg(_IDOT, 19, 'e', 12)
1739 .arg(0.0, 19, 'e', 12)
1740 .arg(toew, 19, 'e', 12)
1741 .arg(0.0, 19, 'e', 12);
1742
1743 out << QString(fmt)
1744 .arg(_URA, 19, 'e', 12)
1745 .arg(double(_SatH1), 19, 'e', 12)
1746 .arg(_TGD1, 19, 'e', 12)
1747 .arg(_TGD2, 19, 'e', 12);
1748
1749 double tots = 0.0;
1750 if (_TOEweek > -1.0) {// RINEX input
1751 tots = _TOT;
1752 }
1753 else {// RTCM stream input
1754 tots = _TOE.bdssec();
1755 }
1756 out << QString(fmt)
1757 .arg(tots, 19, 'e', 12)
1758 .arg(double(_AODC), 19, 'e', 12)
1759 .arg("", 19, QChar(' '))
1760 .arg("", 19, QChar(' '));
1761 return rnxStr;
1762}
Note: See TracBrowser for help on using the repository browser.