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

Last change on this file since 6603 was 6603, checked in by stuerze, 7 years ago

BDS SatH1 added

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