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

Last change on this file since 8483 was 8483, checked in by stuerze, 11 months ago

SSR parameter clock rate, clock drift and URA are added within RTNET format

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