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

Last change on this file since 6807 was 6803, checked in by stuerze, 10 years ago

some bit changes regarding INAV

File size: 48.7 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 // Bit 2 and 0 are set because from MT1046 the data source cannot be determined
1153 // and RNXv3.03 says both can be set if the navigation messages were merged
1154 dataSource |= (1<<0);
1155 dataSource |= (1<<2);
1156 dataSource |= (1<<9);
1157 // SVhealth
1158 // Bit 0 : E1-B DVS
1159 if ((_flags & GALEPHF_E1DINVALID) == GALEPHF_E1DINVALID) {
1160 SVhealth |= (1<<0);
1161 }
1162 // Bit 1-2: E1-B HS
1163 if (_E1_bHS == 1.0) {
1164 SVhealth |= (1<<1);
1165 }
1166 else if (_E1_bHS == 2.0) {
1167 SVhealth |= (1<<2);
1168 }
1169 else if (_E1_bHS == 3.0) {
1170 SVhealth |= (1<<1);
1171 SVhealth |= (1<<2);
1172 }
1173 // Bit 3 : E5a DVS
1174 if ((_flags & GALEPHF_E5ADINVALID) == GALEPHF_E5ADINVALID) {
1175 SVhealth |= (1<<3);
1176 }
1177 // Bit 4-5: E5a HS
1178 if (_E5aHS == 1.0) {
1179 SVhealth |= (1<<4);
1180 }
1181 else if (_E5aHS == 2.0) {
1182 SVhealth |= (1<<5);
1183 }
1184 else if (_E5aHS == 3.0) {
1185 SVhealth |= (1<<4);
1186 SVhealth |= (1<<5);
1187 }
1188 // Bit 6 : E5b DVS
1189 if ((_flags & GALEPHF_E5BDINVALID) == GALEPHF_E5BDINVALID) {
1190 SVhealth |= (1<<6);
1191 }
1192 // Bit 7-8: E5b HS
1193 if (_E5bHS == 1.0) {
1194 SVhealth |= (1<<7);
1195 }
1196 else if (_E5bHS == 2.0) {
1197 SVhealth |= (1<<8);
1198 }
1199 else if (_E5bHS == 3.0) {
1200 SVhealth |= (1<<7);
1201 SVhealth |= (1<<8);
1202 }
1203 }
1204
1205 out << QString(fmt)
1206 .arg(_IDOT, 19, 'e', 12)
1207 .arg(double(dataSource), 19, 'e', 12)
1208 .arg(_TOEweek, 19, 'e', 12)
1209 .arg(0.0, 19, 'e', 12);
1210
1211 out << QString(fmt)
1212 .arg(_SISA, 19, 'e', 12)
1213 .arg(double(SVhealth), 19, 'e', 12)
1214 .arg(BGD_1_5A, 19, 'e', 12)
1215 .arg(BGD_1_5B, 19, 'e', 12);
1216
1217 out << QString(fmt)
1218 .arg(_TOT, 19, 'e', 12)
1219 .arg("", 19, QChar(' '))
1220 .arg("", 19, QChar(' '))
1221 .arg("", 19, QChar(' '));
1222
1223 return rnxStr;
1224}
1225
1226// Constructor
1227//////////////////////////////////////////////////////////////////////////////
1228t_ephSBAS::t_ephSBAS(float rnxVersion, const QStringList& lines) {
1229
1230 const int nLines = 4;
1231
1232 if (lines.size() != nLines) {
1233 _checkState = bad;
1234 return;
1235 }
1236
1237 // RINEX Format
1238 // ------------
1239 int fieldLen = 19;
1240
1241 int pos[4];
1242 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
1243 pos[1] = pos[0] + fieldLen;
1244 pos[2] = pos[1] + fieldLen;
1245 pos[3] = pos[2] + fieldLen;
1246
1247 // Read four lines
1248 // ---------------
1249 for (int iLine = 0; iLine < nLines; iLine++) {
1250 QString line = lines[iLine];
1251
1252 if ( iLine == 0 ) {
1253 QTextStream in(line.left(pos[1]).toAscii());
1254
1255 int year, month, day, hour, min;
1256 double sec;
1257
1258 QString prnStr;
1259 in >> prnStr >> year >> month >> day >> hour >> min >> sec;
1260 if (prnStr.at(0) == 'S') {
1261 _prn.set('S', prnStr.mid(1).toInt());
1262 }
1263 else {
1264 _prn.set('S', prnStr.toInt());
1265 }
1266
1267 if (year < 80) {
1268 year += 2000;
1269 }
1270 else if (year < 100) {
1271 year += 1900;
1272 }
1273
1274 _TOC.set(year, month, day, hour, min, sec);
1275
1276 if ( readDbl(line, pos[1], fieldLen, _agf0 ) ||
1277 readDbl(line, pos[2], fieldLen, _agf1 ) ||
1278 readDbl(line, pos[3], fieldLen, _TOW ) ) {
1279 _checkState = bad;
1280 return;
1281 }
1282 }
1283
1284 else if ( iLine == 1 ) {
1285 if ( readDbl(line, pos[0], fieldLen, _x_pos ) ||
1286 readDbl(line, pos[1], fieldLen, _x_velocity ) ||
1287 readDbl(line, pos[2], fieldLen, _x_acceleration) ||
1288 readDbl(line, pos[3], fieldLen, _health ) ) {
1289 _checkState = bad;
1290 return;
1291 }
1292 }
1293
1294 else if ( iLine == 2 ) {
1295 if ( readDbl(line, pos[0], fieldLen, _y_pos ) ||
1296 readDbl(line, pos[1], fieldLen, _y_velocity ) ||
1297 readDbl(line, pos[2], fieldLen, _y_acceleration ) ||
1298 readDbl(line, pos[3], fieldLen, _ura ) ) {
1299 _checkState = bad;
1300 return;
1301 }
1302 }
1303
1304 else if ( iLine == 3 ) {
1305 double iodn;
1306 if ( readDbl(line, pos[0], fieldLen, _z_pos ) ||
1307 readDbl(line, pos[1], fieldLen, _z_velocity ) ||
1308 readDbl(line, pos[2], fieldLen, _z_acceleration) ||
1309 readDbl(line, pos[3], fieldLen, iodn ) ) {
1310 _checkState = bad;
1311 return;
1312 _IODN = int(iodn);
1313 }
1314 }
1315 }
1316
1317 _x_pos *= 1.e3;
1318 _y_pos *= 1.e3;
1319 _z_pos *= 1.e3;
1320 _x_velocity *= 1.e3;
1321 _y_velocity *= 1.e3;
1322 _z_velocity *= 1.e3;
1323 _x_acceleration *= 1.e3;
1324 _y_acceleration *= 1.e3;
1325 _z_acceleration *= 1.e3;
1326}
1327
1328// Set SBAS Satellite Position
1329////////////////////////////////////////////////////////////////////////////
1330void t_ephSBAS::set(const sbasephemeris* ee) {
1331
1332 _prn.set('S', ee->satellite - PRN_SBAS_START + 20);
1333 _TOC.set(ee->GPSweek_TOE, double(ee->TOE));
1334
1335 _IODN = ee->IODN;
1336 _TOW = ee->TOW;
1337
1338 _agf0 = ee->agf0;
1339 _agf1 = ee->agf1;
1340
1341 _x_pos = ee->x_pos;
1342 _x_velocity = ee->x_velocity;
1343 _x_acceleration = ee->x_acceleration;
1344
1345 _y_pos = ee->y_pos;
1346 _y_velocity = ee->y_velocity;
1347 _y_acceleration = ee->y_acceleration;
1348
1349 _z_pos = ee->z_pos;
1350 _z_velocity = ee->z_velocity;
1351 _z_acceleration = ee->z_acceleration;
1352
1353 _ura = accuracyFromIndex(ee->URA, type());
1354
1355 _health = 0;
1356}
1357
1358// Compute SBAS Satellite Position (virtual)
1359////////////////////////////////////////////////////////////////////////////
1360t_irc t_ephSBAS::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
1361
1362 if (_checkState == bad) {
1363 return failure;
1364 }
1365
1366 bncTime tt(GPSweek, GPSweeks);
1367 double dt = tt - _TOC;
1368
1369 xc[0] = _x_pos + _x_velocity * dt + _x_acceleration * dt * dt / 2.0;
1370 xc[1] = _y_pos + _y_velocity * dt + _y_acceleration * dt * dt / 2.0;
1371 xc[2] = _z_pos + _z_velocity * dt + _z_acceleration * dt * dt / 2.0;
1372
1373 vv[0] = _x_velocity + _x_acceleration * dt;
1374 vv[1] = _y_velocity + _y_acceleration * dt;
1375 vv[2] = _z_velocity + _z_acceleration * dt;
1376
1377 xc[3] = _agf0 + _agf1 * dt;
1378
1379 return success;
1380}
1381
1382// RINEX Format String
1383//////////////////////////////////////////////////////////////////////////////
1384QString t_ephSBAS::toString(double version) const {
1385
1386 QString rnxStr = rinexDateStr(_TOC, _prn, version);
1387
1388 QTextStream out(&rnxStr);
1389
1390 out << QString("%1%2%3\n")
1391 .arg(_agf0, 19, 'e', 12)
1392 .arg(_agf1, 19, 'e', 12)
1393 .arg(_TOW, 19, 'e', 12);
1394
1395 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1396
1397 out << QString(fmt)
1398 .arg(1.e-3*_x_pos, 19, 'e', 12)
1399 .arg(1.e-3*_x_velocity, 19, 'e', 12)
1400 .arg(1.e-3*_x_acceleration, 19, 'e', 12)
1401 .arg(_health, 19, 'e', 12);
1402
1403 out << QString(fmt)
1404 .arg(1.e-3*_y_pos, 19, 'e', 12)
1405 .arg(1.e-3*_y_velocity, 19, 'e', 12)
1406 .arg(1.e-3*_y_acceleration, 19, 'e', 12)
1407 .arg(_ura, 19, 'e', 12);
1408
1409 out << QString(fmt)
1410 .arg(1.e-3*_z_pos, 19, 'e', 12)
1411 .arg(1.e-3*_z_velocity, 19, 'e', 12)
1412 .arg(1.e-3*_z_acceleration, 19, 'e', 12)
1413 .arg(double(_IODN), 19, 'e', 12);
1414
1415 return rnxStr;
1416}
1417
1418// Constructor
1419//////////////////////////////////////////////////////////////////////////////
1420t_ephBDS::t_ephBDS(float rnxVersion, const QStringList& lines) {
1421
1422 const int nLines = 8;
1423
1424 if (lines.size() != nLines) {
1425 _checkState = bad;
1426 return;
1427 }
1428
1429 // RINEX Format
1430 // ------------
1431 int fieldLen = 19;
1432 double TOEw;
1433
1434 int pos[4];
1435 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
1436 pos[1] = pos[0] + fieldLen;
1437 pos[2] = pos[1] + fieldLen;
1438 pos[3] = pos[2] + fieldLen;
1439
1440 // Read eight lines
1441 // ----------------
1442 for (int iLine = 0; iLine < nLines; iLine++) {
1443 QString line = lines[iLine];
1444
1445 if ( iLine == 0 ) {
1446 QTextStream in(line.left(pos[1]).toAscii());
1447
1448 int year, month, day, hour, min;
1449 double sec;
1450
1451 QString prnStr;
1452 in >> prnStr >> year >> month >> day >> hour >> min >> sec;
1453 if (prnStr.at(0) == 'C') {
1454 _prn.set('C', prnStr.mid(1).toInt());
1455 }
1456 else {
1457 _prn.set('C', prnStr.toInt());
1458 }
1459
1460 if (year < 80) {
1461 year += 2000;
1462 }
1463 else if (year < 100) {
1464 year += 1900;
1465 }
1466
1467 _TOC_bdt.set(year, month, day, hour, min, sec);
1468
1469 if ( readDbl(line, pos[1], fieldLen, _clock_bias ) ||
1470 readDbl(line, pos[2], fieldLen, _clock_drift ) ||
1471 readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
1472 _checkState = bad;
1473 return;
1474 }
1475 }
1476
1477 else if ( iLine == 1 ) {
1478 double aode;
1479 if ( readDbl(line, pos[0], fieldLen, aode ) ||
1480 readDbl(line, pos[1], fieldLen, _Crs ) ||
1481 readDbl(line, pos[2], fieldLen, _Delta_n) ||
1482 readDbl(line, pos[3], fieldLen, _M0 ) ) {
1483 _checkState = bad;
1484 return;
1485 }
1486 _AODE = int(aode);
1487 }
1488
1489 else if ( iLine == 2 ) {
1490 if ( readDbl(line, pos[0], fieldLen, _Cuc ) ||
1491 readDbl(line, pos[1], fieldLen, _e ) ||
1492 readDbl(line, pos[2], fieldLen, _Cus ) ||
1493 readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
1494 _checkState = bad;
1495 return;
1496 }
1497 }
1498
1499 else if ( iLine == 3 ) {
1500 if ( readDbl(line, pos[0], fieldLen, _TOEs ) ||
1501 readDbl(line, pos[1], fieldLen, _Cic ) ||
1502 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
1503 readDbl(line, pos[3], fieldLen, _Cis ) ) {
1504 _checkState = bad;
1505 return;
1506 }
1507 }
1508
1509 else if ( iLine == 4 ) {
1510 if ( readDbl(line, pos[0], fieldLen, _i0 ) ||
1511 readDbl(line, pos[1], fieldLen, _Crc ) ||
1512 readDbl(line, pos[2], fieldLen, _omega ) ||
1513 readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
1514 _checkState = bad;
1515 return;
1516 }
1517 }
1518
1519 else if ( iLine == 5 ) {
1520 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
1521 readDbl(line, pos[2], fieldLen, TOEw) ) {
1522 _checkState = bad;
1523 return;
1524 }
1525 }
1526
1527 else if ( iLine == 6 ) {
1528 double SatH1;
1529 if ( readDbl(line, pos[0], fieldLen, _URA ) ||
1530 readDbl(line, pos[1], fieldLen, SatH1) ||
1531 readDbl(line, pos[2], fieldLen, _TGD1) ||
1532 readDbl(line, pos[3], fieldLen, _TGD2) ) {
1533 _checkState = bad;
1534 return;
1535 }
1536 _SatH1 = int(SatH1);
1537 }
1538
1539 else if ( iLine == 7 ) {
1540 double aodc;
1541 if ( readDbl(line, pos[0], fieldLen, _TOTs) ||
1542 readDbl(line, pos[1], fieldLen, aodc) ) {
1543 _checkState = bad;
1544 return;
1545 }
1546 if (_TOTs == 0.9999e9) { // 0.9999e9 means not known (RINEX standard)
1547 _TOTs = _TOEs;
1548 }
1549 _AODC = int(aodc);
1550 }
1551 }
1552
1553 TOEw += 1356; // BDT -> GPS week number
1554 _TOE_bdt.set(int(TOEw), _TOEs);
1555
1556 // GPS->BDT
1557 // --------
1558 _TOC = _TOC_bdt + 14.0;
1559 _TOE = _TOE_bdt + 14.0;
1560
1561 // remark: actually should be computed from second_tot
1562 // but it seems to be unreliable in RINEX files
1563 _TOT = _TOC;
1564}
1565
1566// Set BDS Satellite Position
1567////////////////////////////////////////////////////////////////////////////
1568void t_ephBDS::set(const bdsephemeris* ee) {
1569
1570 // RTCM usage: set RINEX File entries to zero
1571 // ------------------------------------------
1572 _TOTs = 0.0;
1573 _TOEs = 0.0;
1574
1575 _receptDateTime = currentDateAndTimeGPS();
1576
1577 _prn.set('C', ee->satellite - PRN_BDS_START + 1);
1578
1579 _TOE_bdt.set(1356 + ee->BDSweek, ee->TOE);
1580 _TOE = _TOE_bdt + 14.0;
1581
1582 _TOC_bdt.set(1356 + ee->BDSweek, ee->TOC);
1583 _TOC = _TOC_bdt + 14.0;
1584
1585 _AODE = ee->AODE;
1586 _AODC = ee->AODC;
1587
1588 _clock_bias = ee->clock_bias;
1589 _clock_drift = ee->clock_drift;
1590 _clock_driftrate = ee->clock_driftrate;
1591
1592 _Crs = ee->Crs;
1593 _Delta_n = ee->Delta_n;
1594 _M0 = ee->M0;
1595
1596 _Cuc = ee->Cuc;
1597 _e = ee->e;
1598 _Cus = ee->Cus;
1599 _sqrt_A = ee->sqrt_A;
1600
1601 _Cic = ee->Cic;
1602 _OMEGA0 = ee->OMEGA0;
1603 _Cis = ee->Cis;
1604
1605 _i0 = ee->i0;
1606 _Crc = ee->Crc;
1607 _omega = ee->omega;
1608 _OMEGADOT = ee->OMEGADOT;
1609
1610 _IDOT = ee->IDOT;
1611
1612 _URA = accuracyFromIndex(ee->URAI, type());
1613 _SatH1 = (ee->flags & BDSEPHF_SATH1) ? 1: 0;
1614 _TGD1 = ee->TGD_B1_B3;
1615 _TGD2 = ee->TGD_B2_B3;
1616}
1617
1618// Compute BDS Satellite Position (virtual)
1619//////////////////////////////////////////////////////////////////////////////
1620t_irc t_ephBDS::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
1621
1622 if (_checkState == bad) {
1623 return failure;
1624 }
1625
1626 static const double gmBDS = 398.6004418e12;
1627 static const double omegaBDS = 7292115.0000e-11;
1628
1629 xc[0] = xc[1] = xc[2] = xc[3] = 0.0;
1630 vv[0] = vv[1] = vv[2] = 0.0;
1631
1632 bncTime tt(GPSweek, GPSweeks);
1633
1634 if (_sqrt_A == 0) {
1635 return failure;
1636 }
1637 double a0 = _sqrt_A * _sqrt_A;
1638
1639 double n0 = sqrt(gmBDS/(a0*a0*a0));
1640 double tk = tt - _TOE;
1641 double n = n0 + _Delta_n;
1642 double M = _M0 + n*tk;
1643 double E = M;
1644 double E_last;
1645 int nLoop = 0;
1646 do {
1647 E_last = E;
1648 E = M + _e*sin(E);
1649
1650 if (++nLoop == 100) {
1651 return failure;
1652 }
1653 } while ( fabs(E-E_last)*a0 > 0.001 );
1654
1655 double v = atan2(sqrt(1-_e*_e) * sin(E), cos(E) - _e);
1656 double u0 = v + _omega;
1657 double sin2u0 = sin(2*u0);
1658 double cos2u0 = cos(2*u0);
1659 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
1660 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
1661 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
1662 double xp = r*cos(u);
1663 double yp = r*sin(u);
1664 double toesec = (_TOE.gpssec() - 14.0);
1665
1666 double sinom = 0;
1667 double cosom = 0;
1668 double sini = 0;
1669 double cosi = 0;
1670
1671 const double iMaxGEO = 10.0 / 180.0 * M_PI;
1672
1673 // MEO/IGSO satellite
1674 // ------------------
1675 if (_i0 > iMaxGEO) {
1676 double OM = _OMEGA0 + (_OMEGADOT - omegaBDS)*tk - omegaBDS*toesec;
1677
1678 sinom = sin(OM);
1679 cosom = cos(OM);
1680 sini = sin(i);
1681 cosi = cos(i);
1682
1683 xc[0] = xp*cosom - yp*cosi*sinom;
1684 xc[1] = xp*sinom + yp*cosi*cosom;
1685 xc[2] = yp*sini;
1686 }
1687
1688 // GEO satellite
1689 // -------------
1690 else {
1691 double OM = _OMEGA0 + _OMEGADOT*tk - omegaBDS*toesec;
1692 double ll = omegaBDS*tk;
1693
1694 sinom = sin(OM);
1695 cosom = cos(OM);
1696 sini = sin(i);
1697 cosi = cos(i);
1698
1699 double xx = xp*cosom - yp*cosi*sinom;
1700 double yy = xp*sinom + yp*cosi*cosom;
1701 double zz = yp*sini;
1702
1703 Matrix R1 = BNC_PPP::t_astro::rotX(-5.0 / 180.0 * M_PI);
1704 Matrix R2 = BNC_PPP::t_astro::rotZ(ll);
1705
1706 ColumnVector X1(3); X1 << xx << yy << zz;
1707 ColumnVector X2 = R2*R1*X1;
1708
1709 xc[0] = X2(1);
1710 xc[1] = X2(2);
1711 xc[2] = X2(3);
1712 }
1713
1714 double tc = tt - _TOC;
1715 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc
1716 - 4.442807633e-10 * _e * sqrt(a0) *sin(E);
1717
1718 // Velocity
1719 // --------
1720 double tanv2 = tan(v/2);
1721 double dEdM = 1 / (1 - _e*cos(E));
1722 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2)
1723 / (1 + tanv2*tanv2) * dEdM * n;
1724 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
1725 double dotom = _OMEGADOT - t_CST::omega;
1726 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
1727 double dotr = a0 * _e*sin(E) * dEdM * n
1728 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
1729 double dotx = dotr*cos(u) - r*sin(u)*dotu;
1730 double doty = dotr*sin(u) + r*cos(u)*dotu;
1731
1732 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
1733 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
1734 + yp*sini*sinom*doti; // dX / di
1735
1736 vv[1] = sinom *dotx + cosi*cosom *doty
1737 + xp*cosom*dotom - yp*cosi*sinom*dotom
1738 - yp*sini*cosom*doti;
1739
1740 vv[2] = sini *doty + yp*cosi *doti;
1741
1742 // dotC = _clock_drift + _clock_driftrate*tc
1743 // - 4.442807633e-10*_e*sqrt(a0)*cos(E) * dEdM * n;
1744
1745 return success;
1746}
1747
1748// RINEX Format String
1749//////////////////////////////////////////////////////////////////////////////
1750QString t_ephBDS::toString(double version) const {
1751
1752 QString rnxStr = rinexDateStr(_TOC_bdt, _prn, version);
1753
1754 QTextStream out(&rnxStr);
1755
1756 out << QString("%1%2%3\n")
1757 .arg(_clock_bias, 19, 'e', 12)
1758 .arg(_clock_drift, 19, 'e', 12)
1759 .arg(_clock_driftrate, 19, 'e', 12);
1760
1761 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1762
1763 out << QString(fmt)
1764 .arg(double(_AODE), 19, 'e', 12)
1765 .arg(_Crs, 19, 'e', 12)
1766 .arg(_Delta_n, 19, 'e', 12)
1767 .arg(_M0, 19, 'e', 12);
1768
1769 out << QString(fmt)
1770 .arg(_Cuc, 19, 'e', 12)
1771 .arg(_e, 19, 'e', 12)
1772 .arg(_Cus, 19, 'e', 12)
1773 .arg(_sqrt_A, 19, 'e', 12);
1774
1775 double toes = _TOEs;
1776 if (!toes) { // RTCM stream input
1777 toes = _TOE_bdt.gpssec();
1778 }
1779 out << QString(fmt)
1780 .arg(toes, 19, 'e', 12)
1781 .arg(_Cic, 19, 'e', 12)
1782 .arg(_OMEGA0, 19, 'e', 12)
1783 .arg(_Cis, 19, 'e', 12);
1784
1785 out << QString(fmt)
1786 .arg(_i0, 19, 'e', 12)
1787 .arg(_Crc, 19, 'e', 12)
1788 .arg(_omega, 19, 'e', 12)
1789 .arg(_OMEGADOT, 19, 'e', 12);
1790
1791 out << QString(fmt)
1792 .arg(_IDOT, 19, 'e', 12)
1793 .arg(0.0, 19, 'e', 12)
1794 .arg(double(_TOE_bdt.gpsw() - 1356.0), 19, 'e', 12)
1795 .arg(0.0, 19, 'e', 12);
1796
1797 out << QString(fmt)
1798 .arg(_URA, 19, 'e', 12)
1799 .arg(double(_SatH1), 19, 'e', 12)
1800 .arg(_TGD1, 19, 'e', 12)
1801 .arg(_TGD2, 19, 'e', 12);
1802
1803 double tots = _TOTs;
1804 if (!tots) { // RTCM stream input
1805 tots = _TOE_bdt.gpssec();
1806 }
1807 out << QString(fmt)
1808 .arg(tots, 19, 'e', 12)
1809 .arg(double(_AODC), 19, 'e', 12)
1810 .arg("", 19, QChar(' '))
1811 .arg("", 19, QChar(' '));
1812 return rnxStr;
1813}
1814
Note: See TracBrowser for help on using the repository browser.