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

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