source: ntrip/branches/BNC_2.12/src/ephemeris.cpp @ 8216

Last change on this file since 8216 was 8216, checked in by stuerze, 15 months ago

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