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

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

some complements with respect to Galileo navigation messages

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