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

Last change on this file since 6793 was 6793, checked in by stuerze, 9 years ago

consider different SISA definitions for Galileo NAV; RTCM: index, RINEX: meters
File size: 49.1 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 _SISA = -1.0; // set RINEX entry to invalid
429
430 _receptDateTime = currentDateAndTimeGPS();
431
432 _prn.set('E', ee->satellite);
433
434 _TOC.set(ee->Week, ee->TOC);
435 _clock_bias = ee->clock_bias;
436 _clock_drift = ee->clock_drift;
437 _clock_driftrate = ee->clock_driftrate;
438
439 _IODnav = ee->IODnav;
440 _Crs = ee->Crs;
441 _Delta_n = ee->Delta_n;
442 _M0 = ee->M0;
443
444 _Cuc = ee->Cuc;
445 _e = ee->e;
446 _Cus = ee->Cus;
447 _sqrt_A = ee->sqrt_A;
448
449 _TOEsec = _TOC.gpssec();
450
451 _Cic = ee->Cic;
452 _OMEGA0 = ee->OMEGA0;
453 _Cis = ee->Cis;
454
455 _i0 = ee->i0;
456 _Crc = ee->Crc;
457 _omega = ee->omega;
458 _OMEGADOT = ee->OMEGADOT;
459
460 _IDOT = ee->IDOT;
461 _TOEweek = ee->Week;
462
463 _SISAI = ee->SISA; // index from RTCM
464 _E5aHS = ee->E5aHS;
465 _E5bHS = ee->E5bHS;
466 _E1_bHS = ee->E1_HS;
467 _BGD_1_5A = ee->BGD_1_5A;
468 _BGD_1_5B = ee->BGD_1_5B;
469
470 _TOT = 0.9999e9;
471
472 _flags = ee->flags;
473}
474
475// Compute Galileo Satellite Position (virtual)
476////////////////////////////////////////////////////////////////////////////
477t_irc t_ephGal::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
478
479 if (_checkState == bad) {
480 return failure;
481 }
482
483 static const double omegaEarth = 7292115.1467e-11;
484 static const double gmWGS = 398.60044e12;
485
486 memset(xc, 0, 4*sizeof(double));
487 memset(vv, 0, 3*sizeof(double));
488
489 double a0 = _sqrt_A * _sqrt_A;
490 if (a0 == 0) {
491 return failure;
492 }
493
494 double n0 = sqrt(gmWGS/(a0*a0*a0));
495
496 bncTime tt(GPSweek, GPSweeks);
497 double tk = tt - bncTime(_TOC.gpsw(), _TOEsec);
498
499 double n = n0 + _Delta_n;
500 double M = _M0 + n*tk;
501 double E = M;
502 double E_last;
503 do {
504 E_last = E;
505 E = M + _e*sin(E);
506 } while ( fabs(E-E_last)*a0 > 0.001 );
507 double v = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) );
508 double u0 = v + _omega;
509 double sin2u0 = sin(2*u0);
510 double cos2u0 = cos(2*u0);
511 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
512 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
513 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
514 double xp = r*cos(u);
515 double yp = r*sin(u);
516 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
517 omegaEarth*_TOEsec;
518
519 double sinom = sin(OM);
520 double cosom = cos(OM);
521 double sini = sin(i);
522 double cosi = cos(i);
523 xc[0] = xp*cosom - yp*cosi*sinom;
524 xc[1] = xp*sinom + yp*cosi*cosom;
525 xc[2] = yp*sini;
526
527 double tc = tt - _TOC;
528 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
529
530 // Velocity
531 // --------
532 double tanv2 = tan(v/2);
533 double dEdM = 1 / (1 - _e*cos(E));
534 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
535 * dEdM * n;
536 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
537 double dotom = _OMEGADOT - omegaEarth;
538 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
539 double dotr = a0 * _e*sin(E) * dEdM * n
540 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
541 double dotx = dotr*cos(u) - r*sin(u)*dotu;
542 double doty = dotr*sin(u) + r*cos(u)*dotu;
543
544 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
545 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
546 + yp*sini*sinom*doti; // dX / di
547
548 vv[1] = sinom *dotx + cosi*cosom *doty
549 + xp*cosom*dotom - yp*cosi*sinom*dotom
550 - yp*sini*cosom*doti;
551
552 vv[2] = sini *doty + yp*cosi *doti;
553
554 // Relativistic Correction
555 // -----------------------
556 // xc(4) -= 4.442807633e-10 * _e * sqrt(a0) *sin(E);
557 xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c;
558
559 return success;
560}
561
562// Constructor
563//////////////////////////////////////////////////////////////////////////////
564t_ephGPS::t_ephGPS(float rnxVersion, const QStringList& lines) {
565
566 const int nLines = 8;
567
568 if (lines.size() != nLines) {
569 _checkState = bad;
570 return;
571 }
572
573 // RINEX Format
574 // ------------
575 int fieldLen = 19;
576
577 int pos[4];
578 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
579 pos[1] = pos[0] + fieldLen;
580 pos[2] = pos[1] + fieldLen;
581 pos[3] = pos[2] + fieldLen;
582
583 // Read eight lines
584 // ----------------
585 for (int iLine = 0; iLine < nLines; iLine++) {
586 QString line = lines[iLine];
587
588 if ( iLine == 0 ) {
589 QTextStream in(line.left(pos[1]).toAscii());
590
591 int year, month, day, hour, min;
592 double sec;
593
594 QString prnStr;
595 in >> prnStr >> year >> month >> day >> hour >> min >> sec;
596 if (prnStr.at(0) == 'G') {
597 _prn.set('G', prnStr.mid(1).toInt());
598 }
599 else if (prnStr.at(0) == 'J') {
600 _prn.set('J', prnStr.mid(1).toInt());
601 }
602 else {
603 _prn.set('G', prnStr.toInt());
604 }
605
606 if (year < 80) {
607 year += 2000;
608 }
609 else if (year < 100) {
610 year += 1900;
611 }
612
613 _TOC.set(year, month, day, hour, min, sec);
614
615 if ( readDbl(line, pos[1], fieldLen, _clock_bias ) ||
616 readDbl(line, pos[2], fieldLen, _clock_drift ) ||
617 readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
618 _checkState = bad;
619 return;
620 }
621 }
622
623 else if ( iLine == 1 ) {
624 if ( readDbl(line, pos[0], fieldLen, _IODE ) ||
625 readDbl(line, pos[1], fieldLen, _Crs ) ||
626 readDbl(line, pos[2], fieldLen, _Delta_n) ||
627 readDbl(line, pos[3], fieldLen, _M0 ) ) {
628 _checkState = bad;
629 return;
630 }
631 }
632
633 else if ( iLine == 2 ) {
634 if ( readDbl(line, pos[0], fieldLen, _Cuc ) ||
635 readDbl(line, pos[1], fieldLen, _e ) ||
636 readDbl(line, pos[2], fieldLen, _Cus ) ||
637 readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
638 _checkState = bad;
639 return;
640 }
641 }
642
643 else if ( iLine == 3 ) {
644 if ( readDbl(line, pos[0], fieldLen, _TOEsec) ||
645 readDbl(line, pos[1], fieldLen, _Cic ) ||
646 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
647 readDbl(line, pos[3], fieldLen, _Cis ) ) {
648 _checkState = bad;
649 return;
650 }
651 }
652
653 else if ( iLine == 4 ) {
654 if ( readDbl(line, pos[0], fieldLen, _i0 ) ||
655 readDbl(line, pos[1], fieldLen, _Crc ) ||
656 readDbl(line, pos[2], fieldLen, _omega ) ||
657 readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
658 _checkState = bad;
659 return;
660 }
661 }
662
663 else if ( iLine == 5 ) {
664 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
665 readDbl(line, pos[1], fieldLen, _L2Codes) ||
666 readDbl(line, pos[2], fieldLen, _TOEweek ) ||
667 readDbl(line, pos[3], fieldLen, _L2PFlag) ) {
668 _checkState = bad;
669 return;
670 }
671 }
672
673 else if ( iLine == 6 ) {
674 if ( readDbl(line, pos[0], fieldLen, _ura ) ||
675 readDbl(line, pos[1], fieldLen, _health) ||
676 readDbl(line, pos[2], fieldLen, _TGD ) ||
677 readDbl(line, pos[3], fieldLen, _IODC ) ) {
678 _checkState = bad;
679 return;
680 }
681 }
682
683 else if ( iLine == 7 ) {
684 if ( readDbl(line, pos[0], fieldLen, _TOT) ) {
685 _checkState = bad;
686 return;
687 }
688 readDbl(line, pos[1], fieldLen, _fitInterval); // _fitInterval optional
689 }
690 }
691}
692
693// Constructor
694//////////////////////////////////////////////////////////////////////////////
695t_ephGlo::t_ephGlo(float rnxVersion, const QStringList& lines) {
696
697 const int nLines = 4;
698
699 if (lines.size() != nLines) {
700 _checkState = bad;
701 return;
702 }
703
704 // RINEX Format
705 // ------------
706 int fieldLen = 19;
707
708 int pos[4];
709 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
710 pos[1] = pos[0] + fieldLen;
711 pos[2] = pos[1] + fieldLen;
712 pos[3] = pos[2] + fieldLen;
713
714 // Read four lines
715 // ---------------
716 for (int iLine = 0; iLine < nLines; iLine++) {
717 QString line = lines[iLine];
718
719 if ( iLine == 0 ) {
720 QTextStream in(line.left(pos[1]).toAscii());
721
722 int year, month, day, hour, min;
723 double sec;
724
725 QString prnStr;
726 in >> prnStr >> year >> month >> day >> hour >> min >> sec;
727 if (prnStr.at(0) == 'R') {
728 _prn.set('R', prnStr.mid(1).toInt());
729 }
730 else {
731 _prn.set('R', prnStr.toInt());
732 }
733
734 if (year < 80) {
735 year += 2000;
736 }
737 else if (year < 100) {
738 year += 1900;
739 }
740
741 _gps_utc = gnumleap(year, month, day);
742
743 _TOC.set(year, month, day, hour, min, sec);
744 _TOC = _TOC + _gps_utc;
745
746 if ( readDbl(line, pos[1], fieldLen, _tau ) ||
747 readDbl(line, pos[2], fieldLen, _gamma) ||
748 readDbl(line, pos[3], fieldLen, _tki ) ) {
749 _checkState = bad;
750 return;
751 }
752
753 _tau = -_tau;
754 }
755
756 else if ( iLine == 1 ) {
757 if ( readDbl(line, pos[0], fieldLen, _x_pos ) ||
758 readDbl(line, pos[1], fieldLen, _x_velocity ) ||
759 readDbl(line, pos[2], fieldLen, _x_acceleration) ||
760 readDbl(line, pos[3], fieldLen, _health ) ) {
761 _checkState = bad;
762 return;
763 }
764 }
765
766 else if ( iLine == 2 ) {
767 if ( readDbl(line, pos[0], fieldLen, _y_pos ) ||
768 readDbl(line, pos[1], fieldLen, _y_velocity ) ||
769 readDbl(line, pos[2], fieldLen, _y_acceleration ) ||
770 readDbl(line, pos[3], fieldLen, _frequency_number) ) {
771 _checkState = bad;
772 return;
773 }
774 }
775
776 else if ( iLine == 3 ) {
777 if ( readDbl(line, pos[0], fieldLen, _z_pos ) ||
778 readDbl(line, pos[1], fieldLen, _z_velocity ) ||
779 readDbl(line, pos[2], fieldLen, _z_acceleration) ||
780 readDbl(line, pos[3], fieldLen, _E ) ) {
781 _checkState = bad;
782 return;
783 }
784 }
785 }
786
787 // Initialize status vector
788 // ------------------------
789 _tt = _TOC;
790 _xv.ReSize(6);
791 _xv(1) = _x_pos * 1.e3;
792 _xv(2) = _y_pos * 1.e3;
793 _xv(3) = _z_pos * 1.e3;
794 _xv(4) = _x_velocity * 1.e3;
795 _xv(5) = _y_velocity * 1.e3;
796 _xv(6) = _z_velocity * 1.e3;
797}
798
799// Constructor
800//////////////////////////////////////////////////////////////////////////////
801t_ephGal::t_ephGal(float rnxVersion, const QStringList& lines) {
802
803 const int nLines = 8;
804
805 if (lines.size() != nLines) {
806 _checkState = bad;
807 return;
808 }
809
810 // RINEX Format
811 // ------------
812 int fieldLen = 19;
813 double SVhealth = 0.0;
814 double datasource = 0.0;
815 int pos[4];
816 _SISAI = -1; // set index from RTCM invalid
817 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
818 pos[1] = pos[0] + fieldLen;
819 pos[2] = pos[1] + fieldLen;
820 pos[3] = pos[2] + fieldLen;
821
822 // Read eight lines
823 // ----------------
824 for (int iLine = 0; iLine < nLines; iLine++) {
825 QString line = lines[iLine];
826
827 if ( iLine == 0 ) {
828 QTextStream in(line.left(pos[1]).toAscii());
829
830 int year, month, day, hour, min;
831 double sec;
832
833 QString prnStr;
834 in >> prnStr >> year >> month >> day >> hour >> min >> sec;
835 if (prnStr.at(0) == 'E') {
836 _prn.set('E', prnStr.mid(1).toInt());
837 }
838 else {
839 _prn.set('E', prnStr.toInt());
840 }
841
842 if (year < 80) {
843 year += 2000;
844 }
845 else if (year < 100) {
846 year += 1900;
847 }
848
849 _TOC.set(year, month, day, hour, min, sec);
850
851 if ( readDbl(line, pos[1], fieldLen, _clock_bias ) ||
852 readDbl(line, pos[2], fieldLen, _clock_drift ) ||
853 readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
854 _checkState = bad;
855 return;
856 }
857 }
858
859 else if ( iLine == 1 ) {
860 if ( readDbl(line, pos[0], fieldLen, _IODnav ) ||
861 readDbl(line, pos[1], fieldLen, _Crs ) ||
862 readDbl(line, pos[2], fieldLen, _Delta_n) ||
863 readDbl(line, pos[3], fieldLen, _M0 ) ) {
864 _checkState = bad;
865 return;
866 }
867 }
868
869 else if ( iLine == 2 ) {
870 if ( readDbl(line, pos[0], fieldLen, _Cuc ) ||
871 readDbl(line, pos[1], fieldLen, _e ) ||
872 readDbl(line, pos[2], fieldLen, _Cus ) ||
873 readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
874 _checkState = bad;
875 return;
876 }
877 }
878
879 else if ( iLine == 3 ) {
880 if ( readDbl(line, pos[0], fieldLen, _TOEsec) ||
881 readDbl(line, pos[1], fieldLen, _Cic ) ||
882 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
883 readDbl(line, pos[3], fieldLen, _Cis ) ) {
884 _checkState = bad;
885 return;
886 }
887 }
888
889 else if ( iLine == 4 ) {
890 if ( readDbl(line, pos[0], fieldLen, _i0 ) ||
891 readDbl(line, pos[1], fieldLen, _Crc ) ||
892 readDbl(line, pos[2], fieldLen, _omega ) ||
893 readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
894 _checkState = bad;
895 return;
896 }
897 }
898
899 else if ( iLine == 5 ) {
900 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
901 readDbl(line, pos[1], fieldLen, datasource) ||
902 readDbl(line, pos[2], fieldLen, _TOEweek ) ) {
903 _checkState = bad;
904 return;
905 } else {
906 if (int(datasource) & (1<<8)) {
907 _flags |= GALEPHF_FNAV;
908 } else if (int(datasource) & (1<<9)) {
909 _flags |= GALEPHF_INAV;
910 }
911 }
912 }
913
914 else if ( iLine == 6 ) {
915 if ( readDbl(line, pos[0], fieldLen, _SISA ) ||
916 readDbl(line, pos[1], fieldLen, SVhealth) ||
917 readDbl(line, pos[2], fieldLen, _BGD_1_5A) ||
918 readDbl(line, pos[3], fieldLen, _BGD_1_5B) ) {
919 _checkState = bad;
920 return;
921 } else {
922 // Bit 0
923 if (int(SVhealth) & (1<<0)) {
924 _flags |= GALEPHF_E1DINVALID;
925 }
926 // Bit 1-2
927 _E1_bHS = double((int(SVhealth) >> 1) & 0x3);
928 // Bit 3
929 if (int(SVhealth) & (1<<3)) {
930 _flags |= GALEPHF_E5ADINVALID;
931 }
932 // Bit 4-5
933 _E5aHS = double((int(SVhealth) >> 4) & 0x3);
934 // Bit 6
935 if (int(SVhealth) & (1<<6)) {
936 _flags |= GALEPHF_E5BDINVALID;
937 }
938 // Bit 7-8
939 _E5bHS = double((int(SVhealth) >> 7) & 0x3);
940 }
941 }
942
943 else if ( iLine == 7 ) {
944 if ( readDbl(line, pos[0], fieldLen, _TOT) ) {
945 _checkState = bad;
946 return;
947 }
948 }
949 }
950}
951
952//
953//////////////////////////////////////////////////////////////////////////////
954QString t_eph::rinexDateStr(const bncTime& tt, const t_prn& prn, double version) {
955 QString prnStr(prn.toString().c_str());
956 return rinexDateStr(tt, prnStr, version);
957}
958
959//
960//////////////////////////////////////////////////////////////////////////////
961QString t_eph::rinexDateStr(const bncTime& tt, const QString& prnStr, double version) {
962
963 QString datStr;
964
965 unsigned year, month, day, hour, min;
966 double sec;
967 tt.civil_date(year, month, day);
968 tt.civil_time(hour, min, sec);
969
970 QTextStream out(&datStr);
971
972 if (version < 3.0) {
973 QString prnHlp = prnStr.mid(1,2); if (prnHlp[0] == '0') prnHlp[0] = ' ';
974 out << prnHlp << QString(" %1 %2 %3 %4 %5%6")
975 .arg(year % 100, 2, 10, QChar('0'))
976 .arg(month, 2)
977 .arg(day, 2)
978 .arg(hour, 2)
979 .arg(min, 2)
980 .arg(sec, 5, 'f',1);
981 }
982 else {
983 out << prnStr << QString(" %1 %2 %3 %4 %5 %6")
984 .arg(year, 4)
985 .arg(month, 2, 10, QChar('0'))
986 .arg(day, 2, 10, QChar('0'))
987 .arg(hour, 2, 10, QChar('0'))
988 .arg(min, 2, 10, QChar('0'))
989 .arg(int(sec), 2, 10, QChar('0'));
990 }
991
992 return datStr;
993}
994
995// RINEX Format String
996//////////////////////////////////////////////////////////////////////////////
997QString t_ephGPS::toString(double version) const {
998
999 QString rnxStr = rinexDateStr(_TOC, _prn, version);
1000
1001 QTextStream out(&rnxStr);
1002
1003 out << QString("%1%2%3\n")
1004 .arg(_clock_bias, 19, 'e', 12)
1005 .arg(_clock_drift, 19, 'e', 12)
1006 .arg(_clock_driftrate, 19, 'e', 12);
1007
1008 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1009
1010 out << QString(fmt)
1011 .arg(_IODE, 19, 'e', 12)
1012 .arg(_Crs, 19, 'e', 12)
1013 .arg(_Delta_n, 19, 'e', 12)
1014 .arg(_M0, 19, 'e', 12);
1015
1016 out << QString(fmt)
1017 .arg(_Cuc, 19, 'e', 12)
1018 .arg(_e, 19, 'e', 12)
1019 .arg(_Cus, 19, 'e', 12)
1020 .arg(_sqrt_A, 19, 'e', 12);
1021
1022 out << QString(fmt)
1023 .arg(_TOEsec, 19, 'e', 12)
1024 .arg(_Cic, 19, 'e', 12)
1025 .arg(_OMEGA0, 19, 'e', 12)
1026 .arg(_Cis, 19, 'e', 12);
1027
1028 out << QString(fmt)
1029 .arg(_i0, 19, 'e', 12)
1030 .arg(_Crc, 19, 'e', 12)
1031 .arg(_omega, 19, 'e', 12)
1032 .arg(_OMEGADOT, 19, 'e', 12);
1033
1034 out << QString(fmt)
1035 .arg(_IDOT, 19, 'e', 12)
1036 .arg(_L2Codes, 19, 'e', 12)
1037 .arg(_TOEweek, 19, 'e', 12)
1038 .arg(_L2PFlag, 19, 'e', 12);
1039
1040 out << QString(fmt)
1041 .arg(_ura, 19, 'e', 12)
1042 .arg(_health, 19, 'e', 12)
1043 .arg(_TGD, 19, 'e', 12)
1044 .arg(_IODC, 19, 'e', 12);
1045
1046 out << QString(fmt)
1047 .arg(_TOT, 19, 'e', 12)
1048 .arg(_fitInterval, 19, 'e', 12)
1049 .arg("", 19, QChar(' '))
1050 .arg("", 19, QChar(' '));
1051
1052 return rnxStr;
1053}
1054
1055// RINEX Format String
1056//////////////////////////////////////////////////////////////////////////////
1057QString t_ephGlo::toString(double version) const {
1058
1059 QString rnxStr = rinexDateStr(_TOC-_gps_utc, _prn, version);
1060
1061 QTextStream out(&rnxStr);
1062
1063 out << QString("%1%2%3\n")
1064 .arg(-_tau, 19, 'e', 12)
1065 .arg(_gamma, 19, 'e', 12)
1066 .arg(_tki, 19, 'e', 12);
1067
1068 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1069
1070 out << QString(fmt)
1071 .arg(_x_pos, 19, 'e', 12)
1072 .arg(_x_velocity, 19, 'e', 12)
1073 .arg(_x_acceleration, 19, 'e', 12)
1074 .arg(_health, 19, 'e', 12);
1075
1076 out << QString(fmt)
1077 .arg(_y_pos, 19, 'e', 12)
1078 .arg(_y_velocity, 19, 'e', 12)
1079 .arg(_y_acceleration, 19, 'e', 12)
1080 .arg(_frequency_number, 19, 'e', 12);
1081
1082 out << QString(fmt)
1083 .arg(_z_pos, 19, 'e', 12)
1084 .arg(_z_velocity, 19, 'e', 12)
1085 .arg(_z_acceleration, 19, 'e', 12)
1086 .arg(_E, 19, 'e', 12);
1087
1088 return rnxStr;
1089}
1090
1091// RINEX Format String
1092//////////////////////////////////////////////////////////////////////////////
1093QString t_ephGal::toString(double version) const {
1094
1095 QString rnxStr = rinexDateStr(_TOC, _prn, version);
1096
1097 QTextStream out(&rnxStr);
1098
1099 out << QString("%1%2%3\n")
1100 .arg(_clock_bias, 19, 'e', 12)
1101 .arg(_clock_drift, 19, 'e', 12)
1102 .arg(_clock_driftrate, 19, 'e', 12);
1103
1104 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1105
1106 out << QString(fmt)
1107 .arg(_IODnav, 19, 'e', 12)
1108 .arg(_Crs, 19, 'e', 12)
1109 .arg(_Delta_n, 19, 'e', 12)
1110 .arg(_M0, 19, 'e', 12);
1111
1112 out << QString(fmt)
1113 .arg(_Cuc, 19, 'e', 12)
1114 .arg(_e, 19, 'e', 12)
1115 .arg(_Cus, 19, 'e', 12)
1116 .arg(_sqrt_A, 19, 'e', 12);
1117
1118 out << QString(fmt)
1119 .arg(_TOEsec, 19, 'e', 12)
1120 .arg(_Cic, 19, 'e', 12)
1121 .arg(_OMEGA0, 19, 'e', 12)
1122 .arg(_Cis, 19, 'e', 12);
1123
1124 out << QString(fmt)
1125 .arg(_i0, 19, 'e', 12)
1126 .arg(_Crc, 19, 'e', 12)
1127 .arg(_omega, 19, 'e', 12)
1128 .arg(_OMEGADOT, 19, 'e', 12);
1129
1130 int dataSource = 0;
1131 int SVhealth = 0;
1132 double BGD_1_5A = _BGD_1_5A;
1133 double BGD_1_5B = _BGD_1_5B;
1134 if ((_flags & GALEPHF_FNAV) == GALEPHF_FNAV) {
1135 dataSource |= (1<<1);
1136 dataSource |= (1<<8);
1137 BGD_1_5B = 0.0;
1138 // SVhealth
1139 // Bit 3 : E5a DVS
1140 if ((_flags & GALEPHF_E5ADINVALID) == GALEPHF_E5ADINVALID) {
1141 SVhealth |= (1<<3);
1142 }
1143 // Bit 4-5: E5a HS
1144 if (_E5aHS == 1.0) {
1145 SVhealth |= (1<<4);
1146 }
1147 else if (_E5aHS == 2.0) {
1148 SVhealth |= (1<<5);
1149 }
1150 else if (_E5aHS == 3.0) {
1151 SVhealth |= (1<<4);
1152 SVhealth |= (1<<5);
1153 }
1154 }
1155 else if ((_flags & GALEPHF_INAV) == GALEPHF_INAV) {
1156 dataSource |= (1<<0);
1157 dataSource |= (1<<2);
1158 dataSource |= (1<<9);
1159 // SVhealth
1160 // Bit 0 : E1-B DVS
1161 if ((_flags & GALEPHF_E1DINVALID) == GALEPHF_E1DINVALID) {
1162 SVhealth |= (1<<0);
1163 }
1164 // Bit 1-2: E1-B HS
1165 if (_E1_bHS == 1.0) {
1166 SVhealth |= (1<<1);
1167 }
1168 else if (_E1_bHS == 2.0) {
1169 SVhealth |= (1<<2);
1170 }
1171 else if (_E1_bHS == 3.0) {
1172 SVhealth |= (1<<1);
1173 SVhealth |= (1<<2);
1174 }
1175 // Bit 6 : E5b DVS
1176 if ((_flags & GALEPHF_E1DINVALID) == GALEPHF_E1DINVALID) {
1177 SVhealth |= (1<<6);
1178 }
1179 // Bit 7-8: E5b HS
1180 if (_E5bHS == 1.0) {
1181 SVhealth |= (1<<7);
1182 }
1183 else if (_E5bHS == 2.0) {
1184 SVhealth |= (1<<8);
1185 }
1186 else if (_E5bHS == 3.0) {
1187 SVhealth |= (1<<7);
1188 SVhealth |= (1<<8);
1189 }
1190 }
1191
1192 out << QString(fmt)
1193 .arg(_IDOT, 19, 'e', 12)
1194 .arg(double(dataSource), 19, 'e', 12)
1195 .arg(_TOEweek, 19, 'e', 12)
1196 .arg(0.0, 19, 'e', 12);
1197
1198 // RINEX file input
1199 double SISA = _SISA; // [m]
1200 // RTCM stream input
1201 if ((_SISAI >= 0) && (_SISAI <= 49)) {
1202 SISA = _SISAI / 100.0;
1203 }
1204 if((_SISAI >= 50) && (_SISAI <= 74)) {
1205 SISA = (50 + (_SISAI - 50.0) * 2.0) / 100.0;
1206 }
1207 if((_SISAI >= 75) && (_SISAI <= 99)) {
1208 SISA = 1.0 + (_SISAI - 75.0) * 0.04;
1209 }
1210 if((_SISAI >= 100) && (_SISAI <= 125)) {
1211 SISA = 2.0 + (_SISAI - 100.0) * 0.16;
1212 }
1213 if (_SISAI >= 126 ) {
1214 SISA = -1.0;
1215 }
1216 out << QString(fmt)
1217 .arg(SISA, 19, 'e', 12)
1218 .arg(double(SVhealth), 19, 'e', 12)
1219 .arg(BGD_1_5A, 19, 'e', 12)
1220 .arg(BGD_1_5B, 19, 'e', 12);
1221
1222 out << QString(fmt)
1223 .arg(_TOT, 19, 'e', 12)
1224 .arg("", 19, QChar(' '))
1225 .arg("", 19, QChar(' '))
1226 .arg("", 19, QChar(' '));
1227
1228 return rnxStr;
1229}
1230
1231// Constructor
1232//////////////////////////////////////////////////////////////////////////////
1233t_ephSBAS::t_ephSBAS(float rnxVersion, const QStringList& lines) {
1234
1235 const int nLines = 4;
1236
1237 if (lines.size() != nLines) {
1238 _checkState = bad;
1239 return;
1240 }
1241
1242 // RINEX Format
1243 // ------------
1244 int fieldLen = 19;
1245
1246 int pos[4];
1247 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
1248 pos[1] = pos[0] + fieldLen;
1249 pos[2] = pos[1] + fieldLen;
1250 pos[3] = pos[2] + fieldLen;
1251
1252 // Read four lines
1253 // ---------------
1254 for (int iLine = 0; iLine < nLines; iLine++) {
1255 QString line = lines[iLine];
1256
1257 if ( iLine == 0 ) {
1258 QTextStream in(line.left(pos[1]).toAscii());
1259
1260 int year, month, day, hour, min;
1261 double sec;
1262
1263 QString prnStr;
1264 in >> prnStr >> year >> month >> day >> hour >> min >> sec;
1265 if (prnStr.at(0) == 'S') {
1266 _prn.set('S', prnStr.mid(1).toInt());
1267 }
1268 else {
1269 _prn.set('S', prnStr.toInt());
1270 }
1271
1272 if (year < 80) {
1273 year += 2000;
1274 }
1275 else if (year < 100) {
1276 year += 1900;
1277 }
1278
1279 _TOC.set(year, month, day, hour, min, sec);
1280
1281 if ( readDbl(line, pos[1], fieldLen, _agf0 ) ||
1282 readDbl(line, pos[2], fieldLen, _agf1 ) ||
1283 readDbl(line, pos[3], fieldLen, _TOW ) ) {
1284 _checkState = bad;
1285 return;
1286 }
1287 }
1288
1289 else if ( iLine == 1 ) {
1290 if ( readDbl(line, pos[0], fieldLen, _x_pos ) ||
1291 readDbl(line, pos[1], fieldLen, _x_velocity ) ||
1292 readDbl(line, pos[2], fieldLen, _x_acceleration) ||
1293 readDbl(line, pos[3], fieldLen, _health ) ) {
1294 _checkState = bad;
1295 return;
1296 }
1297 }
1298
1299 else if ( iLine == 2 ) {
1300 if ( readDbl(line, pos[0], fieldLen, _y_pos ) ||
1301 readDbl(line, pos[1], fieldLen, _y_velocity ) ||
1302 readDbl(line, pos[2], fieldLen, _y_acceleration ) ||
1303 readDbl(line, pos[3], fieldLen, _ura ) ) {
1304 _checkState = bad;
1305 return;
1306 }
1307 }
1308
1309 else if ( iLine == 3 ) {
1310 double iodn;
1311 if ( readDbl(line, pos[0], fieldLen, _z_pos ) ||
1312 readDbl(line, pos[1], fieldLen, _z_velocity ) ||
1313 readDbl(line, pos[2], fieldLen, _z_acceleration) ||
1314 readDbl(line, pos[3], fieldLen, iodn ) ) {
1315 _checkState = bad;
1316 return;
1317 _IODN = int(iodn);
1318 }
1319 }
1320 }
1321
1322 _x_pos *= 1.e3;
1323 _y_pos *= 1.e3;
1324 _z_pos *= 1.e3;
1325 _x_velocity *= 1.e3;
1326 _y_velocity *= 1.e3;
1327 _z_velocity *= 1.e3;
1328 _x_acceleration *= 1.e3;
1329 _y_acceleration *= 1.e3;
1330 _z_acceleration *= 1.e3;
1331}
1332
1333// Set SBAS Satellite Position
1334////////////////////////////////////////////////////////////////////////////
1335void t_ephSBAS::set(const sbasephemeris* ee) {
1336
1337 _prn.set('S', ee->satellite - PRN_SBAS_START + 20);
1338 _TOC.set(ee->GPSweek_TOE, double(ee->TOE));
1339
1340 _IODN = ee->IODN;
1341 _TOW = ee->TOW;
1342
1343 _agf0 = ee->agf0;
1344 _agf1 = ee->agf1;
1345
1346 _x_pos = ee->x_pos;
1347 _x_velocity = ee->x_velocity;
1348 _x_acceleration = ee->x_acceleration;
1349
1350 _y_pos = ee->y_pos;
1351 _y_velocity = ee->y_velocity;
1352 _y_acceleration = ee->y_acceleration;
1353
1354 _z_pos = ee->z_pos;
1355 _z_velocity = ee->z_velocity;
1356 _z_acceleration = ee->z_acceleration;
1357
1358 _ura = ee->URA;
1359
1360 _health = 0;
1361}
1362
1363// Compute SBAS Satellite Position (virtual)
1364////////////////////////////////////////////////////////////////////////////
1365t_irc t_ephSBAS::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
1366
1367 if (_checkState == bad) {
1368 return failure;
1369 }
1370
1371 bncTime tt(GPSweek, GPSweeks);
1372 double dt = tt - _TOC;
1373
1374 xc[0] = _x_pos + _x_velocity * dt + _x_acceleration * dt * dt / 2.0;
1375 xc[1] = _y_pos + _y_velocity * dt + _y_acceleration * dt * dt / 2.0;
1376 xc[2] = _z_pos + _z_velocity * dt + _z_acceleration * dt * dt / 2.0;
1377
1378 vv[0] = _x_velocity + _x_acceleration * dt;
1379 vv[1] = _y_velocity + _y_acceleration * dt;
1380 vv[2] = _z_velocity + _z_acceleration * dt;
1381
1382 xc[3] = _agf0 + _agf1 * dt;
1383
1384 return success;
1385}
1386
1387// RINEX Format String
1388//////////////////////////////////////////////////////////////////////////////
1389QString t_ephSBAS::toString(double version) const {
1390
1391 QString rnxStr = rinexDateStr(_TOC, _prn, version);
1392
1393 QTextStream out(&rnxStr);
1394
1395 out << QString("%1%2%3\n")
1396 .arg(_agf0, 19, 'e', 12)
1397 .arg(_agf1, 19, 'e', 12)
1398 .arg(_TOW, 19, 'e', 12);
1399
1400 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1401
1402 out << QString(fmt)
1403 .arg(1.e-3*_x_pos, 19, 'e', 12)
1404 .arg(1.e-3*_x_velocity, 19, 'e', 12)
1405 .arg(1.e-3*_x_acceleration, 19, 'e', 12)
1406 .arg(_health, 19, 'e', 12);
1407
1408 out << QString(fmt)
1409 .arg(1.e-3*_y_pos, 19, 'e', 12)
1410 .arg(1.e-3*_y_velocity, 19, 'e', 12)
1411 .arg(1.e-3*_y_acceleration, 19, 'e', 12)
1412 .arg(_ura, 19, 'e', 12);
1413
1414 out << QString(fmt)
1415 .arg(1.e-3*_z_pos, 19, 'e', 12)
1416 .arg(1.e-3*_z_velocity, 19, 'e', 12)
1417 .arg(1.e-3*_z_acceleration, 19, 'e', 12)
1418 .arg(double(_IODN), 19, 'e', 12);
1419
1420 return rnxStr;
1421}
1422
1423// Constructor
1424//////////////////////////////////////////////////////////////////////////////
1425t_ephBDS::t_ephBDS(float rnxVersion, const QStringList& lines) {
1426
1427 const int nLines = 8;
1428
1429 if (lines.size() != nLines) {
1430 _checkState = bad;
1431 return;
1432 }
1433
1434 // RINEX Format
1435 // ------------
1436 int fieldLen = 19;
1437 double TOEw;
1438 _URAI = -1; // RINEX usage: set RTCM entry to be undefined
1439
1440 int pos[4];
1441 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
1442 pos[1] = pos[0] + fieldLen;
1443 pos[2] = pos[1] + fieldLen;
1444 pos[3] = pos[2] + fieldLen;
1445
1446 // Read eight lines
1447 // ----------------
1448 for (int iLine = 0; iLine < nLines; iLine++) {
1449 QString line = lines[iLine];
1450
1451 if ( iLine == 0 ) {
1452 QTextStream in(line.left(pos[1]).toAscii());
1453
1454 int year, month, day, hour, min;
1455 double sec;
1456
1457 QString prnStr;
1458 in >> prnStr >> year >> month >> day >> hour >> min >> sec;
1459 if (prnStr.at(0) == 'C') {
1460 _prn.set('C', prnStr.mid(1).toInt());
1461 }
1462 else {
1463 _prn.set('C', prnStr.toInt());
1464 }
1465
1466 if (year < 80) {
1467 year += 2000;
1468 }
1469 else if (year < 100) {
1470 year += 1900;
1471 }
1472
1473 _TOC_bdt.set(year, month, day, hour, min, sec);
1474
1475 if ( readDbl(line, pos[1], fieldLen, _clock_bias ) ||
1476 readDbl(line, pos[2], fieldLen, _clock_drift ) ||
1477 readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
1478 _checkState = bad;
1479 return;
1480 }
1481 }
1482
1483 else if ( iLine == 1 ) {
1484 double aode;
1485 if ( readDbl(line, pos[0], fieldLen, aode ) ||
1486 readDbl(line, pos[1], fieldLen, _Crs ) ||
1487 readDbl(line, pos[2], fieldLen, _Delta_n) ||
1488 readDbl(line, pos[3], fieldLen, _M0 ) ) {
1489 _checkState = bad;
1490 return;
1491 }
1492 _AODE = int(aode);
1493 }
1494
1495 else if ( iLine == 2 ) {
1496 if ( readDbl(line, pos[0], fieldLen, _Cuc ) ||
1497 readDbl(line, pos[1], fieldLen, _e ) ||
1498 readDbl(line, pos[2], fieldLen, _Cus ) ||
1499 readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
1500 _checkState = bad;
1501 return;
1502 }
1503 }
1504
1505 else if ( iLine == 3 ) {
1506 if ( readDbl(line, pos[0], fieldLen, _TOEs ) ||
1507 readDbl(line, pos[1], fieldLen, _Cic ) ||
1508 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
1509 readDbl(line, pos[3], fieldLen, _Cis ) ) {
1510 _checkState = bad;
1511 return;
1512 }
1513 }
1514
1515 else if ( iLine == 4 ) {
1516 if ( readDbl(line, pos[0], fieldLen, _i0 ) ||
1517 readDbl(line, pos[1], fieldLen, _Crc ) ||
1518 readDbl(line, pos[2], fieldLen, _omega ) ||
1519 readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
1520 _checkState = bad;
1521 return;
1522 }
1523 }
1524
1525 else if ( iLine == 5 ) {
1526 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
1527 readDbl(line, pos[2], fieldLen, TOEw) ) {
1528 _checkState = bad;
1529 return;
1530 }
1531 }
1532
1533 else if ( iLine == 6 ) {
1534 double SatH1;
1535 if ( readDbl(line, pos[0], fieldLen, _URA ) ||
1536 readDbl(line, pos[1], fieldLen, SatH1) ||
1537 readDbl(line, pos[2], fieldLen, _TGD1) ||
1538 readDbl(line, pos[3], fieldLen, _TGD2) ) {
1539 _checkState = bad;
1540 return;
1541 }
1542 _SatH1 = int(SatH1);
1543 }
1544
1545 else if ( iLine == 7 ) {
1546 double aodc;
1547 if ( readDbl(line, pos[0], fieldLen, _TOTs) ||
1548 readDbl(line, pos[1], fieldLen, aodc) ) {
1549 _checkState = bad;
1550 return;
1551 }
1552 if (_TOTs == 0.9999e9) { // 0.9999e9 means not known (RINEX standard)
1553 _TOTs = _TOEs;
1554 }
1555 _AODC = int(aodc);
1556 }
1557 }
1558
1559 TOEw += 1356; // BDT -> GPS week number
1560 _TOE_bdt.set(int(TOEw), _TOEs);
1561
1562 // GPS->BDT
1563 // --------
1564 _TOC = _TOC_bdt + 14.0;
1565 _TOE = _TOE_bdt + 14.0;
1566
1567 // remark: actually should be computed from second_tot
1568 // but it seems to be unreliable in RINEX files
1569 _TOT = _TOC;
1570}
1571
1572// Set BDS Satellite Position
1573////////////////////////////////////////////////////////////////////////////
1574void t_ephBDS::set(const bdsephemeris* ee) {
1575
1576 // RTCM usage: set RINEX File entries to zero
1577 // ------------------------------------------
1578 _TOTs = 0.0;
1579 _TOEs = 0.0;
1580
1581 _receptDateTime = currentDateAndTimeGPS();
1582
1583 _prn.set('C', ee->satellite - PRN_BDS_START + 1);
1584
1585 _TOE_bdt.set(1356 + ee->BDSweek, ee->TOE);
1586 _TOE = _TOE_bdt + 14.0;
1587
1588 _TOC_bdt.set(1356 + ee->BDSweek, ee->TOC);
1589 _TOC = _TOC_bdt + 14.0;
1590
1591 _AODE = ee->AODE;
1592 _AODC = ee->AODC;
1593
1594 _clock_bias = ee->clock_bias;
1595 _clock_drift = ee->clock_drift;
1596 _clock_driftrate = ee->clock_driftrate;
1597
1598 _Crs = ee->Crs;
1599 _Delta_n = ee->Delta_n;
1600 _M0 = ee->M0;
1601
1602 _Cuc = ee->Cuc;
1603 _e = ee->e;
1604 _Cus = ee->Cus;
1605 _sqrt_A = ee->sqrt_A;
1606
1607 _Cic = ee->Cic;
1608 _OMEGA0 = ee->OMEGA0;
1609 _Cis = ee->Cis;
1610
1611 _i0 = ee->i0;
1612 _Crc = ee->Crc;
1613 _omega = ee->omega;
1614 _OMEGADOT = ee->OMEGADOT;
1615 _IDOT = ee->IDOT;
1616
1617 _TGD1 = ee->TGD_B1_B3;
1618 _TGD2 = ee->TGD_B2_B3;
1619
1620 _URAI = ee->URAI;
1621 _SatH1 = (ee->flags & BDSEPHF_SATH1) ? 1: 0;
1622
1623}
1624
1625// Compute BDS Satellite Position (virtual)
1626//////////////////////////////////////////////////////////////////////////////
1627t_irc t_ephBDS::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
1628
1629 if (_checkState == bad) {
1630 return failure;
1631 }
1632
1633 static const double gmBDS = 398.6004418e12;
1634 static const double omegaBDS = 7292115.0000e-11;
1635
1636 xc[0] = xc[1] = xc[2] = xc[3] = 0.0;
1637 vv[0] = vv[1] = vv[2] = 0.0;
1638
1639 bncTime tt(GPSweek, GPSweeks);
1640
1641 if (_sqrt_A == 0) {
1642 return failure;
1643 }
1644 double a0 = _sqrt_A * _sqrt_A;
1645
1646 double n0 = sqrt(gmBDS/(a0*a0*a0));
1647 double tk = tt - _TOE;
1648 double n = n0 + _Delta_n;
1649 double M = _M0 + n*tk;
1650 double E = M;
1651 double E_last;
1652 int nLoop = 0;
1653 do {
1654 E_last = E;
1655 E = M + _e*sin(E);
1656
1657 if (++nLoop == 100) {
1658 return failure;
1659 }
1660 } while ( fabs(E-E_last)*a0 > 0.001 );
1661
1662 double v = atan2(sqrt(1-_e*_e) * sin(E), cos(E) - _e);
1663 double u0 = v + _omega;
1664 double sin2u0 = sin(2*u0);
1665 double cos2u0 = cos(2*u0);
1666 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
1667 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
1668 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
1669 double xp = r*cos(u);
1670 double yp = r*sin(u);
1671 double toesec = (_TOE.gpssec() - 14.0);
1672
1673 double sinom = 0;
1674 double cosom = 0;
1675 double sini = 0;
1676 double cosi = 0;
1677
1678 const double iMaxGEO = 10.0 / 180.0 * M_PI;
1679
1680 // MEO/IGSO satellite
1681 // ------------------
1682 if (_i0 > iMaxGEO) {
1683 double OM = _OMEGA0 + (_OMEGADOT - omegaBDS)*tk - omegaBDS*toesec;
1684
1685 sinom = sin(OM);
1686 cosom = cos(OM);
1687 sini = sin(i);
1688 cosi = cos(i);
1689
1690 xc[0] = xp*cosom - yp*cosi*sinom;
1691 xc[1] = xp*sinom + yp*cosi*cosom;
1692 xc[2] = yp*sini;
1693 }
1694
1695 // GEO satellite
1696 // -------------
1697 else {
1698 double OM = _OMEGA0 + _OMEGADOT*tk - omegaBDS*toesec;
1699 double ll = omegaBDS*tk;
1700
1701 sinom = sin(OM);
1702 cosom = cos(OM);
1703 sini = sin(i);
1704 cosi = cos(i);
1705
1706 double xx = xp*cosom - yp*cosi*sinom;
1707 double yy = xp*sinom + yp*cosi*cosom;
1708 double zz = yp*sini;
1709
1710 Matrix R1 = BNC_PPP::t_astro::rotX(-5.0 / 180.0 * M_PI);
1711 Matrix R2 = BNC_PPP::t_astro::rotZ(ll);
1712
1713 ColumnVector X1(3); X1 << xx << yy << zz;
1714 ColumnVector X2 = R2*R1*X1;
1715
1716 xc[0] = X2(1);
1717 xc[1] = X2(2);
1718 xc[2] = X2(3);
1719 }
1720
1721 double tc = tt - _TOC;
1722 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc
1723 - 4.442807633e-10 * _e * sqrt(a0) *sin(E);
1724
1725 // Velocity
1726 // --------
1727 double tanv2 = tan(v/2);
1728 double dEdM = 1 / (1 - _e*cos(E));
1729 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2)
1730 / (1 + tanv2*tanv2) * dEdM * n;
1731 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
1732 double dotom = _OMEGADOT - t_CST::omega;
1733 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
1734 double dotr = a0 * _e*sin(E) * dEdM * n
1735 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
1736 double dotx = dotr*cos(u) - r*sin(u)*dotu;
1737 double doty = dotr*sin(u) + r*cos(u)*dotu;
1738
1739 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
1740 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
1741 + yp*sini*sinom*doti; // dX / di
1742
1743 vv[1] = sinom *dotx + cosi*cosom *doty
1744 + xp*cosom*dotom - yp*cosi*sinom*dotom
1745 - yp*sini*cosom*doti;
1746
1747 vv[2] = sini *doty + yp*cosi *doti;
1748
1749 // dotC = _clock_drift + _clock_driftrate*tc
1750 // - 4.442807633e-10*_e*sqrt(a0)*cos(E) * dEdM * n;
1751
1752 return success;
1753}
1754
1755// RINEX Format String
1756//////////////////////////////////////////////////////////////////////////////
1757QString t_ephBDS::toString(double version) const {
1758
1759 QString rnxStr = rinexDateStr(_TOC_bdt, _prn, version);
1760
1761 QTextStream out(&rnxStr);
1762
1763 out << QString("%1%2%3\n")
1764 .arg(_clock_bias, 19, 'e', 12)
1765 .arg(_clock_drift, 19, 'e', 12)
1766 .arg(_clock_driftrate, 19, 'e', 12);
1767
1768 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1769
1770 out << QString(fmt)
1771 .arg(double(_AODE), 19, 'e', 12)
1772 .arg(_Crs, 19, 'e', 12)
1773 .arg(_Delta_n, 19, 'e', 12)
1774 .arg(_M0, 19, 'e', 12);
1775
1776 out << QString(fmt)
1777 .arg(_Cuc, 19, 'e', 12)
1778 .arg(_e, 19, 'e', 12)
1779 .arg(_Cus, 19, 'e', 12)
1780 .arg(_sqrt_A, 19, 'e', 12);
1781
1782 double toes = _TOEs;
1783 if (!toes) { // RTCM stream input
1784 toes = _TOE_bdt.gpssec();
1785 }
1786 out << QString(fmt)
1787 .arg(toes, 19, 'e', 12)
1788 .arg(_Cic, 19, 'e', 12)
1789 .arg(_OMEGA0, 19, 'e', 12)
1790 .arg(_Cis, 19, 'e', 12);
1791
1792 out << QString(fmt)
1793 .arg(_i0, 19, 'e', 12)
1794 .arg(_Crc, 19, 'e', 12)
1795 .arg(_omega, 19, 'e', 12)
1796 .arg(_OMEGADOT, 19, 'e', 12);
1797
1798
1799 out << QString(fmt)
1800 .arg(_IDOT, 19, 'e', 12)
1801 .arg(0.0, 19, 'e', 12)
1802 .arg(double(_TOE_bdt.gpsw() - 1356.0), 19, 'e', 12)
1803 .arg(0.0, 19, 'e', 12);
1804
1805 // RINEX file input
1806 double ura = _URA;
1807 // RTCM stream input
1808 if ((_URAI < 6) && (_URAI >= 0)) {
1809 ura = ceil(10.0 * pow(2.0, ((double)_URAI/2.0) + 1.0)) / 10.0;
1810 }
1811 if ((_URAI >= 6) && (_URAI < 15)) {
1812 ura = ceil(10.0 * pow(2.0, ((double)_URAI/2.0) )) / 10.0;
1813 }
1814 out << QString(fmt)
1815 .arg(ura, 19, 'e', 12)
1816 .arg(double(_SatH1), 19, 'e', 12)
1817 .arg(_TGD1, 19, 'e', 12)
1818 .arg(_TGD2, 19, 'e', 12);
1819
1820 double tots = _TOTs;
1821 if (!tots) { // RTCM stream input
1822 tots = _TOE_bdt.gpssec();
1823 }
1824 out << QString(fmt)
1825 .arg(tots, 19, 'e', 12)
1826 .arg(double(_AODC), 19, 'e', 12)
1827 .arg("", 19, QChar(' '))
1828 .arg("", 19, QChar(' '));
1829 return rnxStr;
1830}
1831
Note: See TracBrowser for help on using the repository browser.