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

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