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

Last change on this file since 6550 was 6537, checked in by mervart, 11 years ago
File size: 44.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#include "pppModel.h"
17
18using namespace std;
19
20// Constructor
21////////////////////////////////////////////////////////////////////////////
22t_eph::t_eph() {
23 _checkState = unchecked;
24 _orbCorr = 0;
25 _clkCorr = 0;
26}
27
28//
29////////////////////////////////////////////////////////////////////////////
30void t_eph::setOrbCorr(const t_orbCorr* orbCorr) {
31 delete _orbCorr;
32 _orbCorr = new t_orbCorr(*orbCorr);
33}
34
35//
36////////////////////////////////////////////////////////////////////////////
37void t_eph::setClkCorr(const t_clkCorr* clkCorr) {
38 delete _clkCorr;
39 _clkCorr = new t_clkCorr(*clkCorr);
40}
41
42//
43////////////////////////////////////////////////////////////////////////////
44t_irc t_eph::getCrd(const bncTime& tt, ColumnVector& xc, ColumnVector& vv, bool useCorr) const {
45
46 if (_checkState == bad) {
47 return failure;
48 }
49
50 xc.ReSize(4);
51 vv.ReSize(3);
52 if (position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data()) != success) {
53 return failure;
54 }
55 if (useCorr) {
56 if (_orbCorr && _clkCorr) {
57
58 double dtO = tt - _orbCorr->_time;
59 ColumnVector dx(3);
60 dx[0] = _orbCorr->_xr[0] + _orbCorr->_dotXr[0] * dtO;
61 dx[1] = _orbCorr->_xr[1] + _orbCorr->_dotXr[1] * dtO;
62 dx[2] = _orbCorr->_xr[2] + _orbCorr->_dotXr[2] * dtO;
63
64 if (_orbCorr->_system == 'R') {
65 RSW_to_XYZ(xc.Rows(1,3), vv.Rows(1,3), dx, dx);
66 }
67
68 xc[0] -= dx[0];
69 xc[1] -= dx[1];
70 xc[2] -= dx[2];
71
72 double dtC = tt - _clkCorr->_time;
73 xc[3] += _clkCorr->_dClk + _clkCorr->_dotDClk * dtC + _clkCorr->_dotDotDClk * dtC * dtC;
74 }
75 else {
76 return failure;
77 }
78 }
79 return success;
80}
81
82// Set GPS Satellite Position
83////////////////////////////////////////////////////////////////////////////
84void t_ephGPS::set(const gpsephemeris* ee) {
85
86 _receptDateTime = currentDateAndTimeGPS();
87
88 if (PRN_GPS_START <= ee->satellite && ee->satellite <= PRN_GPS_END) {
89 _prn.set('G', ee->satellite);
90 }
91 else if (PRN_QZSS_START <= ee->satellite && ee->satellite <= PRN_QZSS_END) {
92 _prn.set('J', ee->satellite - PRN_QZSS_START + 1);
93 }
94 else {
95 _checkState = bad;
96 return;
97 }
98
99 _TOC.set(ee->GPSweek, ee->TOC);
100 _clock_bias = ee->clock_bias;
101 _clock_drift = ee->clock_drift;
102 _clock_driftrate = ee->clock_driftrate;
103
104 _IODE = ee->IODE;
105 _Crs = ee->Crs;
106 _Delta_n = ee->Delta_n;
107 _M0 = ee->M0;
108
109 _Cuc = ee->Cuc;
110 _e = ee->e;
111 _Cus = ee->Cus;
112 _sqrt_A = ee->sqrt_A;
113
114 _TOEsec = ee->TOE;
115 _Cic = ee->Cic;
116 _OMEGA0 = ee->OMEGA0;
117 _Cis = ee->Cis;
118
119 _i0 = ee->i0;
120 _Crc = ee->Crc;
121 _omega = ee->omega;
122 _OMEGADOT = ee->OMEGADOT;
123
124 _IDOT = ee->IDOT;
125 _L2Codes = 0.0;
126 _TOEweek = ee->GPSweek;
127 _L2PFlag = 0.0;
128
129 if (ee->URAindex <= 6) {
130 _ura = ceil(10.0*pow(2.0, 1.0+((double)ee->URAindex)/2.0))/10.0;
131 }
132 else {
133 _ura = ceil(10.0*pow(2.0, ((double)ee->URAindex)/2.0))/10.0;
134 }
135 _health = ee->SVhealth;
136 _TGD = ee->TGD;
137 _IODC = ee->IODC;
138
139 _TOT = 0.9999e9;
140 _fitInterval = 0.0;
141}
142
143// Compute GPS Satellite Position (virtual)
144////////////////////////////////////////////////////////////////////////////
145t_irc t_ephGPS::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
146
147 if (_checkState == bad) {
148 return failure;
149 }
150
151 static const double omegaEarth = 7292115.1467e-11;
152 static const double gmGRS = 398.6005e12;
153
154 memset(xc, 0, 4*sizeof(double));
155 memset(vv, 0, 3*sizeof(double));
156
157 double a0 = _sqrt_A * _sqrt_A;
158 if (a0 == 0) {
159 return failure;
160 }
161
162 double n0 = sqrt(gmGRS/(a0*a0*a0));
163
164 bncTime tt(GPSweek, GPSweeks);
165 double tk = tt - bncTime(int(_TOEweek), _TOEsec);
166
167 double n = n0 + _Delta_n;
168 double M = _M0 + n*tk;
169 double E = M;
170 double E_last;
171 do {
172 E_last = E;
173 E = M + _e*sin(E);
174 } while ( fabs(E-E_last)*a0 > 0.001 );
175 double v = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) );
176 double u0 = v + _omega;
177 double sin2u0 = sin(2*u0);
178 double cos2u0 = cos(2*u0);
179 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
180 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
181 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
182 double xp = r*cos(u);
183 double yp = r*sin(u);
184 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
185 omegaEarth*_TOEsec;
186
187 double sinom = sin(OM);
188 double cosom = cos(OM);
189 double sini = sin(i);
190 double cosi = cos(i);
191 xc[0] = xp*cosom - yp*cosi*sinom;
192 xc[1] = xp*sinom + yp*cosi*cosom;
193 xc[2] = yp*sini;
194
195 double tc = tt - _TOC;
196 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
197
198 // Velocity
199 // --------
200 double tanv2 = tan(v/2);
201 double dEdM = 1 / (1 - _e*cos(E));
202 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
203 * dEdM * n;
204 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
205 double dotom = _OMEGADOT - omegaEarth;
206 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
207 double dotr = a0 * _e*sin(E) * dEdM * n
208 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
209 double dotx = dotr*cos(u) - r*sin(u)*dotu;
210 double doty = dotr*sin(u) + r*cos(u)*dotu;
211
212 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
213 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
214 + yp*sini*sinom*doti; // dX / di
215
216 vv[1] = sinom *dotx + cosi*cosom *doty
217 + xp*cosom*dotom - yp*cosi*sinom*dotom
218 - yp*sini*cosom*doti;
219
220 vv[2] = sini *doty + yp*cosi *doti;
221
222 // Relativistic Correction
223 // -----------------------
224 xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c;
225
226 return success;
227}
228
229// Derivative of the state vector using a simple force model (static)
230////////////////////////////////////////////////////////////////////////////
231ColumnVector t_ephGlo::glo_deriv(double /* tt */, const ColumnVector& xv,
232 double* acc) {
233
234 // State vector components
235 // -----------------------
236 ColumnVector rr = xv.rows(1,3);
237 ColumnVector vv = xv.rows(4,6);
238
239 // Acceleration
240 // ------------
241 static const double gmWGS = 398.60044e12;
242 static const double AE = 6378136.0;
243 static const double OMEGA = 7292115.e-11;
244 static const double C20 = -1082.6257e-6;
245
246 double rho = rr.norm_Frobenius();
247 double t1 = -gmWGS/(rho*rho*rho);
248 double t2 = 3.0/2.0 * C20 * (gmWGS*AE*AE) / (rho*rho*rho*rho*rho);
249 double t3 = OMEGA * OMEGA;
250 double t4 = 2.0 * OMEGA;
251 double z2 = rr(3) * rr(3);
252
253 // Vector of derivatives
254 // ---------------------
255 ColumnVector va(6);
256 va(1) = vv(1);
257 va(2) = vv(2);
258 va(3) = vv(3);
259 va(4) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2) + acc[0];
260 va(5) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1) + acc[1];
261 va(6) = (t1 + t2*(3.0-5.0*z2/(rho*rho)) ) * rr(3) + acc[2];
262
263 return va;
264}
265
266// Compute Glonass Satellite Position (virtual)
267////////////////////////////////////////////////////////////////////////////
268t_irc t_ephGlo::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
269
270 if (_checkState == bad) {
271 return failure;
272 }
273
274 static const double nominalStep = 10.0;
275
276 memset(xc, 0, 4*sizeof(double));
277 memset(vv, 0, 3*sizeof(double));
278
279 double dtPos = bncTime(GPSweek, GPSweeks) - _tt;
280
281 if (fabs(dtPos) > 24*3600.0) {
282 return failure;
283 }
284
285 int nSteps = int(fabs(dtPos) / nominalStep) + 1;
286 double step = dtPos / nSteps;
287
288 double acc[3];
289 acc[0] = _x_acceleration * 1.e3;
290 acc[1] = _y_acceleration * 1.e3;
291 acc[2] = _z_acceleration * 1.e3;
292 for (int ii = 1; ii <= nSteps; ii++) {
293 _xv = rungeKutta4(_tt.gpssec(), _xv, step, acc, glo_deriv);
294 _tt = _tt + step;
295 }
296
297 // Position and Velocity
298 // ---------------------
299 xc[0] = _xv(1);
300 xc[1] = _xv(2);
301 xc[2] = _xv(3);
302
303 vv[0] = _xv(4);
304 vv[1] = _xv(5);
305 vv[2] = _xv(6);
306
307 // Clock Correction
308 // ----------------
309 double dtClk = bncTime(GPSweek, GPSweeks) - _TOC;
310 xc[3] = -_tau + _gamma * dtClk;
311
312 return success;
313}
314
315// IOD of Glonass Ephemeris (virtual)
316////////////////////////////////////////////////////////////////////////////
317int t_ephGlo::IOD() const {
318 bncTime tMoscow = _TOC - _gps_utc + 3 * 3600.0;
319 return int(tMoscow.daysec() / 900);
320}
321
322// Set Glonass Ephemeris
323////////////////////////////////////////////////////////////////////////////
324void t_ephGlo::set(const glonassephemeris* ee) {
325
326 _receptDateTime = currentDateAndTimeGPS();
327
328 _prn.set('R', ee->almanac_number);
329
330 int ww = ee->GPSWeek;
331 int tow = ee->GPSTOW;
332 updatetime(&ww, &tow, ee->tb*1000, 0); // Moscow -> GPS
333
334 // Check the day once more
335 // -----------------------
336 bool timeChanged = false;
337 {
338 const double secPerDay = 24 * 3600.0;
339 const double secPerWeek = 7 * secPerDay;
340 int ww_old = ww;
341 int tow_old = tow;
342 int currentWeek;
343 double currentSec;
344 currentGPSWeeks(currentWeek, currentSec);
345 bncTime currentTime(currentWeek, currentSec);
346 bncTime hTime(ww, (double) tow);
347
348 if (hTime - currentTime > secPerDay/2.0) {
349 timeChanged = true;
350 tow -= int(secPerDay);
351 if (tow < 0) {
352 tow += int(secPerWeek);
353 ww -= 1;
354 }
355 }
356 else if (hTime - currentTime < -secPerDay/2.0) {
357 timeChanged = true;
358 tow += int(secPerDay);
359 if (tow > secPerWeek) {
360 tow -= int(secPerWeek);
361 ww += 1;
362 }
363 }
364
365 if (false && timeChanged && BNC_CORE->mode() == t_bncCore::batchPostProcessing) {
366 bncTime newHTime(ww, (double) tow);
367 cout << "GLONASS " << ee->almanac_number << " Time Changed at "
368 << currentTime.datestr() << " " << currentTime.timestr()
369 << endl
370 << "old: " << hTime.datestr() << " " << hTime.timestr()
371 << endl
372 << "new: " << newHTime.datestr() << " " << newHTime.timestr()
373 << endl
374 << "eph: " << ee->GPSWeek << " " << ee->GPSTOW << " " << ee->tb
375 << endl
376 << "ww, tow (old): " << ww_old << " " << tow_old
377 << endl
378 << "ww, tow (new): " << ww << " " << tow
379 << endl << endl;
380 }
381 }
382
383 bncTime hlpTime(ww, (double) tow);
384 unsigned year, month, day;
385 hlpTime.civil_date(year, month, day);
386 _gps_utc = gnumleap(year, month, day);
387
388 _TOC.set(ww, tow);
389 _E = ee->E;
390 _tau = ee->tau;
391 _gamma = ee->gamma;
392 _x_pos = ee->x_pos;
393 _x_velocity = ee->x_velocity;
394 _x_acceleration = ee->x_acceleration;
395 _y_pos = ee->y_pos;
396 _y_velocity = ee->y_velocity;
397 _y_acceleration = ee->y_acceleration;
398 _z_pos = ee->z_pos;
399 _z_velocity = ee->z_velocity;
400 _z_acceleration = ee->z_acceleration;
401 _health = 0;
402 _frequency_number = ee->frequency_number;
403 _tki = ee->tk-3*60*60; if (_tki < 0) _tki += 86400;
404
405 // Initialize status vector
406 // ------------------------
407 _tt = _TOC;
408
409 _xv(1) = _x_pos * 1.e3;
410 _xv(2) = _y_pos * 1.e3;
411 _xv(3) = _z_pos * 1.e3;
412 _xv(4) = _x_velocity * 1.e3;
413 _xv(5) = _y_velocity * 1.e3;
414 _xv(6) = _z_velocity * 1.e3;
415}
416
417// Set Galileo Satellite Position
418////////////////////////////////////////////////////////////////////////////
419void t_ephGal::set(const galileoephemeris* ee) {
420
421 _receptDateTime = currentDateAndTimeGPS();
422
423 _prn.set('E', ee->satellite);
424
425 _TOC.set(ee->Week, ee->TOC);
426 _clock_bias = ee->clock_bias;
427 _clock_drift = ee->clock_drift;
428 _clock_driftrate = ee->clock_driftrate;
429
430 _IODnav = ee->IODnav;
431 _Crs = ee->Crs;
432 _Delta_n = ee->Delta_n;
433 _M0 = ee->M0;
434
435 _Cuc = ee->Cuc;
436 _e = ee->e;
437 _Cus = ee->Cus;
438 _sqrt_A = ee->sqrt_A;
439
440 _TOEsec = _TOC.gpssec();
441 //// _TOEsec = ee->TOE; //// TODO:
442 _Cic = ee->Cic;
443 _OMEGA0 = ee->OMEGA0;
444 _Cis = ee->Cis;
445
446 _i0 = ee->i0;
447 _Crc = ee->Crc;
448 _omega = ee->omega;
449 _OMEGADOT = ee->OMEGADOT;
450
451 _IDOT = ee->IDOT;
452 _TOEweek = ee->Week;
453
454 _SISA = ee->SISA;
455 _E5aHS = ee->E5aHS;
456 _E5bHS = ee->E5bHS;
457 _BGD_1_5A = ee->BGD_1_5A;
458 _BGD_1_5B = ee->BGD_1_5B;
459
460 _TOT = 0.9999e9;
461
462 _flags = ee->flags;
463}
464
465// Compute Galileo Satellite Position (virtual)
466////////////////////////////////////////////////////////////////////////////
467t_irc t_ephGal::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
468
469 if (_checkState == bad) {
470 return failure;
471 }
472
473 static const double omegaEarth = 7292115.1467e-11;
474 static const double gmWGS = 398.60044e12;
475
476 memset(xc, 0, 4*sizeof(double));
477 memset(vv, 0, 3*sizeof(double));
478
479 double a0 = _sqrt_A * _sqrt_A;
480 if (a0 == 0) {
481 return failure;
482 }
483
484 double n0 = sqrt(gmWGS/(a0*a0*a0));
485
486 bncTime tt(GPSweek, GPSweeks);
487 double tk = tt - bncTime(_TOC.gpsw(), _TOEsec);
488
489 double n = n0 + _Delta_n;
490 double M = _M0 + n*tk;
491 double E = M;
492 double E_last;
493 do {
494 E_last = E;
495 E = M + _e*sin(E);
496 } while ( fabs(E-E_last)*a0 > 0.001 );
497 double v = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) );
498 double u0 = v + _omega;
499 double sin2u0 = sin(2*u0);
500 double cos2u0 = cos(2*u0);
501 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
502 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
503 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
504 double xp = r*cos(u);
505 double yp = r*sin(u);
506 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
507 omegaEarth*_TOEsec;
508
509 double sinom = sin(OM);
510 double cosom = cos(OM);
511 double sini = sin(i);
512 double cosi = cos(i);
513 xc[0] = xp*cosom - yp*cosi*sinom;
514 xc[1] = xp*sinom + yp*cosi*cosom;
515 xc[2] = yp*sini;
516
517 double tc = tt - _TOC;
518 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
519
520 // Velocity
521 // --------
522 double tanv2 = tan(v/2);
523 double dEdM = 1 / (1 - _e*cos(E));
524 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
525 * dEdM * n;
526 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
527 double dotom = _OMEGADOT - omegaEarth;
528 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
529 double dotr = a0 * _e*sin(E) * dEdM * n
530 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
531 double dotx = dotr*cos(u) - r*sin(u)*dotu;
532 double doty = dotr*sin(u) + r*cos(u)*dotu;
533
534 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
535 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
536 + yp*sini*sinom*doti; // dX / di
537
538 vv[1] = sinom *dotx + cosi*cosom *doty
539 + xp*cosom*dotom - yp*cosi*sinom*dotom
540 - yp*sini*cosom*doti;
541
542 vv[2] = sini *doty + yp*cosi *doti;
543
544 // Relativistic Correction
545 // -----------------------
546 // xc(4) -= 4.442807633e-10 * _e * sqrt(a0) *sin(E);
547 xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c;
548
549 return success;
550}
551
552// Constructor
553//////////////////////////////////////////////////////////////////////////////
554t_ephGPS::t_ephGPS(float rnxVersion, const QStringList& lines) {
555
556 const int nLines = 8;
557
558 if (lines.size() != nLines) {
559 _checkState = bad;
560 return;
561 }
562
563 // RINEX Format
564 // ------------
565 int fieldLen = 19;
566
567 int pos[4];
568 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
569 pos[1] = pos[0] + fieldLen;
570 pos[2] = pos[1] + fieldLen;
571 pos[3] = pos[2] + fieldLen;
572
573 // Read eight lines
574 // ----------------
575 for (int iLine = 0; iLine < nLines; iLine++) {
576 QString line = lines[iLine];
577
578 if ( iLine == 0 ) {
579 QTextStream in(line.left(pos[1]).toAscii());
580
581 int year, month, day, hour, min;
582 double sec;
583
584 QString prnStr;
585 in >> prnStr >> year >> month >> day >> hour >> min >> sec;
586 if (prnStr.at(0) == 'G') {
587 _prn.set('G', prnStr.mid(1).toInt());
588 }
589 else if (prnStr.at(0) == 'J') {
590 _prn.set('J', prnStr.mid(1).toInt());
591 }
592 else {
593 _prn.set('G', prnStr.toInt());
594 }
595
596 if (year < 80) {
597 year += 2000;
598 }
599 else if (year < 100) {
600 year += 1900;
601 }
602
603 _TOC.set(year, month, day, hour, min, sec);
604
605 if ( readDbl(line, pos[1], fieldLen, _clock_bias ) ||
606 readDbl(line, pos[2], fieldLen, _clock_drift ) ||
607 readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
608 _checkState = bad;
609 return;
610 }
611 }
612
613 else if ( iLine == 1 ) {
614 if ( readDbl(line, pos[0], fieldLen, _IODE ) ||
615 readDbl(line, pos[1], fieldLen, _Crs ) ||
616 readDbl(line, pos[2], fieldLen, _Delta_n) ||
617 readDbl(line, pos[3], fieldLen, _M0 ) ) {
618 _checkState = bad;
619 return;
620 }
621 }
622
623 else if ( iLine == 2 ) {
624 if ( readDbl(line, pos[0], fieldLen, _Cuc ) ||
625 readDbl(line, pos[1], fieldLen, _e ) ||
626 readDbl(line, pos[2], fieldLen, _Cus ) ||
627 readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
628 _checkState = bad;
629 return;
630 }
631 }
632
633 else if ( iLine == 3 ) {
634 if ( readDbl(line, pos[0], fieldLen, _TOEsec) ||
635 readDbl(line, pos[1], fieldLen, _Cic ) ||
636 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
637 readDbl(line, pos[3], fieldLen, _Cis ) ) {
638 _checkState = bad;
639 return;
640 }
641 }
642
643 else if ( iLine == 4 ) {
644 if ( readDbl(line, pos[0], fieldLen, _i0 ) ||
645 readDbl(line, pos[1], fieldLen, _Crc ) ||
646 readDbl(line, pos[2], fieldLen, _omega ) ||
647 readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
648 _checkState = bad;
649 return;
650 }
651 }
652
653 else if ( iLine == 5 ) {
654 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
655 readDbl(line, pos[1], fieldLen, _L2Codes) ||
656 readDbl(line, pos[2], fieldLen, _TOEweek ) ||
657 readDbl(line, pos[3], fieldLen, _L2PFlag) ) {
658 _checkState = bad;
659 return;
660 }
661 }
662
663 else if ( iLine == 6 ) {
664 if ( readDbl(line, pos[0], fieldLen, _ura ) ||
665 readDbl(line, pos[1], fieldLen, _health) ||
666 readDbl(line, pos[2], fieldLen, _TGD ) ||
667 readDbl(line, pos[3], fieldLen, _IODC ) ) {
668 _checkState = bad;
669 return;
670 }
671 }
672
673 else if ( iLine == 7 ) {
674 if ( readDbl(line, pos[0], fieldLen, _TOT) ) {
675 _checkState = bad;
676 return;
677 }
678 readDbl(line, pos[1], fieldLen, _fitInterval); // _fitInterval optional
679 }
680 }
681}
682
683// Constructor
684//////////////////////////////////////////////////////////////////////////////
685t_ephGlo::t_ephGlo(float rnxVersion, const QStringList& lines) {
686
687 const int nLines = 4;
688
689 if (lines.size() != nLines) {
690 _checkState = bad;
691 return;
692 }
693
694 // RINEX Format
695 // ------------
696 int fieldLen = 19;
697
698 int pos[4];
699 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
700 pos[1] = pos[0] + fieldLen;
701 pos[2] = pos[1] + fieldLen;
702 pos[3] = pos[2] + fieldLen;
703
704 // Read four lines
705 // ---------------
706 for (int iLine = 0; iLine < nLines; iLine++) {
707 QString line = lines[iLine];
708
709 if ( iLine == 0 ) {
710 QTextStream in(line.left(pos[1]).toAscii());
711
712 int year, month, day, hour, min;
713 double sec;
714
715 QString prnStr;
716 in >> prnStr >> year >> month >> day >> hour >> min >> sec;
717 if (prnStr.at(0) == 'R') {
718 _prn.set('R', prnStr.mid(1).toInt());
719 }
720 else {
721 _prn.set('R', prnStr.toInt());
722 }
723
724 if (year < 80) {
725 year += 2000;
726 }
727 else if (year < 100) {
728 year += 1900;
729 }
730
731 _gps_utc = gnumleap(year, month, day);
732
733 _TOC.set(year, month, day, hour, min, sec);
734 _TOC = _TOC + _gps_utc;
735
736 if ( readDbl(line, pos[1], fieldLen, _tau ) ||
737 readDbl(line, pos[2], fieldLen, _gamma) ||
738 readDbl(line, pos[3], fieldLen, _tki ) ) {
739 _checkState = bad;
740 return;
741 }
742
743 _tau = -_tau;
744 }
745
746 else if ( iLine == 1 ) {
747 if ( readDbl(line, pos[0], fieldLen, _x_pos ) ||
748 readDbl(line, pos[1], fieldLen, _x_velocity ) ||
749 readDbl(line, pos[2], fieldLen, _x_acceleration) ||
750 readDbl(line, pos[3], fieldLen, _health ) ) {
751 _checkState = bad;
752 return;
753 }
754 }
755
756 else if ( iLine == 2 ) {
757 if ( readDbl(line, pos[0], fieldLen, _y_pos ) ||
758 readDbl(line, pos[1], fieldLen, _y_velocity ) ||
759 readDbl(line, pos[2], fieldLen, _y_acceleration ) ||
760 readDbl(line, pos[3], fieldLen, _frequency_number) ) {
761 _checkState = bad;
762 return;
763 }
764 }
765
766 else if ( iLine == 3 ) {
767 if ( readDbl(line, pos[0], fieldLen, _z_pos ) ||
768 readDbl(line, pos[1], fieldLen, _z_velocity ) ||
769 readDbl(line, pos[2], fieldLen, _z_acceleration) ||
770 readDbl(line, pos[3], fieldLen, _E ) ) {
771 _checkState = bad;
772 return;
773 }
774 }
775 }
776
777 // Initialize status vector
778 // ------------------------
779 _tt = _TOC;
780 _xv.ReSize(6);
781 _xv(1) = _x_pos * 1.e3;
782 _xv(2) = _y_pos * 1.e3;
783 _xv(3) = _z_pos * 1.e3;
784 _xv(4) = _x_velocity * 1.e3;
785 _xv(5) = _y_velocity * 1.e3;
786 _xv(6) = _z_velocity * 1.e3;
787}
788
789// Constructor
790//////////////////////////////////////////////////////////////////////////////
791t_ephGal::t_ephGal(float rnxVersion, const QStringList& lines) {
792
793 const int nLines = 8;
794
795 if (lines.size() != nLines) {
796 _checkState = bad;
797 return;
798 }
799
800 // RINEX Format
801 // ------------
802 int fieldLen = 19;
803
804 int pos[4];
805 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
806 pos[1] = pos[0] + fieldLen;
807 pos[2] = pos[1] + fieldLen;
808 pos[3] = pos[2] + fieldLen;
809
810 // Read eight lines
811 // ----------------
812 for (int iLine = 0; iLine < nLines; iLine++) {
813 QString line = lines[iLine];
814
815 if ( iLine == 0 ) {
816 QTextStream in(line.left(pos[1]).toAscii());
817
818 int year, month, day, hour, min;
819 double sec;
820
821 QString prnStr;
822 in >> prnStr >> year >> month >> day >> hour >> min >> sec;
823 if (prnStr.at(0) == 'E') {
824 _prn.set('E', prnStr.mid(1).toInt());
825 }
826 else {
827 _prn.set('E', prnStr.toInt());
828 }
829
830 if (year < 80) {
831 year += 2000;
832 }
833 else if (year < 100) {
834 year += 1900;
835 }
836
837 _TOC.set(year, month, day, hour, min, sec);
838
839 if ( readDbl(line, pos[1], fieldLen, _clock_bias ) ||
840 readDbl(line, pos[2], fieldLen, _clock_drift ) ||
841 readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
842 _checkState = bad;
843 return;
844 }
845 }
846
847 else if ( iLine == 1 ) {
848 if ( readDbl(line, pos[0], fieldLen, _IODnav ) ||
849 readDbl(line, pos[1], fieldLen, _Crs ) ||
850 readDbl(line, pos[2], fieldLen, _Delta_n) ||
851 readDbl(line, pos[3], fieldLen, _M0 ) ) {
852 _checkState = bad;
853 return;
854 }
855 }
856
857 else if ( iLine == 2 ) {
858 if ( readDbl(line, pos[0], fieldLen, _Cuc ) ||
859 readDbl(line, pos[1], fieldLen, _e ) ||
860 readDbl(line, pos[2], fieldLen, _Cus ) ||
861 readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
862 _checkState = bad;
863 return;
864 }
865 }
866
867 else if ( iLine == 3 ) {
868 if ( readDbl(line, pos[0], fieldLen, _TOEsec) ||
869 readDbl(line, pos[1], fieldLen, _Cic ) ||
870 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
871 readDbl(line, pos[3], fieldLen, _Cis ) ) {
872 _checkState = bad;
873 return;
874 }
875 }
876
877 else if ( iLine == 4 ) {
878 if ( readDbl(line, pos[0], fieldLen, _i0 ) ||
879 readDbl(line, pos[1], fieldLen, _Crc ) ||
880 readDbl(line, pos[2], fieldLen, _omega ) ||
881 readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
882 _checkState = bad;
883 return;
884 }
885 }
886
887 else if ( iLine == 5 ) {
888 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
889 readDbl(line, pos[2], fieldLen, _TOEweek) ) {
890 _checkState = bad;
891 return;
892 }
893 }
894
895 else if ( iLine == 6 ) {
896 if ( readDbl(line, pos[0], fieldLen, _SISA ) ||
897 readDbl(line, pos[1], fieldLen, _E5aHS ) ||
898 readDbl(line, pos[2], fieldLen, _BGD_1_5A) ||
899 readDbl(line, pos[3], fieldLen, _BGD_1_5B) ) {
900 _checkState = bad;
901 return;
902 }
903 }
904
905 else if ( iLine == 7 ) {
906 if ( readDbl(line, pos[0], fieldLen, _TOT) ) {
907 _checkState = bad;
908 return;
909 }
910 }
911 }
912}
913
914//
915//////////////////////////////////////////////////////////////////////////////
916QString t_eph::rinexDateStr(const bncTime& tt, const t_prn& prn, double version) {
917 QString prnStr(prn.toString().c_str());
918 return rinexDateStr(tt, prnStr, version);
919}
920
921//
922//////////////////////////////////////////////////////////////////////////////
923QString t_eph::rinexDateStr(const bncTime& tt, const QString& prnStr, double version) {
924
925 QString datStr;
926
927 unsigned year, month, day, hour, min;
928 double sec;
929 tt.civil_date(year, month, day);
930 tt.civil_time(hour, min, sec);
931
932 QTextStream out(&datStr);
933
934 if (version < 3.0) {
935 QString prnHlp = prnStr.mid(1,2); if (prnHlp[0] == '0') prnHlp[0] = ' ';
936 out << prnHlp << QString(" %1 %2 %3 %4 %5%6")
937 .arg(year % 100, 2, 10, QChar('0'))
938 .arg(month, 2)
939 .arg(day, 2)
940 .arg(hour, 2)
941 .arg(min, 2)
942 .arg(sec, 5, 'f',1);
943 }
944 else {
945 out << prnStr << QString(" %1 %2 %3 %4 %5 %6")
946 .arg(year, 4)
947 .arg(month, 2, 10, QChar('0'))
948 .arg(day, 2, 10, QChar('0'))
949 .arg(hour, 2, 10, QChar('0'))
950 .arg(min, 2, 10, QChar('0'))
951 .arg(int(sec), 2, 10, QChar('0'));
952 }
953
954 return datStr;
955}
956
957// RINEX Format String
958//////////////////////////////////////////////////////////////////////////////
959QString t_ephGPS::toString(double version) const {
960
961 QString rnxStr = rinexDateStr(_TOC, _prn, version);
962
963 QTextStream out(&rnxStr);
964
965 out << QString("%1%2%3\n")
966 .arg(_clock_bias, 19, 'e', 12)
967 .arg(_clock_drift, 19, 'e', 12)
968 .arg(_clock_driftrate, 19, 'e', 12);
969
970 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
971
972 out << QString(fmt)
973 .arg(_IODE, 19, 'e', 12)
974 .arg(_Crs, 19, 'e', 12)
975 .arg(_Delta_n, 19, 'e', 12)
976 .arg(_M0, 19, 'e', 12);
977
978 out << QString(fmt)
979 .arg(_Cuc, 19, 'e', 12)
980 .arg(_e, 19, 'e', 12)
981 .arg(_Cus, 19, 'e', 12)
982 .arg(_sqrt_A, 19, 'e', 12);
983
984 out << QString(fmt)
985 .arg(_TOEsec, 19, 'e', 12)
986 .arg(_Cic, 19, 'e', 12)
987 .arg(_OMEGA0, 19, 'e', 12)
988 .arg(_Cis, 19, 'e', 12);
989
990 out << QString(fmt)
991 .arg(_i0, 19, 'e', 12)
992 .arg(_Crc, 19, 'e', 12)
993 .arg(_omega, 19, 'e', 12)
994 .arg(_OMEGADOT, 19, 'e', 12);
995
996 out << QString(fmt)
997 .arg(_IDOT, 19, 'e', 12)
998 .arg(_L2Codes, 19, 'e', 12)
999 .arg(_TOEweek, 19, 'e', 12)
1000 .arg(_L2PFlag, 19, 'e', 12);
1001
1002 out << QString(fmt)
1003 .arg(_ura, 19, 'e', 12)
1004 .arg(_health, 19, 'e', 12)
1005 .arg(_TGD, 19, 'e', 12)
1006 .arg(_IODC, 19, 'e', 12);
1007
1008 out << QString(fmt)
1009 .arg(_TOT, 19, 'e', 12)
1010 .arg(_fitInterval, 19, 'e', 12)
1011 .arg("", 19, QChar(' '))
1012 .arg("", 19, QChar(' '));
1013
1014 return rnxStr;
1015}
1016
1017// RINEX Format String
1018//////////////////////////////////////////////////////////////////////////////
1019QString t_ephGlo::toString(double version) const {
1020
1021 QString rnxStr = rinexDateStr(_TOC-_gps_utc, _prn, version);
1022
1023 QTextStream out(&rnxStr);
1024
1025 out << QString("%1%2%3\n")
1026 .arg(-_tau, 19, 'e', 12)
1027 .arg(_gamma, 19, 'e', 12)
1028 .arg(_tki, 19, 'e', 12);
1029
1030 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1031
1032 out << QString(fmt)
1033 .arg(_x_pos, 19, 'e', 12)
1034 .arg(_x_velocity, 19, 'e', 12)
1035 .arg(_x_acceleration, 19, 'e', 12)
1036 .arg(_health, 19, 'e', 12);
1037
1038 out << QString(fmt)
1039 .arg(_y_pos, 19, 'e', 12)
1040 .arg(_y_velocity, 19, 'e', 12)
1041 .arg(_y_acceleration, 19, 'e', 12)
1042 .arg(_frequency_number, 19, 'e', 12);
1043
1044 out << QString(fmt)
1045 .arg(_z_pos, 19, 'e', 12)
1046 .arg(_z_velocity, 19, 'e', 12)
1047 .arg(_z_acceleration, 19, 'e', 12)
1048 .arg(_E, 19, 'e', 12);
1049
1050 return rnxStr;
1051}
1052
1053// RINEX Format String
1054//////////////////////////////////////////////////////////////////////////////
1055QString t_ephGal::toString(double version) const {
1056
1057 QString rnxStr = rinexDateStr(_TOC, _prn, version);
1058
1059 QTextStream out(&rnxStr);
1060
1061 out << QString("%1%2%3\n")
1062 .arg(_clock_bias, 19, 'e', 12)
1063 .arg(_clock_drift, 19, 'e', 12)
1064 .arg(_clock_driftrate, 19, 'e', 12);
1065
1066 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1067
1068 out << QString(fmt)
1069 .arg(_IODnav, 19, 'e', 12)
1070 .arg(_Crs, 19, 'e', 12)
1071 .arg(_Delta_n, 19, 'e', 12)
1072 .arg(_M0, 19, 'e', 12);
1073
1074 out << QString(fmt)
1075 .arg(_Cuc, 19, 'e', 12)
1076 .arg(_e, 19, 'e', 12)
1077 .arg(_Cus, 19, 'e', 12)
1078 .arg(_sqrt_A, 19, 'e', 12);
1079
1080 out << QString(fmt)
1081 .arg(_TOEsec, 19, 'e', 12)
1082 .arg(_Cic, 19, 'e', 12)
1083 .arg(_OMEGA0, 19, 'e', 12)
1084 .arg(_Cis, 19, 'e', 12);
1085
1086 out << QString(fmt)
1087 .arg(_i0, 19, 'e', 12)
1088 .arg(_Crc, 19, 'e', 12)
1089 .arg(_omega, 19, 'e', 12)
1090 .arg(_OMEGADOT, 19, 'e', 12);
1091
1092 int dataSource = 0;
1093 double HS = 0.0;
1094 if ( (_flags & GALEPHF_INAV) == GALEPHF_INAV ) {
1095 dataSource |= (1<<0);
1096 dataSource |= (1<<9);
1097 HS = _E5bHS;
1098 }
1099 else if ( (_flags & GALEPHF_FNAV) == GALEPHF_FNAV ) {
1100 dataSource |= (1<<1);
1101 dataSource |= (1<<8);
1102 HS = _E5aHS;
1103 }
1104 out << QString(fmt)
1105 .arg(_IDOT, 19, 'e', 12)
1106 .arg(double(dataSource), 19, 'e', 12)
1107 .arg(_TOEweek, 19, 'e', 12)
1108 .arg(0.0, 19, 'e', 12);
1109
1110 out << QString(fmt)
1111 .arg(_SISA, 19, 'e', 12)
1112 .arg(HS, 19, 'e', 12)
1113 .arg(_BGD_1_5A, 19, 'e', 12)
1114 .arg(_BGD_1_5B, 19, 'e', 12);
1115
1116 out << QString(fmt)
1117 .arg(_TOT, 19, 'e', 12)
1118 .arg("", 19, QChar(' '))
1119 .arg("", 19, QChar(' '))
1120 .arg("", 19, QChar(' '));
1121
1122 return rnxStr;
1123}
1124
1125// Constructor
1126//////////////////////////////////////////////////////////////////////////////
1127t_ephSBAS::t_ephSBAS(float rnxVersion, const QStringList& lines) {
1128
1129 const int nLines = 4;
1130
1131 if (lines.size() != nLines) {
1132 _checkState = bad;
1133 return;
1134 }
1135
1136 // RINEX Format
1137 // ------------
1138 int fieldLen = 19;
1139
1140 int pos[4];
1141 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
1142 pos[1] = pos[0] + fieldLen;
1143 pos[2] = pos[1] + fieldLen;
1144 pos[3] = pos[2] + fieldLen;
1145
1146 // Read four lines
1147 // ---------------
1148 for (int iLine = 0; iLine < nLines; iLine++) {
1149 QString line = lines[iLine];
1150
1151 if ( iLine == 0 ) {
1152 QTextStream in(line.left(pos[1]).toAscii());
1153
1154 int year, month, day, hour, min;
1155 double sec;
1156
1157 QString prnStr;
1158 in >> prnStr >> year >> month >> day >> hour >> min >> sec;
1159 if (prnStr.at(0) == 'S') {
1160 _prn.set('S', prnStr.mid(1).toInt());
1161 }
1162 else {
1163 _prn.set('S', prnStr.toInt());
1164 }
1165
1166 if (year < 80) {
1167 year += 2000;
1168 }
1169 else if (year < 100) {
1170 year += 1900;
1171 }
1172
1173 _TOC.set(year, month, day, hour, min, sec);
1174
1175 if ( readDbl(line, pos[1], fieldLen, _agf0 ) ||
1176 readDbl(line, pos[2], fieldLen, _agf1 ) ||
1177 readDbl(line, pos[3], fieldLen, _TOW ) ) {
1178 _checkState = bad;
1179 return;
1180 }
1181 }
1182
1183 else if ( iLine == 1 ) {
1184 if ( readDbl(line, pos[0], fieldLen, _x_pos ) ||
1185 readDbl(line, pos[1], fieldLen, _x_velocity ) ||
1186 readDbl(line, pos[2], fieldLen, _x_acceleration) ||
1187 readDbl(line, pos[3], fieldLen, _health ) ) {
1188 _checkState = bad;
1189 return;
1190 }
1191 }
1192
1193 else if ( iLine == 2 ) {
1194 if ( readDbl(line, pos[0], fieldLen, _y_pos ) ||
1195 readDbl(line, pos[1], fieldLen, _y_velocity ) ||
1196 readDbl(line, pos[2], fieldLen, _y_acceleration ) ||
1197 readDbl(line, pos[3], fieldLen, _ura ) ) {
1198 _checkState = bad;
1199 return;
1200 }
1201 }
1202
1203 else if ( iLine == 3 ) {
1204 double iodn;
1205 if ( readDbl(line, pos[0], fieldLen, _z_pos ) ||
1206 readDbl(line, pos[1], fieldLen, _z_velocity ) ||
1207 readDbl(line, pos[2], fieldLen, _z_acceleration) ||
1208 readDbl(line, pos[3], fieldLen, iodn ) ) {
1209 _checkState = bad;
1210 return;
1211 _IODN = int(iodn);
1212 }
1213 }
1214 }
1215
1216 _x_pos *= 1.e3;
1217 _y_pos *= 1.e3;
1218 _z_pos *= 1.e3;
1219 _x_velocity *= 1.e3;
1220 _y_velocity *= 1.e3;
1221 _z_velocity *= 1.e3;
1222 _x_acceleration *= 1.e3;
1223 _y_acceleration *= 1.e3;
1224 _z_acceleration *= 1.e3;
1225}
1226
1227// Set SBAS Satellite Position
1228////////////////////////////////////////////////////////////////////////////
1229void t_ephSBAS::set(const sbasephemeris* ee) {
1230
1231 _prn.set('S', ee->satellite - PRN_SBAS_START + 20);
1232 _TOC.set(ee->GPSweek_TOE, double(ee->TOE));
1233
1234 _IODN = ee->IODN;
1235 _TOW = ee->TOW;
1236
1237 _agf0 = ee->agf0;
1238 _agf1 = ee->agf1;
1239
1240 _x_pos = ee->x_pos;
1241 _x_velocity = ee->x_velocity;
1242 _x_acceleration = ee->x_acceleration;
1243
1244 _y_pos = ee->y_pos;
1245 _y_velocity = ee->y_velocity;
1246 _y_acceleration = ee->y_acceleration;
1247
1248 _z_pos = ee->z_pos;
1249 _z_velocity = ee->z_velocity;
1250 _z_acceleration = ee->z_acceleration;
1251
1252 _ura = ee->URA;
1253
1254 _health = 0;
1255}
1256
1257// Compute SBAS Satellite Position (virtual)
1258////////////////////////////////////////////////////////////////////////////
1259t_irc t_ephSBAS::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
1260
1261 if (_checkState == bad) {
1262 return failure;
1263 }
1264
1265 bncTime tt(GPSweek, GPSweeks);
1266 double dt = tt - _TOC;
1267
1268 xc[0] = _x_pos + _x_velocity * dt + _x_acceleration * dt * dt / 2.0;
1269 xc[1] = _y_pos + _y_velocity * dt + _y_acceleration * dt * dt / 2.0;
1270 xc[2] = _z_pos + _z_velocity * dt + _z_acceleration * dt * dt / 2.0;
1271
1272 vv[0] = _x_velocity + _x_acceleration * dt;
1273 vv[1] = _y_velocity + _y_acceleration * dt;
1274 vv[2] = _z_velocity + _z_acceleration * dt;
1275
1276 xc[3] = _agf0 + _agf1 * dt;
1277
1278 return success;
1279}
1280
1281// RINEX Format String
1282//////////////////////////////////////////////////////////////////////////////
1283QString t_ephSBAS::toString(double version) const {
1284
1285 QString rnxStr = rinexDateStr(_TOC, _prn, version);
1286
1287 QTextStream out(&rnxStr);
1288
1289 out << QString("%1%2%3\n")
1290 .arg(_agf0, 19, 'e', 12)
1291 .arg(_agf1, 19, 'e', 12)
1292 .arg(_TOW, 19, 'e', 12);
1293
1294 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1295
1296 out << QString(fmt)
1297 .arg(1.e-3*_x_pos, 19, 'e', 12)
1298 .arg(1.e-3*_x_velocity, 19, 'e', 12)
1299 .arg(1.e-3*_x_acceleration, 19, 'e', 12)
1300 .arg(_health, 19, 'e', 12);
1301
1302 out << QString(fmt)
1303 .arg(1.e-3*_y_pos, 19, 'e', 12)
1304 .arg(1.e-3*_y_velocity, 19, 'e', 12)
1305 .arg(1.e-3*_y_acceleration, 19, 'e', 12)
1306 .arg(_ura, 19, 'e', 12);
1307
1308 out << QString(fmt)
1309 .arg(1.e-3*_z_pos, 19, 'e', 12)
1310 .arg(1.e-3*_z_velocity, 19, 'e', 12)
1311 .arg(1.e-3*_z_acceleration, 19, 'e', 12)
1312 .arg(double(_IODN), 19, 'e', 12);
1313
1314 return rnxStr;
1315}
1316
1317// Constructor
1318//////////////////////////////////////////////////////////////////////////////
1319t_ephCompass::t_ephCompass(float rnxVersion, const QStringList& lines) {
1320
1321 const int nLines = 8;
1322
1323 if (lines.size() != nLines) {
1324 _checkState = bad;
1325 return;
1326 }
1327
1328 // RINEX Format
1329 // ------------
1330 int fieldLen = 19;
1331
1332 int pos[4];
1333 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
1334 pos[1] = pos[0] + fieldLen;
1335 pos[2] = pos[1] + fieldLen;
1336 pos[3] = pos[2] + fieldLen;
1337
1338 double TOTs;
1339 double TOEs;
1340 double TOEw;
1341
1342 // Read eight lines
1343 // ----------------
1344 for (int iLine = 0; iLine < nLines; iLine++) {
1345 QString line = lines[iLine];
1346
1347 if ( iLine == 0 ) {
1348 QTextStream in(line.left(pos[1]).toAscii());
1349
1350 int year, month, day, hour, min;
1351 double sec;
1352
1353 QString prnStr;
1354 in >> prnStr >> year >> month >> day >> hour >> min >> sec;
1355 if (prnStr.at(0) == 'C') {
1356 _prn.set('C', prnStr.mid(1).toInt());
1357 }
1358 else {
1359 _prn.set('C', prnStr.toInt());
1360 }
1361
1362 if (year < 80) {
1363 year += 2000;
1364 }
1365 else if (year < 100) {
1366 year += 1900;
1367 }
1368
1369 _TOC_bdt.set(year, month, day, hour, min, sec);
1370
1371 if ( readDbl(line, pos[1], fieldLen, _clock_bias ) ||
1372 readDbl(line, pos[2], fieldLen, _clock_drift ) ||
1373 readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
1374 _checkState = bad;
1375 return;
1376 }
1377 }
1378
1379 else if ( iLine == 1 ) {
1380 double aode;
1381 if ( readDbl(line, pos[0], fieldLen, aode ) ||
1382 readDbl(line, pos[1], fieldLen, _Crs ) ||
1383 readDbl(line, pos[2], fieldLen, _Delta_n) ||
1384 readDbl(line, pos[3], fieldLen, _M0 ) ) {
1385 _checkState = bad;
1386 return;
1387 }
1388 _AODE = int(aode);
1389 }
1390
1391 else if ( iLine == 2 ) {
1392 if ( readDbl(line, pos[0], fieldLen, _Cuc ) ||
1393 readDbl(line, pos[1], fieldLen, _e ) ||
1394 readDbl(line, pos[2], fieldLen, _Cus ) ||
1395 readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
1396 _checkState = bad;
1397 return;
1398 }
1399 }
1400
1401 else if ( iLine == 3 ) {
1402 if ( readDbl(line, pos[0], fieldLen, TOEs ) ||
1403 readDbl(line, pos[1], fieldLen, _Cic ) ||
1404 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
1405 readDbl(line, pos[3], fieldLen, _Cis ) ) {
1406 _checkState = bad;
1407 return;
1408 }
1409 }
1410
1411 else if ( iLine == 4 ) {
1412 if ( readDbl(line, pos[0], fieldLen, _i0 ) ||
1413 readDbl(line, pos[1], fieldLen, _Crc ) ||
1414 readDbl(line, pos[2], fieldLen, _omega ) ||
1415 readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
1416 _checkState = bad;
1417 return;
1418 }
1419 }
1420
1421 else if ( iLine == 5 ) {
1422 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
1423 readDbl(line, pos[2], fieldLen, TOEw) ) {
1424 _checkState = bad;
1425 return;
1426 }
1427 }
1428
1429 else if ( iLine == 6 ) {
1430 double SatH1;
1431 if ( readDbl(line, pos[1], fieldLen, SatH1) ||
1432 readDbl(line, pos[2], fieldLen, _TGD1) ||
1433 readDbl(line, pos[3], fieldLen, _TGD2) ) {
1434 _checkState = bad;
1435 return;
1436 }
1437 _SatH1 = int(SatH1);
1438 }
1439
1440 else if ( iLine == 7 ) {
1441 double aodc;
1442 if ( readDbl(line, pos[0], fieldLen, TOTs) ||
1443 readDbl(line, pos[1], fieldLen, aodc) ) {
1444 _checkState = bad;
1445 return;
1446 }
1447 if (TOTs == 0.9999e9) { // 0.9999e9 means not known (RINEX standard)
1448 TOTs = TOEs;
1449 }
1450 _AODC = int(aodc);
1451 }
1452 }
1453
1454 TOEw += 1356; // BDT -> GPS week number
1455 _TOE_bdt.set(int(TOEw), TOEs);
1456
1457 // GPS->BDT
1458 // --------
1459 _TOC = _TOC_bdt + 14.0;
1460 _TOE = _TOE_bdt + 14.0;
1461
1462 // remark: actually should be computed from second_tot
1463 // but it seems to be unreliable in RINEX files
1464 _TOT = _TOC;
1465}
1466
1467// Constructor
1468//////////////////////////////////////////////////////////////////////////////
1469t_irc t_ephCompass::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
1470
1471 if (_checkState == bad) {
1472 return failure;
1473 }
1474
1475 static const double gmCompass = 398.6004418e12;
1476 static const double omegaCompass = 7292115.0000e-11;
1477
1478 xc[0] = xc[1] = xc[2] = xc[3] = 0.0;
1479 vv[0] = vv[1] = vv[2] = 0.0;
1480
1481 bncTime tt(GPSweek, GPSweeks);
1482
1483 if (_sqrt_A == 0) {
1484 return failure;
1485 }
1486 double a0 = _sqrt_A * _sqrt_A;
1487
1488 double n0 = sqrt(gmCompass/(a0*a0*a0));
1489 double tk = tt - _TOE;
1490 double n = n0 + _Delta_n;
1491 double M = _M0 + n*tk;
1492 double E = M;
1493 double E_last;
1494 int nLoop = 0;
1495 do {
1496 E_last = E;
1497 E = M + _e*sin(E);
1498
1499 if (++nLoop == 100) {
1500 return failure;
1501 }
1502 } while ( fabs(E-E_last)*a0 > 0.001 );
1503
1504 double v = atan2(sqrt(1-_e*_e) * sin(E), cos(E) - _e);
1505 double u0 = v + _omega;
1506 double sin2u0 = sin(2*u0);
1507 double cos2u0 = cos(2*u0);
1508 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
1509 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
1510 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
1511 double xp = r*cos(u);
1512 double yp = r*sin(u);
1513 double toesec = (_TOE.gpssec() - 14.0);
1514
1515 double sinom = 0;
1516 double cosom = 0;
1517 double sini = 0;
1518 double cosi = 0;
1519
1520 const double iMaxGEO = 10.0 / 180.0 * M_PI;
1521
1522 // MEO/IGSO satellite
1523 // ------------------
1524 if (_i0 > iMaxGEO) {
1525 double OM = _OMEGA0 + (_OMEGADOT - omegaCompass)*tk - omegaCompass*toesec;
1526
1527 sinom = sin(OM);
1528 cosom = cos(OM);
1529 sini = sin(i);
1530 cosi = cos(i);
1531
1532 xc[0] = xp*cosom - yp*cosi*sinom;
1533 xc[1] = xp*sinom + yp*cosi*cosom;
1534 xc[2] = yp*sini;
1535 }
1536
1537 // GEO satellite
1538 // -------------
1539 else {
1540 double OM = _OMEGA0 + _OMEGADOT*tk - omegaCompass*toesec;
1541 double ll = omegaCompass*tk;
1542
1543 sinom = sin(OM);
1544 cosom = cos(OM);
1545 sini = sin(i);
1546 cosi = cos(i);
1547
1548 double xx = xp*cosom - yp*cosi*sinom;
1549 double yy = xp*sinom + yp*cosi*cosom;
1550 double zz = yp*sini;
1551
1552 Matrix R1 = BNC_PPP::t_astro::rotX(-5.0 / 180.0 * M_PI);
1553 Matrix R2 = BNC_PPP::t_astro::rotZ(ll);
1554
1555 ColumnVector X1(3); X1 << xx << yy << zz;
1556 ColumnVector X2 = R2*R1*X1;
1557
1558 xc[0] = X2(1);
1559 xc[1] = X2(2);
1560 xc[2] = X2(3);
1561 }
1562
1563 double tc = tt - _TOC;
1564 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc
1565 - 4.442807633e-10 * _e * sqrt(a0) *sin(E);
1566
1567 // Velocity
1568 // --------
1569 double tanv2 = tan(v/2);
1570 double dEdM = 1 / (1 - _e*cos(E));
1571 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2)
1572 / (1 + tanv2*tanv2) * dEdM * n;
1573 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
1574 double dotom = _OMEGADOT - t_CST::omega;
1575 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
1576 double dotr = a0 * _e*sin(E) * dEdM * n
1577 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
1578 double dotx = dotr*cos(u) - r*sin(u)*dotu;
1579 double doty = dotr*sin(u) + r*cos(u)*dotu;
1580
1581 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
1582 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
1583 + yp*sini*sinom*doti; // dX / di
1584
1585 vv[1] = sinom *dotx + cosi*cosom *doty
1586 + xp*cosom*dotom - yp*cosi*sinom*dotom
1587 - yp*sini*cosom*doti;
1588
1589 vv[2] = sini *doty + yp*cosi *doti;
1590
1591 // dotC = _clock_drift + _clock_driftrate*tc
1592 // - 4.442807633e-10*_e*sqrt(a0)*cos(E) * dEdM * n;
1593
1594 return success;
1595}
1596
1597// RINEX Format String
1598//////////////////////////////////////////////////////////////////////////////
1599QString t_ephCompass::toString(double version) const {
1600
1601 QString rnxStr = rinexDateStr(_TOC_bdt, _prn, version);
1602
1603 QTextStream out(&rnxStr);
1604
1605 out << QString("%1%2%3\n")
1606 .arg(_clock_bias, 19, 'e', 12)
1607 .arg(_clock_drift, 19, 'e', 12)
1608 .arg(_clock_driftrate, 19, 'e', 12);
1609
1610 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1611
1612 out << QString(fmt)
1613 .arg(double(_AODE), 19, 'e', 12)
1614 .arg(_Crs, 19, 'e', 12)
1615 .arg(_Delta_n, 19, 'e', 12)
1616 .arg(_M0, 19, 'e', 12);
1617
1618 out << QString(fmt)
1619 .arg(_Cuc, 19, 'e', 12)
1620 .arg(_e, 19, 'e', 12)
1621 .arg(_Cus, 19, 'e', 12)
1622 .arg(_sqrt_A, 19, 'e', 12);
1623
1624 out << QString(fmt)
1625 .arg(_TOE_bdt.gpssec(), 19, 'e', 12)
1626 .arg(_Cic, 19, 'e', 12)
1627 .arg(_OMEGA0, 19, 'e', 12)
1628 .arg(_Cis, 19, 'e', 12);
1629
1630 out << QString(fmt)
1631 .arg(_i0, 19, 'e', 12)
1632 .arg(_Crc, 19, 'e', 12)
1633 .arg(_omega, 19, 'e', 12)
1634 .arg(_OMEGADOT, 19, 'e', 12);
1635
1636 out << QString(fmt)
1637 .arg(_IDOT, 19, 'e', 12)
1638 .arg(0.0, 19, 'e', 12)
1639 .arg(double(_TOE_bdt.gpsw()), 19, 'e', 12)
1640 .arg(0.0, 19, 'e', 12);
1641
1642 out << QString(fmt)
1643 .arg(0.0, 19, 'e', 12)
1644 .arg(double(_SatH1), 19, 'e', 12)
1645 .arg(_TGD1, 19, 'e', 12)
1646 .arg(_TGD2, 19, 'e', 12);
1647
1648 out << QString(fmt)
1649 .arg(_TOE_bdt.gpssec(), 19, 'e', 12)
1650 .arg(double(_AODC), 19, 'e', 12);
1651
1652 return rnxStr;
1653}
1654
Note: See TracBrowser for help on using the repository browser.