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

Last change on this file since 5868 was 5856, checked in by mervart, 10 years ago
File size: 29.1 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 "timeutils.h"
12#include "bnctime.h"
13#include "bnccore.h"
14#include "bncutils.h"
15#include "PPP/pppInclude.h"
16
17using namespace std;
18
19// Constructor
20////////////////////////////////////////////////////////////////////////////
21t_eph::t_eph() {
22 _ok = false;
23 _orbCorr = 0;
24 _clkCorr = 0;
25}
26
27//
28////////////////////////////////////////////////////////////////////////////
29void t_eph::setOrbCorr(const BNC_PPP::t_orbCorr* orbCorr) {
30 delete _orbCorr;
31 _orbCorr = new BNC_PPP::t_orbCorr(*orbCorr);
32}
33
34//
35////////////////////////////////////////////////////////////////////////////
36void t_eph::setClkCorr(const BNC_PPP::t_clkCorr* clkCorr) {
37 delete _clkCorr;
38 _clkCorr = new BNC_PPP::t_clkCorr(*clkCorr);
39}
40
41//
42////////////////////////////////////////////////////////////////////////////
43t_irc t_eph::getCrd(const bncTime& tt, ColumnVector& xc,
44 ColumnVector& vv, bool useCorr) const {
45 xc.ReSize(4);
46 vv.ReSize(3);
47 position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data());
48 if (useCorr) {
49 if (_orbCorr && _clkCorr) {
50
51 double dtO = tt - _orbCorr->_time;
52 ColumnVector dx(3);
53 dx[0] = _orbCorr->_xr[0] + _orbCorr->_dotXr[0] * dtO;
54 dx[1] = _orbCorr->_xr[1] + _orbCorr->_dotXr[1] * dtO;
55 dx[2] = _orbCorr->_xr[2] + _orbCorr->_dotXr[2] * dtO;
56
57 if (_orbCorr->_system == 'R') {
58 RSW_to_XYZ(xc.Rows(1,3), vv.Rows(1,3), dx, dx);
59 }
60
61 xc[0] -= dx[0];
62 xc[1] -= dx[1];
63 xc[2] -= dx[2];
64
65 double dtC = tt - _clkCorr->_time;
66 xc[3] += _clkCorr->_dClk + _clkCorr->_dotDClk * dtC + _clkCorr->_dotDotDClk * dtC * dtC;
67 }
68 else {
69 return failure;
70 }
71 }
72 return success;
73}
74
75// Set GPS Satellite Position
76////////////////////////////////////////////////////////////////////////////
77void t_ephGPS::set(const gpsephemeris* ee) {
78
79 _receptDateTime = currentDateAndTimeGPS();
80
81 _prn.set('G', ee->satellite);
82
83 _TOC.set(ee->GPSweek, ee->TOC);
84 _clock_bias = ee->clock_bias;
85 _clock_drift = ee->clock_drift;
86 _clock_driftrate = ee->clock_driftrate;
87
88 _IODE = ee->IODE;
89 _Crs = ee->Crs;
90 _Delta_n = ee->Delta_n;
91 _M0 = ee->M0;
92
93 _Cuc = ee->Cuc;
94 _e = ee->e;
95 _Cus = ee->Cus;
96 _sqrt_A = ee->sqrt_A;
97
98 _TOEsec = ee->TOE;
99 _Cic = ee->Cic;
100 _OMEGA0 = ee->OMEGA0;
101 _Cis = ee->Cis;
102
103 _i0 = ee->i0;
104 _Crc = ee->Crc;
105 _omega = ee->omega;
106 _OMEGADOT = ee->OMEGADOT;
107
108 _IDOT = ee->IDOT;
109 _L2Codes = 0.0;
110 _TOEweek = ee->GPSweek;
111 _L2PFlag = 0.0;
112
113 if (ee->URAindex <= 6) {
114 _ura = ceil(10.0*pow(2.0, 1.0+((double)ee->URAindex)/2.0))/10.0;
115 }
116 else {
117 _ura = ceil(10.0*pow(2.0, ((double)ee->URAindex)/2.0))/10.0;
118 }
119 _health = ee->SVhealth;
120 _TGD = ee->TGD;
121 _IODC = ee->IODC;
122
123 _TOT = 0.9999e9;
124 _fitInterval = 0.0;
125
126 _ok = true;
127}
128
129// Compute GPS Satellite Position (virtual)
130////////////////////////////////////////////////////////////////////////////
131void t_ephGPS::position(int GPSweek, double GPSweeks,
132 double* xc,
133 double* vv) const {
134
135
136 static const double omegaEarth = 7292115.1467e-11;
137 static const double gmGRS = 398.6005e12;
138
139 memset(xc, 0, 4*sizeof(double));
140 memset(vv, 0, 3*sizeof(double));
141
142 double a0 = _sqrt_A * _sqrt_A;
143 if (a0 == 0) {
144 return;
145 }
146
147 double n0 = sqrt(gmGRS/(a0*a0*a0));
148
149 bncTime tt(GPSweek, GPSweeks);
150 double tk = tt - bncTime(int(_TOEweek), _TOEsec);
151
152 double n = n0 + _Delta_n;
153 double M = _M0 + n*tk;
154 double E = M;
155 double E_last;
156 do {
157 E_last = E;
158 E = M + _e*sin(E);
159 } while ( fabs(E-E_last)*a0 > 0.001 );
160 double v = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) );
161 double u0 = v + _omega;
162 double sin2u0 = sin(2*u0);
163 double cos2u0 = cos(2*u0);
164 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
165 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
166 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
167 double xp = r*cos(u);
168 double yp = r*sin(u);
169 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
170 omegaEarth*_TOEsec;
171
172 double sinom = sin(OM);
173 double cosom = cos(OM);
174 double sini = sin(i);
175 double cosi = cos(i);
176 xc[0] = xp*cosom - yp*cosi*sinom;
177 xc[1] = xp*sinom + yp*cosi*cosom;
178 xc[2] = yp*sini;
179
180 double tc = tt - _TOC;
181 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
182
183 // Velocity
184 // --------
185 double tanv2 = tan(v/2);
186 double dEdM = 1 / (1 - _e*cos(E));
187 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
188 * dEdM * n;
189 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
190 double dotom = _OMEGADOT - omegaEarth;
191 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
192 double dotr = a0 * _e*sin(E) * dEdM * n
193 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
194 double dotx = dotr*cos(u) - r*sin(u)*dotu;
195 double doty = dotr*sin(u) + r*cos(u)*dotu;
196
197 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
198 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
199 + yp*sini*sinom*doti; // dX / di
200
201 vv[1] = sinom *dotx + cosi*cosom *doty
202 + xp*cosom*dotom - yp*cosi*sinom*dotom
203 - yp*sini*cosom*doti;
204
205 vv[2] = sini *doty + yp*cosi *doti;
206
207 // Relativistic Correction
208 // -----------------------
209 xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c;
210}
211
212// Derivative of the state vector using a simple force model (static)
213////////////////////////////////////////////////////////////////////////////
214ColumnVector t_ephGlo::glo_deriv(double /* tt */, const ColumnVector& xv,
215 double* acc) {
216
217 // State vector components
218 // -----------------------
219 ColumnVector rr = xv.rows(1,3);
220 ColumnVector vv = xv.rows(4,6);
221
222 // Acceleration
223 // ------------
224 static const double gmWGS = 398.60044e12;
225 static const double AE = 6378136.0;
226 static const double OMEGA = 7292115.e-11;
227 static const double C20 = -1082.6257e-6;
228
229 double rho = rr.norm_Frobenius();
230 double t1 = -gmWGS/(rho*rho*rho);
231 double t2 = 3.0/2.0 * C20 * (gmWGS*AE*AE) / (rho*rho*rho*rho*rho);
232 double t3 = OMEGA * OMEGA;
233 double t4 = 2.0 * OMEGA;
234 double z2 = rr(3) * rr(3);
235
236 // Vector of derivatives
237 // ---------------------
238 ColumnVector va(6);
239 va(1) = vv(1);
240 va(2) = vv(2);
241 va(3) = vv(3);
242 va(4) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2) + acc[0];
243 va(5) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1) + acc[1];
244 va(6) = (t1 + t2*(3.0-5.0*z2/(rho*rho)) ) * rr(3) + acc[2];
245
246 return va;
247}
248
249// Compute Glonass Satellite Position (virtual)
250////////////////////////////////////////////////////////////////////////////
251void t_ephGlo::position(int GPSweek, double GPSweeks,
252 double* xc, double* vv) const {
253
254 static const double nominalStep = 10.0;
255
256 memset(xc, 0, 4*sizeof(double));
257 memset(vv, 0, 3*sizeof(double));
258
259 double dtPos = bncTime(GPSweek, GPSweeks) - _tt;
260
261 int nSteps = int(fabs(dtPos) / nominalStep) + 1;
262 double step = dtPos / nSteps;
263
264 double acc[3];
265 acc[0] = _x_acceleration * 1.e3;
266 acc[1] = _y_acceleration * 1.e3;
267 acc[2] = _z_acceleration * 1.e3;
268 for (int ii = 1; ii <= nSteps; ii++) {
269 _xv = rungeKutta4(_tt.gpssec(), _xv, step, acc, glo_deriv);
270 _tt = _tt + step;
271 }
272
273 // Position and Velocity
274 // ---------------------
275 xc[0] = _xv(1);
276 xc[1] = _xv(2);
277 xc[2] = _xv(3);
278
279 vv[0] = _xv(4);
280 vv[1] = _xv(5);
281 vv[2] = _xv(6);
282
283 // Clock Correction
284 // ----------------
285 double dtClk = bncTime(GPSweek, GPSweeks) - _TOC;
286 xc[3] = -_tau + _gamma * dtClk;
287}
288
289// IOD of Glonass Ephemeris (virtual)
290////////////////////////////////////////////////////////////////////////////
291int t_ephGlo::IOD() const {
292 bncTime tMoscow = _TOC - _gps_utc + 3 * 3600.0;
293 return int(tMoscow.daysec() / 900);
294}
295
296// Set Glonass Ephemeris
297////////////////////////////////////////////////////////////////////////////
298void t_ephGlo::set(const glonassephemeris* ee) {
299
300 _receptDateTime = currentDateAndTimeGPS();
301
302 _prn.set('R', ee->almanac_number);
303
304 int ww = ee->GPSWeek;
305 int tow = ee->GPSTOW;
306 updatetime(&ww, &tow, ee->tb*1000, 0); // Moscow -> GPS
307
308 // Check the day once more
309 // -----------------------
310 bool timeChanged = false;
311 {
312 const double secPerDay = 24 * 3600.0;
313 const double secPerWeek = 7 * secPerDay;
314 int ww_old = ww;
315 int tow_old = tow;
316 int currentWeek;
317 double currentSec;
318 currentGPSWeeks(currentWeek, currentSec);
319 bncTime currentTime(currentWeek, currentSec);
320 bncTime hTime(ww, (double) tow);
321
322 if (hTime - currentTime > secPerDay/2.0) {
323 timeChanged = true;
324 tow -= int(secPerDay);
325 if (tow < 0) {
326 tow += int(secPerWeek);
327 ww -= 1;
328 }
329 }
330 else if (hTime - currentTime < -secPerDay/2.0) {
331 timeChanged = true;
332 tow += int(secPerDay);
333 if (tow > secPerWeek) {
334 tow -= int(secPerWeek);
335 ww += 1;
336 }
337 }
338
339 if (false && timeChanged && BNC_CORE->mode() == t_bncCore::batchPostProcessing) {
340 bncTime newHTime(ww, (double) tow);
341 cout << "GLONASS " << ee->almanac_number << " Time Changed at "
342 << currentTime.datestr() << " " << currentTime.timestr()
343 << endl
344 << "old: " << hTime.datestr() << " " << hTime.timestr()
345 << endl
346 << "new: " << newHTime.datestr() << " " << newHTime.timestr()
347 << endl
348 << "eph: " << ee->GPSWeek << " " << ee->GPSTOW << " " << ee->tb
349 << endl
350 << "ww, tow (old): " << ww_old << " " << tow_old
351 << endl
352 << "ww, tow (new): " << ww << " " << tow
353 << endl << endl;
354 }
355 }
356
357 bncTime hlpTime(ww, (double) tow);
358 unsigned year, month, day;
359 hlpTime.civil_date(year, month, day);
360 _gps_utc = gnumleap(year, month, day);
361
362 _TOC.set(ww, tow);
363 _E = ee->E;
364 _tau = ee->tau;
365 _gamma = ee->gamma;
366 _x_pos = ee->x_pos;
367 _x_velocity = ee->x_velocity;
368 _x_acceleration = ee->x_acceleration;
369 _y_pos = ee->y_pos;
370 _y_velocity = ee->y_velocity;
371 _y_acceleration = ee->y_acceleration;
372 _z_pos = ee->z_pos;
373 _z_velocity = ee->z_velocity;
374 _z_acceleration = ee->z_acceleration;
375 _health = 0;
376 _frequency_number = ee->frequency_number;
377 _tki = ee->tk-3*60*60; if (_tki < 0) _tki += 86400;
378
379 // Initialize status vector
380 // ------------------------
381 _tt = _TOC;
382
383 _xv(1) = _x_pos * 1.e3;
384 _xv(2) = _y_pos * 1.e3;
385 _xv(3) = _z_pos * 1.e3;
386 _xv(4) = _x_velocity * 1.e3;
387 _xv(5) = _y_velocity * 1.e3;
388 _xv(6) = _z_velocity * 1.e3;
389
390 _ok = true;
391}
392
393// Set Galileo Satellite Position
394////////////////////////////////////////////////////////////////////////////
395void t_ephGal::set(const galileoephemeris* ee) {
396
397 _receptDateTime = currentDateAndTimeGPS();
398
399 _prn.set('E', ee->satellite);
400
401 _TOC.set(ee->Week, ee->TOC);
402 _clock_bias = ee->clock_bias;
403 _clock_drift = ee->clock_drift;
404 _clock_driftrate = ee->clock_driftrate;
405
406 _IODnav = ee->IODnav;
407 _Crs = ee->Crs;
408 _Delta_n = ee->Delta_n;
409 _M0 = ee->M0;
410
411 _Cuc = ee->Cuc;
412 _e = ee->e;
413 _Cus = ee->Cus;
414 _sqrt_A = ee->sqrt_A;
415
416 _TOEsec = _TOC.gpssec();
417 //// _TOEsec = ee->TOE; //// TODO:
418 _Cic = ee->Cic;
419 _OMEGA0 = ee->OMEGA0;
420 _Cis = ee->Cis;
421
422 _i0 = ee->i0;
423 _Crc = ee->Crc;
424 _omega = ee->omega;
425 _OMEGADOT = ee->OMEGADOT;
426
427 _IDOT = ee->IDOT;
428 _TOEweek = ee->Week;
429
430 _SISA = ee->SISA;
431 _E5aHS = ee->E5aHS;
432 _E5bHS = ee->E5bHS;
433 _BGD_1_5A = ee->BGD_1_5A;
434 _BGD_1_5B = ee->BGD_1_5B;
435
436 _TOT = 0.9999e9;
437
438 _flags = ee->flags;
439
440 _ok = true;
441}
442
443// Compute Galileo Satellite Position (virtual)
444////////////////////////////////////////////////////////////////////////////
445void t_ephGal::position(int GPSweek, double GPSweeks,
446 double* xc,
447 double* vv) const {
448
449 static const double omegaEarth = 7292115.1467e-11;
450 static const double gmWGS = 398.60044e12;
451
452 memset(xc, 0, 4*sizeof(double));
453 memset(vv, 0, 3*sizeof(double));
454
455 double a0 = _sqrt_A * _sqrt_A;
456 if (a0 == 0) {
457 return;
458 }
459
460 double n0 = sqrt(gmWGS/(a0*a0*a0));
461
462 bncTime tt(GPSweek, GPSweeks);
463 double tk = tt - bncTime(_TOC.gpsw(), _TOEsec);
464
465 double n = n0 + _Delta_n;
466 double M = _M0 + n*tk;
467 double E = M;
468 double E_last;
469 do {
470 E_last = E;
471 E = M + _e*sin(E);
472 } while ( fabs(E-E_last)*a0 > 0.001 );
473 double v = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) );
474 double u0 = v + _omega;
475 double sin2u0 = sin(2*u0);
476 double cos2u0 = cos(2*u0);
477 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
478 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
479 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
480 double xp = r*cos(u);
481 double yp = r*sin(u);
482 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
483 omegaEarth*_TOEsec;
484
485 double sinom = sin(OM);
486 double cosom = cos(OM);
487 double sini = sin(i);
488 double cosi = cos(i);
489 xc[0] = xp*cosom - yp*cosi*sinom;
490 xc[1] = xp*sinom + yp*cosi*cosom;
491 xc[2] = yp*sini;
492
493 double tc = tt - _TOC;
494 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
495
496 // Velocity
497 // --------
498 double tanv2 = tan(v/2);
499 double dEdM = 1 / (1 - _e*cos(E));
500 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
501 * dEdM * n;
502 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
503 double dotom = _OMEGADOT - omegaEarth;
504 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
505 double dotr = a0 * _e*sin(E) * dEdM * n
506 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
507 double dotx = dotr*cos(u) - r*sin(u)*dotu;
508 double doty = dotr*sin(u) + r*cos(u)*dotu;
509
510 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
511 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
512 + yp*sini*sinom*doti; // dX / di
513
514 vv[1] = sinom *dotx + cosi*cosom *doty
515 + xp*cosom*dotom - yp*cosi*sinom*dotom
516 - yp*sini*cosom*doti;
517
518 vv[2] = sini *doty + yp*cosi *doti;
519
520 // Relativistic Correction
521 // -----------------------
522 // xc(4) -= 4.442807633e-10 * _e * sqrt(a0) *sin(E);
523 xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c;
524}
525
526// Constructor
527//////////////////////////////////////////////////////////////////////////////
528t_ephGPS::t_ephGPS(float rnxVersion, const QStringList& lines) {
529
530 const int nLines = 8;
531
532 _ok = false;
533
534 if (lines.size() != nLines) {
535 return;
536 }
537
538 // RINEX Format
539 // ------------
540 int fieldLen = 19;
541
542 int pos[4];
543 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
544 pos[1] = pos[0] + fieldLen;
545 pos[2] = pos[1] + fieldLen;
546 pos[3] = pos[2] + fieldLen;
547
548 // Read eight lines
549 // ----------------
550 for (int iLine = 0; iLine < nLines; iLine++) {
551 QString line = lines[iLine];
552
553 if ( iLine == 0 ) {
554 QTextStream in(line.left(pos[1]).toAscii());
555
556 int year, month, day, hour, min;
557 double sec;
558
559 QString prnStr;
560 in >> prnStr >> year >> month >> day >> hour >> min >> sec;
561 if (prnStr.at(0) == 'G') {
562 _prn.set('G', prnStr.mid(1).toInt());
563 }
564 else {
565 _prn.set('G', prnStr.toInt());
566 }
567
568 if (year < 80) {
569 year += 2000;
570 }
571 else if (year < 100) {
572 year += 1900;
573 }
574
575 _TOC.set(year, month, day, hour, min, sec);
576
577 if ( readDbl(line, pos[1], fieldLen, _clock_bias ) ||
578 readDbl(line, pos[2], fieldLen, _clock_drift ) ||
579 readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
580 return;
581 }
582 }
583
584 else if ( iLine == 1 ) {
585 if ( readDbl(line, pos[0], fieldLen, _IODE ) ||
586 readDbl(line, pos[1], fieldLen, _Crs ) ||
587 readDbl(line, pos[2], fieldLen, _Delta_n) ||
588 readDbl(line, pos[3], fieldLen, _M0 ) ) {
589 return;
590 }
591 }
592
593 else if ( iLine == 2 ) {
594 if ( readDbl(line, pos[0], fieldLen, _Cuc ) ||
595 readDbl(line, pos[1], fieldLen, _e ) ||
596 readDbl(line, pos[2], fieldLen, _Cus ) ||
597 readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
598 return;
599 }
600 }
601
602 else if ( iLine == 3 ) {
603 if ( readDbl(line, pos[0], fieldLen, _TOEsec) ||
604 readDbl(line, pos[1], fieldLen, _Cic ) ||
605 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
606 readDbl(line, pos[3], fieldLen, _Cis ) ) {
607 return;
608 }
609 }
610
611 else if ( iLine == 4 ) {
612 if ( readDbl(line, pos[0], fieldLen, _i0 ) ||
613 readDbl(line, pos[1], fieldLen, _Crc ) ||
614 readDbl(line, pos[2], fieldLen, _omega ) ||
615 readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
616 return;
617 }
618 }
619
620 else if ( iLine == 5 ) {
621 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
622 readDbl(line, pos[1], fieldLen, _L2Codes) ||
623 readDbl(line, pos[2], fieldLen, _TOEweek ) ||
624 readDbl(line, pos[3], fieldLen, _L2PFlag) ) {
625 return;
626 }
627 }
628
629 else if ( iLine == 6 ) {
630 if ( readDbl(line, pos[0], fieldLen, _ura ) ||
631 readDbl(line, pos[1], fieldLen, _health) ||
632 readDbl(line, pos[2], fieldLen, _TGD ) ||
633 readDbl(line, pos[3], fieldLen, _IODC ) ) {
634 return;
635 }
636 }
637
638 else if ( iLine == 7 ) {
639 if ( readDbl(line, pos[0], fieldLen, _TOT) ) {
640 return;
641 }
642 readDbl(line, pos[1], fieldLen, _fitInterval); // _fitInterval optional
643 }
644 }
645
646 _ok = true;
647}
648
649// Constructor
650//////////////////////////////////////////////////////////////////////////////
651t_ephGlo::t_ephGlo(float rnxVersion, const QStringList& lines) {
652
653 const int nLines = 4;
654
655 _ok = false;
656
657 if (lines.size() != nLines) {
658 return;
659 }
660
661 // RINEX Format
662 // ------------
663 int fieldLen = 19;
664
665 int pos[4];
666 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
667 pos[1] = pos[0] + fieldLen;
668 pos[2] = pos[1] + fieldLen;
669 pos[3] = pos[2] + fieldLen;
670
671 // Read four lines
672 // ---------------
673 for (int iLine = 0; iLine < nLines; iLine++) {
674 QString line = lines[iLine];
675
676 if ( iLine == 0 ) {
677 QTextStream in(line.left(pos[1]).toAscii());
678
679 int year, month, day, hour, min;
680 double sec;
681
682 QString prnStr;
683 in >> prnStr >> year >> month >> day >> hour >> min >> sec;
684 if (prnStr.at(0) == 'R') {
685 _prn.set('R', prnStr.mid(1).toInt());
686 }
687 else {
688 _prn.set('R', prnStr.toInt());
689 }
690
691 if (year < 80) {
692 year += 2000;
693 }
694 else if (year < 100) {
695 year += 1900;
696 }
697
698 _gps_utc = gnumleap(year, month, day);
699
700 _TOC.set(year, month, day, hour, min, sec);
701 _TOC = _TOC + _gps_utc;
702
703 if ( readDbl(line, pos[1], fieldLen, _tau ) ||
704 readDbl(line, pos[2], fieldLen, _gamma) ||
705 readDbl(line, pos[3], fieldLen, _tki ) ) {
706 return;
707 }
708
709 _tau = -_tau;
710 }
711
712 else if ( iLine == 1 ) {
713 if ( readDbl(line, pos[0], fieldLen, _x_pos ) ||
714 readDbl(line, pos[1], fieldLen, _x_velocity ) ||
715 readDbl(line, pos[2], fieldLen, _x_acceleration) ||
716 readDbl(line, pos[3], fieldLen, _health ) ) {
717 return;
718 }
719 }
720
721 else if ( iLine == 2 ) {
722 if ( readDbl(line, pos[0], fieldLen, _y_pos ) ||
723 readDbl(line, pos[1], fieldLen, _y_velocity ) ||
724 readDbl(line, pos[2], fieldLen, _y_acceleration ) ||
725 readDbl(line, pos[3], fieldLen, _frequency_number) ) {
726 return;
727 }
728 }
729
730 else if ( iLine == 3 ) {
731 if ( readDbl(line, pos[0], fieldLen, _z_pos ) ||
732 readDbl(line, pos[1], fieldLen, _z_velocity ) ||
733 readDbl(line, pos[2], fieldLen, _z_acceleration) ||
734 readDbl(line, pos[3], fieldLen, _E ) ) {
735 return;
736 }
737 }
738 }
739
740 // Initialize status vector
741 // ------------------------
742 _tt = _TOC;
743 _xv.ReSize(6);
744 _xv(1) = _x_pos * 1.e3;
745 _xv(2) = _y_pos * 1.e3;
746 _xv(3) = _z_pos * 1.e3;
747 _xv(4) = _x_velocity * 1.e3;
748 _xv(5) = _y_velocity * 1.e3;
749 _xv(6) = _z_velocity * 1.e3;
750
751 _ok = true;
752}
753
754// Constructor
755//////////////////////////////////////////////////////////////////////////////
756t_ephGal::t_ephGal(float rnxVersion, const QStringList& lines) {
757
758 const int nLines = 8;
759
760 _ok = false;
761
762 if (lines.size() != nLines) {
763 return;
764 }
765
766 // RINEX Format
767 // ------------
768 int fieldLen = 19;
769
770 int pos[4];
771 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
772 pos[1] = pos[0] + fieldLen;
773 pos[2] = pos[1] + fieldLen;
774 pos[3] = pos[2] + fieldLen;
775
776 // Read eight lines
777 // ----------------
778 for (int iLine = 0; iLine < nLines; iLine++) {
779 QString line = lines[iLine];
780
781 if ( iLine == 0 ) {
782 QTextStream in(line.left(pos[1]).toAscii());
783
784 int year, month, day, hour, min;
785 double sec;
786
787 QString prnStr;
788 in >> prnStr >> year >> month >> day >> hour >> min >> sec;
789 if (prnStr.at(0) == 'E') {
790 _prn.set('E', prnStr.mid(1).toInt());
791 }
792 else {
793 _prn.set('E', prnStr.toInt());
794 }
795
796 if (year < 80) {
797 year += 2000;
798 }
799 else if (year < 100) {
800 year += 1900;
801 }
802
803 _TOC.set(year, month, day, hour, min, sec);
804
805 if ( readDbl(line, pos[1], fieldLen, _clock_bias ) ||
806 readDbl(line, pos[2], fieldLen, _clock_drift ) ||
807 readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
808 return;
809 }
810 }
811
812 else if ( iLine == 1 ) {
813 if ( readDbl(line, pos[0], fieldLen, _IODnav ) ||
814 readDbl(line, pos[1], fieldLen, _Crs ) ||
815 readDbl(line, pos[2], fieldLen, _Delta_n) ||
816 readDbl(line, pos[3], fieldLen, _M0 ) ) {
817 return;
818 }
819 }
820
821 else if ( iLine == 2 ) {
822 if ( readDbl(line, pos[0], fieldLen, _Cuc ) ||
823 readDbl(line, pos[1], fieldLen, _e ) ||
824 readDbl(line, pos[2], fieldLen, _Cus ) ||
825 readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
826 return;
827 }
828 }
829
830 else if ( iLine == 3 ) {
831 if ( readDbl(line, pos[0], fieldLen, _TOEsec) ||
832 readDbl(line, pos[1], fieldLen, _Cic ) ||
833 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
834 readDbl(line, pos[3], fieldLen, _Cis ) ) {
835 return;
836 }
837 }
838
839 else if ( iLine == 4 ) {
840 if ( readDbl(line, pos[0], fieldLen, _i0 ) ||
841 readDbl(line, pos[1], fieldLen, _Crc ) ||
842 readDbl(line, pos[2], fieldLen, _omega ) ||
843 readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
844 return;
845 }
846 }
847
848 else if ( iLine == 5 ) {
849 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
850 readDbl(line, pos[2], fieldLen, _TOEweek) ) {
851 return;
852 }
853 }
854
855 else if ( iLine == 6 ) {
856 if ( readDbl(line, pos[0], fieldLen, _SISA ) ||
857 readDbl(line, pos[1], fieldLen, _E5aHS ) ||
858 readDbl(line, pos[2], fieldLen, _BGD_1_5A) ||
859 readDbl(line, pos[3], fieldLen, _BGD_1_5B) ) {
860 return;
861 }
862 }
863
864 else if ( iLine == 7 ) {
865 if ( readDbl(line, pos[0], fieldLen, _TOT) ) {
866 return;
867 }
868 }
869 }
870
871 _ok = true;
872}
873
874//
875//////////////////////////////////////////////////////////////////////////////
876QString t_eph::rinexDateStr(const bncTime& tt, const t_prn& prn, double version) {
877 QString prnStr(prn.toString().c_str());
878 return rinexDateStr(tt, prnStr, version);
879}
880
881//
882//////////////////////////////////////////////////////////////////////////////
883QString t_eph::rinexDateStr(const bncTime& tt, const QString& prnStr, double version) {
884
885 QString datStr;
886
887 unsigned year, month, day, hour, min;
888 double sec;
889 tt.civil_date(year, month, day);
890 tt.civil_time(hour, min, sec);
891
892 QTextStream out(&datStr);
893
894 if (version < 3.0) {
895 QString prnHlp = prnStr.mid(1,2); if (prnHlp[0] == '0') prnHlp[0] = ' ';
896 out << prnHlp << QString(" %1 %2 %3 %4 %5%6")
897 .arg(year % 100, 2, 10, QChar('0'))
898 .arg(month, 2)
899 .arg(day, 2)
900 .arg(hour, 2)
901 .arg(min, 2)
902 .arg(sec, 5, 'f',1);
903 }
904 else {
905 out << prnStr << QString(" %1 %2 %3 %4 %5 %6")
906 .arg(year, 4)
907 .arg(month, 2, 10, QChar('0'))
908 .arg(day, 2, 10, QChar('0'))
909 .arg(hour, 2, 10, QChar('0'))
910 .arg(min, 2, 10, QChar('0'))
911 .arg(int(sec), 2, 10, QChar('0'));
912 }
913
914 return datStr;
915}
916
917// RINEX Format String
918//////////////////////////////////////////////////////////////////////////////
919QString t_ephGPS::toString(double version) const {
920
921 QString rnxStr = rinexDateStr(_TOC, _prn, version);
922
923 QTextStream out(&rnxStr);
924
925 out << QString("%1%2%3\n")
926 .arg(_clock_bias, 19, 'e', 12)
927 .arg(_clock_drift, 19, 'e', 12)
928 .arg(_clock_driftrate, 19, 'e', 12);
929
930 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
931
932 out << QString(fmt)
933 .arg(_IODE, 19, 'e', 12)
934 .arg(_Crs, 19, 'e', 12)
935 .arg(_Delta_n, 19, 'e', 12)
936 .arg(_M0, 19, 'e', 12);
937
938 out << QString(fmt)
939 .arg(_Cuc, 19, 'e', 12)
940 .arg(_e, 19, 'e', 12)
941 .arg(_Cus, 19, 'e', 12)
942 .arg(_sqrt_A, 19, 'e', 12);
943
944 out << QString(fmt)
945 .arg(_TOEsec, 19, 'e', 12)
946 .arg(_Cic, 19, 'e', 12)
947 .arg(_OMEGA0, 19, 'e', 12)
948 .arg(_Cis, 19, 'e', 12);
949
950 out << QString(fmt)
951 .arg(_i0, 19, 'e', 12)
952 .arg(_Crc, 19, 'e', 12)
953 .arg(_omega, 19, 'e', 12)
954 .arg(_OMEGADOT, 19, 'e', 12);
955
956 out << QString(fmt)
957 .arg(_IDOT, 19, 'e', 12)
958 .arg(_L2Codes, 19, 'e', 12)
959 .arg(_TOEweek, 19, 'e', 12)
960 .arg(_L2PFlag, 19, 'e', 12);
961
962 out << QString(fmt)
963 .arg(_ura, 19, 'e', 12)
964 .arg(_health, 19, 'e', 12)
965 .arg(_TGD, 19, 'e', 12)
966 .arg(_IODC, 19, 'e', 12);
967
968 out << QString(fmt)
969 .arg(_TOT, 19, 'e', 12)
970 .arg(_fitInterval, 19, 'e', 12)
971 .arg("", 19, QChar(' '))
972 .arg("", 19, QChar(' '));
973
974 return rnxStr;
975}
976
977// RINEX Format String
978//////////////////////////////////////////////////////////////////////////////
979QString t_ephGlo::toString(double version) const {
980
981 QString rnxStr = rinexDateStr(_TOC-_gps_utc, _prn, version);
982
983 QTextStream out(&rnxStr);
984
985 out << QString("%1%2%3\n")
986 .arg(-_tau, 19, 'e', 12)
987 .arg(_gamma, 19, 'e', 12)
988 .arg(_tki, 19, 'e', 12);
989
990 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
991
992 out << QString(fmt)
993 .arg(_x_pos, 19, 'e', 12)
994 .arg(_x_velocity, 19, 'e', 12)
995 .arg(_x_acceleration, 19, 'e', 12)
996 .arg(_health, 19, 'e', 12);
997
998 out << QString(fmt)
999 .arg(_y_pos, 19, 'e', 12)
1000 .arg(_y_velocity, 19, 'e', 12)
1001 .arg(_y_acceleration, 19, 'e', 12)
1002 .arg(_frequency_number, 19, 'e', 12);
1003
1004 out << QString(fmt)
1005 .arg(_z_pos, 19, 'e', 12)
1006 .arg(_z_velocity, 19, 'e', 12)
1007 .arg(_z_acceleration, 19, 'e', 12)
1008 .arg(_E, 19, 'e', 12);
1009
1010 return rnxStr;
1011}
1012
1013// RINEX Format String
1014//////////////////////////////////////////////////////////////////////////////
1015QString t_ephGal::toString(double version) const {
1016
1017 QString rnxStr = rinexDateStr(_TOC, _prn, version);
1018
1019 QTextStream out(&rnxStr);
1020
1021 out << QString("%1%2%3\n")
1022 .arg(_clock_bias, 19, 'e', 12)
1023 .arg(_clock_drift, 19, 'e', 12)
1024 .arg(_clock_driftrate, 19, 'e', 12);
1025
1026 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1027
1028 out << QString(fmt)
1029 .arg(_IODnav, 19, 'e', 12)
1030 .arg(_Crs, 19, 'e', 12)
1031 .arg(_Delta_n, 19, 'e', 12)
1032 .arg(_M0, 19, 'e', 12);
1033
1034 out << QString(fmt)
1035 .arg(_Cuc, 19, 'e', 12)
1036 .arg(_e, 19, 'e', 12)
1037 .arg(_Cus, 19, 'e', 12)
1038 .arg(_sqrt_A, 19, 'e', 12);
1039
1040 out << QString(fmt)
1041 .arg(_TOEsec, 19, 'e', 12)
1042 .arg(_Cic, 19, 'e', 12)
1043 .arg(_OMEGA0, 19, 'e', 12)
1044 .arg(_Cis, 19, 'e', 12);
1045
1046 out << QString(fmt)
1047 .arg(_i0, 19, 'e', 12)
1048 .arg(_Crc, 19, 'e', 12)
1049 .arg(_omega, 19, 'e', 12)
1050 .arg(_OMEGADOT, 19, 'e', 12);
1051
1052 int dataSource = 0;
1053 double HS = 0.0;
1054 if ( (_flags & GALEPHF_INAV) == GALEPHF_INAV ) {
1055 dataSource |= (1<<0);
1056 dataSource |= (1<<9);
1057 HS = _E5bHS;
1058 }
1059 else if ( (_flags & GALEPHF_FNAV) == GALEPHF_FNAV ) {
1060 dataSource |= (1<<1);
1061 dataSource |= (1<<8);
1062 HS = _E5aHS;
1063 }
1064 out << QString(fmt)
1065 .arg(_IDOT, 19, 'e', 12)
1066 .arg(double(dataSource), 19, 'e', 12)
1067 .arg(_TOEweek, 19, 'e', 12)
1068 .arg(0.0, 19, 'e', 12);
1069
1070 out << QString(fmt)
1071 .arg(_SISA, 19, 'e', 12)
1072 .arg(HS, 19, 'e', 12)
1073 .arg(_BGD_1_5A, 19, 'e', 12)
1074 .arg(_BGD_1_5B, 19, 'e', 12);
1075
1076 out << QString(fmt)
1077 .arg(_TOT, 19, 'e', 12)
1078 .arg("", 19, QChar(' '))
1079 .arg("", 19, QChar(' '))
1080 .arg("", 19, QChar(' '));
1081
1082 return rnxStr;
1083}
1084
Note: See TracBrowser for help on using the repository browser.