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

Last change on this file was 8800, checked in by stuerze, 31 hours ago

small bug fixed with respect to GLONASS message frame time, written into RINEX files

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