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

Last change on this file since 6802 was 6802, checked in by stuerze, 9 years ago

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