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

Last change on this file since 6621 was 6621, checked in by stoecker, 7 years ago

small security cleanup

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