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

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