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

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

some additions regarding rinexV305: navigation data

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