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

Last change on this file since 8484 was 8484, checked in by stuerze, 12 months ago

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

File size: 49.2 KB
Line 
1#include <sstream>
2#include <iostream>
3#include <iomanip>
4#include <cstring>
5
6#include <newmatio.h>
7
8#include "ephemeris.h"
9#include "bncutils.h"
10#include "bnctime.h"
11#include "bnccore.h"
12#include "bncutils.h"
13#include "satObs.h"
14#include "pppInclude.h"
15#include "pppModel.h"
16
17using namespace std;
18
19// Constructor
20////////////////////////////////////////////////////////////////////////////
21t_eph::t_eph() {
22  _checkState = unchecked;
23  _orbCorr    = 0;
24  _clkCorr    = 0;
25}
26// 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]).toAscii());
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]).toAscii());
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
630  int nSteps  = int(fabs(dtPos) / nominalStep) + 1;
631  double step = dtPos / nSteps;
632
633  double acc[3];
634  acc[0] = _x_acceleration * 1.e3;
635  acc[1] = _y_acceleration * 1.e3;
636  acc[2] = _z_acceleration * 1.e3;
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// Constructor
764//////////////////////////////////////////////////////////////////////////////
765t_ephGal::t_ephGal(float rnxVersion, const QStringList& lines) {
766  int       year, month, day, hour, min;
767  double    sec;
768  QString   prnStr;
769  const int nLines = 8;
770  if (lines.size() != nLines) {
771    _checkState = bad;
772    return;
773  }
774
775  // RINEX Format
776  // ------------
777  int fieldLen = 19;
778  double SVhealth = 0.0;
779  double datasource = 0.0;
780
781  int pos[4];
782  pos[0] = (rnxVersion <= 2.12) ?  3 :  4;
783  pos[1] = pos[0] + fieldLen;
784  pos[2] = pos[1] + fieldLen;
785  pos[3] = pos[2] + fieldLen;
786
787  // Read eight lines
788  // ----------------
789  for (int iLine = 0; iLine < nLines; iLine++) {
790    QString line = lines[iLine];
791
792    if      ( iLine == 0 ) {
793      QTextStream in(line.left(pos[1]).toAscii());
794      QString n;
795      in >> prnStr;
796      if (prnStr.size() == 1 && prnStr[0] == 'E') {
797        in >> n;
798        prnStr.append(n);
799      }
800      in >> year >> month >> day >> hour >> min >> sec;
801      if      (year <  80) {
802        year += 2000;
803      }
804      else if (year < 100) {
805        year += 1900;
806      }
807
808      _TOC.set(year, month, day, hour, min, sec);
809
810      if ( readDbl(line, pos[1], fieldLen, _clock_bias     ) ||
811           readDbl(line, pos[2], fieldLen, _clock_drift    ) ||
812           readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
813        _checkState = bad;
814        return;
815      }
816    }
817
818    else if      ( iLine == 1 ) {
819      if ( readDbl(line, pos[0], fieldLen, _IODnav ) ||
820           readDbl(line, pos[1], fieldLen, _Crs    ) ||
821           readDbl(line, pos[2], fieldLen, _Delta_n) ||
822           readDbl(line, pos[3], fieldLen, _M0     ) ) {
823        _checkState = bad;
824        return;
825      }
826    }
827
828    else if ( iLine == 2 ) {
829      if ( readDbl(line, pos[0], fieldLen, _Cuc   ) ||
830           readDbl(line, pos[1], fieldLen, _e     ) ||
831           readDbl(line, pos[2], fieldLen, _Cus   ) ||
832           readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
833        _checkState = bad;
834        return;
835      }
836    }
837
838    else if ( iLine == 3 ) {
839      if ( readDbl(line, pos[0], fieldLen, _TOEsec)  ||
840           readDbl(line, pos[1], fieldLen, _Cic   )  ||
841           readDbl(line, pos[2], fieldLen, _OMEGA0)  ||
842           readDbl(line, pos[3], fieldLen, _Cis   ) ) {
843        _checkState = bad;
844        return;
845      }
846    }
847
848    else if ( iLine == 4 ) {
849      if ( readDbl(line, pos[0], fieldLen, _i0      ) ||
850           readDbl(line, pos[1], fieldLen, _Crc     ) ||
851           readDbl(line, pos[2], fieldLen, _omega   ) ||
852           readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
853        _checkState = bad;
854        return;
855      }
856    }
857
858    else if ( iLine == 5 ) {
859      if ( readDbl(line, pos[0], fieldLen, _IDOT      ) ||
860           readDbl(line, pos[1], fieldLen, datasource) ||
861           readDbl(line, pos[2], fieldLen, _TOEweek   ) ) {
862        _checkState = bad;
863        return;
864      } else {
865        if        (int(datasource) & (1<<8)) {
866          _fnav = true;
867          _inav = false;
868        } else if (int(datasource) & (1<<9)) {
869          _fnav = false;
870          _inav = true;
871        }
872        _TOEweek -= 1024.0;
873      }
874    }
875
876    else if ( iLine == 6 ) {
877      if ( readDbl(line, pos[0], fieldLen, _SISA    ) ||
878           readDbl(line, pos[1], fieldLen,  SVhealth) ||
879           readDbl(line, pos[2], fieldLen, _BGD_1_5A) ||
880           readDbl(line, pos[3], fieldLen, _BGD_1_5B) ) {
881        _checkState = bad;
882        return;
883      } else {
884        // Bit 0
885        _e1DataInValid = (int(SVhealth) & (1<<0));
886        // Bit 1-2
887        _E1_bHS = double((int(SVhealth) >> 1) & 0x3);
888        // Bit 3
889        _e5aDataInValid = (int(SVhealth) & (1<<3));
890        // Bit 4-5
891        _E5aHS = double((int(SVhealth) >> 4) & 0x3);
892        // Bit 6
893        _e5bDataInValid = (int(SVhealth) & (1<<6));
894        // Bit 7-8
895        _E5bHS = double((int(SVhealth) >> 7) & 0x3);
896
897        if (prnStr.at(0) == 'E') {
898          _prn.set('E', prnStr.mid(1).toInt(), _inav ? 1 : 0);
899        }
900      }
901    }
902
903    else if ( iLine == 7 ) {
904      if ( readDbl(line, pos[0], fieldLen, _TOT) ) {
905        _checkState = bad;
906        return;
907      }
908    }
909  }
910}
911
912// Compute Galileo Satellite Position (virtual)
913////////////////////////////////////////////////////////////////////////////
914t_irc t_ephGal::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
915
916  if (_checkState == bad ||
917      _checkState == unhealthy) {
918    return failure;
919  }
920
921  static const double omegaEarth = 7292115.1467e-11;
922  static const double gmWGS = 398.6004418e12;
923
924  memset(xc, 0, 4*sizeof(double));
925  memset(vv, 0, 3*sizeof(double));
926
927  double a0 = _sqrt_A * _sqrt_A;
928  if (a0 == 0) {
929    return failure;
930  }
931
932  double n0 = sqrt(gmWGS/(a0*a0*a0));
933
934  bncTime tt(GPSweek, GPSweeks);
935  double tk = tt - bncTime(_TOC.gpsw(), _TOEsec);
936
937  double n  = n0 + _Delta_n;
938  double M  = _M0 + n*tk;
939  double E  = M;
940  double E_last;
941  int    nLoop = 0;
942  do {
943    E_last = E;
944    E = M + _e*sin(E);
945
946    if (++nLoop == 100) {
947      return failure;
948    }
949  } while ( fabs(E-E_last)*a0 > 0.001 );
950  double v      = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) );
951  double u0     = v + _omega;
952  double sin2u0 = sin(2*u0);
953  double cos2u0 = cos(2*u0);
954  double r      = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
955  double i      = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
956  double u      = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
957  double xp     = r*cos(u);
958  double yp     = r*sin(u);
959  double OM     = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
960                  omegaEarth*_TOEsec;
961
962  double sinom = sin(OM);
963  double cosom = cos(OM);
964  double sini  = sin(i);
965  double cosi  = cos(i);
966  xc[0] = xp*cosom - yp*cosi*sinom;
967  xc[1] = xp*sinom + yp*cosi*cosom;
968  xc[2] = yp*sini;
969
970  double tc = tt - _TOC;
971  xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
972
973  xc[4] = _clock_bias;
974  xc[5] = _clock_drift;
975  xc[6] = _clock_driftrate;
976
977  // Velocity
978  // --------
979  double tanv2 = tan(v/2);
980  double dEdM  = 1 / (1 - _e*cos(E));
981  double dotv  = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
982               * dEdM * n;
983  double dotu  = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
984  double dotom = _OMEGADOT - omegaEarth;
985  double doti  = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
986  double dotr  = a0 * _e*sin(E) * dEdM * n
987                + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
988  double dotx  = dotr*cos(u) - r*sin(u)*dotu;
989  double doty  = dotr*sin(u) + r*cos(u)*dotu;
990
991  vv[0]  = cosom   *dotx  - cosi*sinom   *doty      // dX / dr
992           - xp*sinom*dotom - yp*cosi*cosom*dotom   // dX / dOMEGA
993                       + yp*sini*sinom*doti;        // dX / di
994
995  vv[1]  = sinom   *dotx  + cosi*cosom   *doty
996           + xp*cosom*dotom - yp*cosi*sinom*dotom
997                          - yp*sini*cosom*doti;
998
999  vv[2]  = sini    *doty  + yp*cosi      *doti;
1000
1001  // Relativistic Correction
1002  // -----------------------
1003  // correspondent to Galileo ICD and to SSR standard
1004  xc[3] -= 4.442807633e-10 * _e * sqrt(a0) *sin(E);
1005  // correspondent to IGS convention
1006  //xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c;
1007
1008  return success;
1009}
1010
1011// Health status of Galileo Ephemeris (virtual)
1012////////////////////////////////////////////////////////////////////////////
1013unsigned int t_ephGal::isUnhealthy() const {
1014  if (_E5aHS && _E5bHS && _E1_bHS) {
1015    return 1;
1016  }
1017  return 0;
1018}
1019
1020// RINEX Format String
1021//////////////////////////////////////////////////////////////////////////////
1022QString t_ephGal::toString(double version) const {
1023
1024  QString rnxStr = rinexDateStr(_TOC, _prn, version);
1025
1026  QTextStream out(&rnxStr);
1027
1028  out << QString("%1%2%3\n")
1029    .arg(_clock_bias,      19, 'e', 12)
1030    .arg(_clock_drift,     19, 'e', 12)
1031    .arg(_clock_driftrate, 19, 'e', 12);
1032
1033  QString fmt = version < 3.0 ? "   %1%2%3%4\n" : "    %1%2%3%4\n";
1034
1035  out << QString(fmt)
1036    .arg(_IODnav,  19, 'e', 12)
1037    .arg(_Crs,     19, 'e', 12)
1038    .arg(_Delta_n, 19, 'e', 12)
1039    .arg(_M0,      19, 'e', 12);
1040
1041  out << QString(fmt)
1042    .arg(_Cuc,    19, 'e', 12)
1043    .arg(_e,      19, 'e', 12)
1044    .arg(_Cus,    19, 'e', 12)
1045    .arg(_sqrt_A, 19, 'e', 12);
1046
1047  out << QString(fmt)
1048    .arg(_TOEsec, 19, 'e', 12)
1049    .arg(_Cic,    19, 'e', 12)
1050    .arg(_OMEGA0, 19, 'e', 12)
1051    .arg(_Cis,    19, 'e', 12);
1052
1053  out << QString(fmt)
1054    .arg(_i0,       19, 'e', 12)
1055    .arg(_Crc,      19, 'e', 12)
1056    .arg(_omega,    19, 'e', 12)
1057    .arg(_OMEGADOT, 19, 'e', 12);
1058
1059  int    dataSource = 0;
1060  int    SVhealth   = 0;
1061  double BGD_1_5A   = _BGD_1_5A;
1062  double BGD_1_5B   = _BGD_1_5B;
1063  if (_fnav) {
1064    dataSource |= (1<<1);
1065    dataSource |= (1<<8);
1066    BGD_1_5B = 0.0;
1067    // SVhealth
1068    //   Bit 3  : E5a DVS
1069    if (_e5aDataInValid) {
1070      SVhealth |= (1<<3);
1071    }
1072    //   Bit 4-5: E5a HS
1073    if (_E5aHS == 1.0) {
1074      SVhealth |= (1<<4);
1075    }
1076    else if (_E5aHS == 2.0) {
1077      SVhealth |= (1<<5);
1078    }
1079    else if (_E5aHS  == 3.0) {
1080      SVhealth |= (1<<4);
1081      SVhealth |= (1<<5);
1082    }
1083  }
1084  else if(_inav) {
1085    // Bit 2 and 0 are set because from MT1046 the data source cannot be determined
1086    // and RNXv3.03 says both can be set if the navigation messages were merged
1087    dataSource |= (1<<0);
1088    dataSource |= (1<<2);
1089    dataSource |= (1<<9);
1090    // SVhealth
1091    //   Bit 0  : E1-B DVS
1092    if (_e1DataInValid) {
1093      SVhealth |= (1<<0);
1094    }
1095    //   Bit 1-2: E1-B HS
1096    if      (_E1_bHS == 1.0) {
1097      SVhealth |= (1<<1);
1098    }
1099    else if (_E1_bHS == 2.0) {
1100      SVhealth |= (1<<2);
1101    }
1102    else if (_E1_bHS == 3.0) {
1103      SVhealth |= (1<<1);
1104      SVhealth |= (1<<2);
1105    }
1106    //   Bit 3  : E5a DVS
1107    if (_e5aDataInValid) {
1108      SVhealth |= (1<<3);
1109    }
1110    //   Bit 4-5: E5a HS
1111    if      (_E5aHS == 1.0) {
1112      SVhealth |= (1<<4);
1113    }
1114    else if (_E5aHS == 2.0) {
1115      SVhealth |= (1<<5);
1116    }
1117    else if (_E5aHS == 3.0) {
1118      SVhealth |= (1<<4);
1119      SVhealth |= (1<<5);
1120    }
1121    //   Bit 6  : E5b DVS
1122    if (_e5bDataInValid) {
1123      SVhealth |= (1<<6);
1124    }
1125    //   Bit 7-8: E5b HS
1126    if      (_E5bHS == 1.0) {
1127      SVhealth |= (1<<7);
1128    }
1129    else if (_E5bHS == 2.0) {
1130      SVhealth |= (1<<8);
1131    }
1132    else if (_E5bHS == 3.0) {
1133      SVhealth |= (1<<7);
1134      SVhealth |= (1<<8);
1135    }
1136  }
1137
1138  out << QString(fmt)
1139    .arg(_IDOT,              19, 'e', 12)
1140    .arg(double(dataSource), 19, 'e', 12)
1141    .arg(_TOEweek + 1024.0,  19, 'e', 12)
1142    .arg(0.0,                19, 'e', 12);
1143
1144  out << QString(fmt)
1145    .arg(_SISA,            19, 'e', 12)
1146    .arg(double(SVhealth), 19, 'e', 12)
1147    .arg(BGD_1_5A,         19, 'e', 12)
1148    .arg(BGD_1_5B,         19, 'e', 12);
1149
1150  double tot = _TOT;
1151  if (tot == 0.9999e9 && version < 3.0) {
1152    tot = 0.0;
1153  }
1154  out << QString(fmt)
1155    .arg(tot,     19, 'e', 12)
1156    .arg("",      19, QChar(' '))
1157    .arg("",      19, QChar(' '))
1158    .arg("",      19, QChar(' '));
1159
1160  return rnxStr;
1161}
1162
1163// Constructor
1164//////////////////////////////////////////////////////////////////////////////
1165t_ephSBAS::t_ephSBAS(float rnxVersion, const QStringList& lines) {
1166
1167  const int nLines = 4;
1168
1169  if (lines.size() != nLines) {
1170    _checkState = bad;
1171    return;
1172  }
1173
1174  // RINEX Format
1175  // ------------
1176  int fieldLen = 19;
1177
1178  int pos[4];
1179  pos[0] = (rnxVersion <= 2.12) ?  3 :  4;
1180  pos[1] = pos[0] + fieldLen;
1181  pos[2] = pos[1] + fieldLen;
1182  pos[3] = pos[2] + fieldLen;
1183
1184  // Read four lines
1185  // ---------------
1186  for (int iLine = 0; iLine < nLines; iLine++) {
1187    QString line = lines[iLine];
1188
1189    if      ( iLine == 0 ) {
1190      QTextStream in(line.left(pos[1]).toAscii());
1191
1192      int    year, month, day, hour, min;
1193      double sec;
1194
1195      QString prnStr, n;
1196      in >> prnStr;
1197      if (prnStr.size() == 1 && prnStr[0] == 'S') {
1198        in >> n;
1199        prnStr.append(n);
1200      }
1201      in >> year >> month >> day >> hour >> min >> sec;
1202      if (prnStr.at(0) == 'S') {
1203        _prn.set('S', prnStr.mid(1).toInt());
1204      }
1205      else {
1206        _prn.set('S', prnStr.toInt());
1207      }
1208
1209      if      (year <  80) {
1210        year += 2000;
1211      }
1212      else if (year < 100) {
1213        year += 1900;
1214      }
1215
1216      _TOC.set(year, month, day, hour, min, sec);
1217
1218      if ( readDbl(line, pos[1], fieldLen, _agf0 ) ||
1219           readDbl(line, pos[2], fieldLen, _agf1 ) ||
1220           readDbl(line, pos[3], fieldLen, _TOT  ) ) {
1221        _checkState = bad;
1222        return;
1223      }
1224    }
1225
1226    else if      ( iLine == 1 ) {
1227      if ( readDbl(line, pos[0], fieldLen, _x_pos         ) ||
1228           readDbl(line, pos[1], fieldLen, _x_velocity    ) ||
1229           readDbl(line, pos[2], fieldLen, _x_acceleration) ||
1230           readDbl(line, pos[3], fieldLen, _health        ) ) {
1231        _checkState = bad;
1232        return;
1233      }
1234    }
1235
1236    else if ( iLine == 2 ) {
1237      if ( readDbl(line, pos[0], fieldLen, _y_pos           ) ||
1238           readDbl(line, pos[1], fieldLen, _y_velocity      ) ||
1239           readDbl(line, pos[2], fieldLen, _y_acceleration  ) ||
1240           readDbl(line, pos[3], fieldLen, _ura             ) ) {
1241        _checkState = bad;
1242        return;
1243      }
1244    }
1245
1246    else if ( iLine == 3 ) {
1247      double iodn;
1248      if ( readDbl(line, pos[0], fieldLen, _z_pos         )  ||
1249           readDbl(line, pos[1], fieldLen, _z_velocity    )  ||
1250           readDbl(line, pos[2], fieldLen, _z_acceleration)  ||
1251           readDbl(line, pos[3], fieldLen, iodn           ) ) {
1252        _checkState = bad;
1253        return;
1254      } else {
1255        _IODN = int(iodn);
1256      }
1257    }
1258  }
1259
1260  _x_pos          *= 1.e3;
1261  _y_pos          *= 1.e3;
1262  _z_pos          *= 1.e3;
1263  _x_velocity     *= 1.e3;
1264  _y_velocity     *= 1.e3;
1265  _z_velocity     *= 1.e3;
1266  _x_acceleration *= 1.e3;
1267  _y_acceleration *= 1.e3;
1268  _z_acceleration *= 1.e3;
1269}
1270
1271// IOD of SBAS Ephemeris (virtual)
1272////////////////////////////////////////////////////////////////////////////
1273
1274unsigned int t_ephSBAS::IOD() const {
1275  unsigned char buffer[80];
1276  int size = 0;
1277  int numbits = 0;
1278  long long bitbuffer = 0;
1279  unsigned char *startbuffer = buffer;
1280
1281  SBASADDBITSFLOAT(30, this->_x_pos, 0.08)
1282  SBASADDBITSFLOAT(30, this->_y_pos, 0.08)
1283  SBASADDBITSFLOAT(25, this->_z_pos, 0.4)
1284  SBASADDBITSFLOAT(17, this->_x_velocity, 0.000625)
1285  SBASADDBITSFLOAT(17, this->_y_velocity, 0.000625)
1286  SBASADDBITSFLOAT(18, this->_z_velocity, 0.004)
1287  SBASADDBITSFLOAT(10, this->_x_acceleration, 0.0000125)
1288  SBASADDBITSFLOAT(10, this->_y_acceleration, 0.0000125)
1289  SBASADDBITSFLOAT(10, this->_z_acceleration, 0.0000625)
1290  SBASADDBITSFLOAT(12, this->_agf0, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
1291  SBASADDBITSFLOAT(8, this->_agf1, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<10))
1292  SBASADDBITS(5,0); // the last byte is filled by 0-bits to obtain a length of an integer multiple of 8
1293
1294  return CRC24(size, startbuffer);
1295}
1296
1297// Compute SBAS Satellite Position (virtual)
1298////////////////////////////////////////////////////////////////////////////
1299t_irc t_ephSBAS::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
1300
1301  if (_checkState == bad ||
1302      _checkState == unhealthy) {
1303    return failure;
1304  }
1305
1306  bncTime tt(GPSweek, GPSweeks);
1307  double  dt = tt - _TOC;
1308
1309  xc[0] = _x_pos + _x_velocity * dt + _x_acceleration * dt * dt / 2.0;
1310  xc[1] = _y_pos + _y_velocity * dt + _y_acceleration * dt * dt / 2.0;
1311  xc[2] = _z_pos + _z_velocity * dt + _z_acceleration * dt * dt / 2.0;
1312
1313  vv[0] = _x_velocity + _x_acceleration * dt;
1314  vv[1] = _y_velocity + _y_acceleration * dt;
1315  vv[2] = _z_velocity + _z_acceleration * dt;
1316
1317  xc[3] = _agf0 + _agf1 * dt;
1318
1319  xc[4] = _agf0;
1320  xc[5] = _agf1;
1321  xc[6] = 0.0;
1322
1323  return success;
1324}
1325
1326// RINEX Format String
1327//////////////////////////////////////////////////////////////////////////////
1328QString t_ephSBAS::toString(double version) const {
1329
1330  QString rnxStr = rinexDateStr(_TOC, _prn, version);
1331
1332  QTextStream out(&rnxStr);
1333
1334  out << QString("%1%2%3\n")
1335    .arg(_agf0, 19, 'e', 12)
1336    .arg(_agf1, 19, 'e', 12)
1337    .arg(_TOT,  19, 'e', 12);
1338
1339  QString fmt = version < 3.0 ? "   %1%2%3%4\n" : "    %1%2%3%4\n";
1340
1341  out << QString(fmt)
1342    .arg(1.e-3*_x_pos,          19, 'e', 12)
1343    .arg(1.e-3*_x_velocity,     19, 'e', 12)
1344    .arg(1.e-3*_x_acceleration, 19, 'e', 12)
1345    .arg(_health,               19, 'e', 12);
1346
1347  out << QString(fmt)
1348    .arg(1.e-3*_y_pos,          19, 'e', 12)
1349    .arg(1.e-3*_y_velocity,     19, 'e', 12)
1350    .arg(1.e-3*_y_acceleration, 19, 'e', 12)
1351    .arg(_ura,                  19, 'e', 12);
1352
1353  out << QString(fmt)
1354    .arg(1.e-3*_z_pos,          19, 'e', 12)
1355    .arg(1.e-3*_z_velocity,     19, 'e', 12)
1356    .arg(1.e-3*_z_acceleration, 19, 'e', 12)
1357    .arg(double(_IODN),         19, 'e', 12);
1358
1359  return rnxStr;
1360}
1361
1362// Constructor
1363//////////////////////////////////////////////////////////////////////////////
1364t_ephBDS::t_ephBDS(float rnxVersion, const QStringList& lines) {
1365
1366  const int nLines = 8;
1367
1368  if (lines.size() != nLines) {
1369    _checkState = bad;
1370    return;
1371  }
1372
1373  // RINEX Format
1374  // ------------
1375  int fieldLen = 19;
1376
1377  int pos[4];
1378  pos[0] = (rnxVersion <= 2.12) ?  3 :  4;
1379  pos[1] = pos[0] + fieldLen;
1380  pos[2] = pos[1] + fieldLen;
1381  pos[3] = pos[2] + fieldLen;
1382
1383  // Read eight lines
1384  // ----------------
1385  for (int iLine = 0; iLine < nLines; iLine++) {
1386    QString line = lines[iLine];
1387
1388    if      ( iLine == 0 ) {
1389      QTextStream in(line.left(pos[1]).toAscii());
1390
1391      int    year, month, day, hour, min;
1392      double sec;
1393
1394      QString prnStr, n;
1395      in >> prnStr;
1396      if (prnStr.size() == 1 && prnStr[0] == 'C') {
1397        in >> n;
1398        prnStr.append(n);
1399      }
1400      in >> year >> month >> day >> hour >> min >> sec;
1401      if (prnStr.at(0) == 'C') {
1402        _prn.set('C', prnStr.mid(1).toInt());
1403      }
1404      else {
1405        _prn.set('C', prnStr.toInt());
1406      }
1407
1408      if      (year <  80) {
1409        year += 2000;
1410      }
1411      else if (year < 100) {
1412        year += 1900;
1413      }
1414
1415      _TOC.setBDS(year, month, day, hour, min, sec);
1416
1417      if ( readDbl(line, pos[1], fieldLen, _clock_bias     ) ||
1418           readDbl(line, pos[2], fieldLen, _clock_drift    ) ||
1419           readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
1420        _checkState = bad;
1421        return;
1422      }
1423    }
1424
1425    else if      ( iLine == 1 ) {
1426      double aode;
1427      if ( readDbl(line, pos[0], fieldLen, aode    ) ||
1428           readDbl(line, pos[1], fieldLen, _Crs    ) ||
1429           readDbl(line, pos[2], fieldLen, _Delta_n) ||
1430           readDbl(line, pos[3], fieldLen, _M0     ) ) {
1431        _checkState = bad;
1432        return;
1433      }
1434      _AODE = int(aode);
1435    }
1436
1437    else if ( iLine == 2 ) {
1438      if ( readDbl(line, pos[0], fieldLen, _Cuc   ) ||
1439           readDbl(line, pos[1], fieldLen, _e     ) ||
1440           readDbl(line, pos[2], fieldLen, _Cus   ) ||
1441           readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
1442        _checkState = bad;
1443        return;
1444      }
1445    }
1446
1447    else if ( iLine == 3 ) {
1448      if ( readDbl(line, pos[0], fieldLen, _TOEsec )  ||
1449           readDbl(line, pos[1], fieldLen, _Cic   )  ||
1450           readDbl(line, pos[2], fieldLen, _OMEGA0)  ||
1451           readDbl(line, pos[3], fieldLen, _Cis   ) ) {
1452        _checkState = bad;
1453        return;
1454      }
1455    }
1456
1457    else if ( iLine == 4 ) {
1458      if ( readDbl(line, pos[0], fieldLen, _i0      ) ||
1459           readDbl(line, pos[1], fieldLen, _Crc     ) ||
1460           readDbl(line, pos[2], fieldLen, _omega   ) ||
1461           readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
1462        _checkState = bad;
1463        return;
1464      }
1465    }
1466
1467    else if ( iLine == 5 ) {
1468      if ( readDbl(line, pos[0], fieldLen, _IDOT    ) ||
1469           readDbl(line, pos[2], fieldLen, _TOEweek)) {
1470        _checkState = bad;
1471        return;
1472      }
1473    }
1474
1475    else if ( iLine == 6 ) {
1476      double SatH1;
1477      if ( readDbl(line, pos[0], fieldLen, _URA ) ||
1478           readDbl(line, pos[1], fieldLen, SatH1) ||
1479           readDbl(line, pos[2], fieldLen, _TGD1) ||
1480           readDbl(line, pos[3], fieldLen, _TGD2) ) {
1481        _checkState = bad;
1482        return;
1483      }
1484      _SatH1 = int(SatH1);
1485    }
1486
1487    else if ( iLine == 7 ) {
1488      double aodc;
1489      if ( readDbl(line, pos[0], fieldLen, _TOT) ||
1490           readDbl(line, pos[1], fieldLen, aodc) ) {
1491        _checkState = bad;
1492        return;
1493      }
1494      if (_TOT == 0.9999e9) {  // 0.9999e9 means not known (RINEX standard)
1495        _TOT = _TOEsec;
1496      }
1497      _AODC = int(aodc);
1498    }
1499  }
1500
1501  _TOE.setBDS(int(_TOEweek), _TOEsec);
1502
1503  // remark: actually should be computed from second_tot
1504  //         but it seems to be unreliable in RINEX files
1505  //_TOT = _TOC.bdssec();
1506}
1507
1508// IOD of BDS Ephemeris (virtual)
1509////////////////////////////////////////////////////////////////////////////
1510unsigned int t_ephBDS::IOD() const {
1511  unsigned char buffer[80];
1512  int size = 0;
1513  int numbits = 0;
1514  long long bitbuffer = 0;
1515  unsigned char *startbuffer = buffer;
1516
1517  BDSADDBITSFLOAT(14, this->_IDOT, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
1518  BDSADDBITSFLOAT(11, this->_clock_driftrate, 1.0/static_cast<double>(1<<30)
1519      /static_cast<double>(1<<30)/static_cast<double>(1<<6))
1520  BDSADDBITSFLOAT(22, this->_clock_drift, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<20))
1521  BDSADDBITSFLOAT(24, this->_clock_bias, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
1522  BDSADDBITSFLOAT(18, this->_Crs, 1.0/static_cast<double>(1<<6))
1523  BDSADDBITSFLOAT(16, this->_Delta_n, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
1524  BDSADDBITSFLOAT(32, this->_M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
1525  BDSADDBITSFLOAT(18, this->_Cuc, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
1526  BDSADDBITSFLOAT(32, this->_e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
1527  BDSADDBITSFLOAT(18, this->_Cus, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
1528  BDSADDBITSFLOAT(32, this->_sqrt_A, 1.0/static_cast<double>(1<<19))
1529  BDSADDBITSFLOAT(18, this->_Cic, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
1530  BDSADDBITSFLOAT(32, this->_OMEGA0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
1531  BDSADDBITSFLOAT(18, this->_Cis, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
1532  BDSADDBITSFLOAT(32, this->_i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
1533  BDSADDBITSFLOAT(18, this->_Crc, 1.0/static_cast<double>(1<<6))
1534  BDSADDBITSFLOAT(32, this->_omega, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
1535  BDSADDBITSFLOAT(24, this->_OMEGADOT, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
1536  BDSADDBITS(5, 0)  // the last byte is filled by 0-bits to obtain a length of an integer multiple of 8
1537
1538  return CRC24(size, startbuffer);
1539}
1540
1541// Compute BDS Satellite Position (virtual)
1542//////////////////////////////////////////////////////////////////////////////
1543t_irc t_ephBDS::position(int GPSweek, double GPSweeks, double* xc, double* vv) const {
1544
1545  if (_checkState == bad ||
1546      _checkState == unhealthy) {
1547    return failure;
1548  }
1549
1550  static const double gmBDS    = 398.6004418e12;
1551  static const double omegaBDS = 7292115.0000e-11;
1552
1553  xc[0] = xc[1] = xc[2] = xc[3] = 0.0;
1554  vv[0] = vv[1] = vv[2] = 0.0;
1555
1556  bncTime tt(GPSweek, GPSweeks);
1557
1558  if (_sqrt_A == 0) {
1559    return failure;
1560  }
1561  double a0 = _sqrt_A * _sqrt_A;
1562
1563  double n0 = sqrt(gmBDS/(a0*a0*a0));
1564  double tk = tt - _TOE;
1565  double n  = n0 + _Delta_n;
1566  double M  = _M0 + n*tk;
1567  double E  = M;
1568  double E_last;
1569  int    nLoop = 0;
1570  do {
1571    E_last = E;
1572    E = M + _e*sin(E);
1573
1574    if (++nLoop == 100) {
1575      return failure;
1576    }
1577  } while ( fabs(E-E_last)*a0 > 0.001 );
1578
1579  double v      = atan2(sqrt(1-_e*_e) * sin(E), cos(E) - _e);
1580  double u0     = v + _omega;
1581  double sin2u0 = sin(2*u0);
1582  double cos2u0 = cos(2*u0);
1583  double r      = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
1584  double i      = _i0 + _IDOT*tk     + _Cic*cos2u0 + _Cis*sin2u0;
1585  double u      = u0                 + _Cuc*cos2u0 + _Cus*sin2u0;
1586  double xp     = r*cos(u);
1587  double yp     = r*sin(u);
1588  double toesec = (_TOE.gpssec() - 14.0);
1589  double sinom = 0;
1590  double cosom = 0;
1591  double sini  = 0;
1592  double cosi  = 0;
1593
1594  // Velocity
1595  // --------
1596  double tanv2 = tan(v/2);
1597  double dEdM  = 1 / (1 - _e*cos(E));
1598  double dotv  = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2)
1599                 / (1 + tanv2*tanv2) * dEdM * n;
1600  double dotu  = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
1601  double doti  = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
1602  double dotr  = a0 * _e*sin(E) * dEdM * n
1603                + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
1604
1605  double dotx  = dotr*cos(u) - r*sin(u)*dotu;
1606  double doty  = dotr*sin(u) + r*cos(u)*dotu;
1607
1608  const double iMaxGEO = 10.0 / 180.0 * M_PI;
1609
1610  // MEO/IGSO satellite
1611  // ------------------
1612  if (_i0 > iMaxGEO) {
1613    double OM = _OMEGA0 + (_OMEGADOT - omegaBDS)*tk - omegaBDS*toesec;
1614
1615    sinom = sin(OM);
1616    cosom = cos(OM);
1617    sini  = sin(i);
1618    cosi  = cos(i);
1619
1620    xc[0] = xp*cosom - yp*cosi*sinom;
1621    xc[1] = xp*sinom + yp*cosi*cosom;
1622    xc[2] = yp*sini;
1623
1624    // Velocity
1625    // --------
1626
1627    double dotom = _OMEGADOT - t_CST::omega;
1628
1629    vv[0]  = cosom  *dotx   - cosi*sinom   *doty    // dX / dr
1630           - xp*sinom*dotom - yp*cosi*cosom*dotom   // dX / dOMEGA
1631                            + yp*sini*sinom*doti;   // dX / di
1632
1633    vv[1]  = sinom  *dotx   + cosi*cosom   *doty
1634           + xp*cosom*dotom - yp*cosi*sinom*dotom
1635                            - yp*sini*cosom*doti;
1636
1637    vv[2]  = sini   *doty   + yp*cosi      *doti;
1638
1639  }
1640
1641  // GEO satellite
1642  // -------------
1643  else {
1644    double OM    = _OMEGA0 + _OMEGADOT*tk - omegaBDS*toesec;
1645    double ll    = omegaBDS*tk;
1646
1647    sinom = sin(OM);
1648    cosom = cos(OM);
1649    sini  = sin(i);
1650    cosi  = cos(i);
1651
1652    double xx = xp*cosom - yp*cosi*sinom;
1653    double yy = xp*sinom + yp*cosi*cosom;
1654    double zz = yp*sini;
1655
1656    Matrix RX = BNC_PPP::t_astro::rotX(-5.0 / 180.0 * M_PI);
1657    Matrix RZ = BNC_PPP::t_astro::rotZ(ll);
1658
1659    ColumnVector X1(3); X1 << xx << yy << zz;
1660    ColumnVector X2 = RZ*RX*X1;
1661
1662    xc[0] = X2(1);
1663    xc[1] = X2(2);
1664    xc[2] = X2(3);
1665
1666    double dotom = _OMEGADOT;
1667
1668    double vx  = cosom   *dotx  - cosi*sinom   *doty
1669               - xp*sinom*dotom - yp*cosi*cosom*dotom
1670                                + yp*sini*sinom*doti;
1671
1672    double vy  = sinom   *dotx  + cosi*cosom   *doty
1673               + xp*cosom*dotom - yp*cosi*sinom*dotom
1674                                - yp*sini*cosom*doti;
1675
1676    double vz  = sini    *doty  + yp*cosi      *doti;
1677
1678    ColumnVector V(3); V << vx << vy << vz;
1679
1680    Matrix RdotZ(3,3);
1681    double C = cos(ll);
1682    double S = sin(ll);
1683    Matrix UU(3,3);
1684    UU[0][0] =  -S;  UU[0][1] =  +C;  UU[0][2] = 0.0;
1685    UU[1][0] =  -C;  UU[1][1] =  -S;  UU[1][2] = 0.0;
1686    UU[2][0] = 0.0;  UU[2][1] = 0.0;  UU[2][2] = 0.0;
1687    RdotZ = omegaBDS * UU;
1688
1689    ColumnVector VV(3);
1690    VV = RZ*RX*V + RdotZ*RX*X1;
1691
1692    vv[0] = VV(1);
1693    vv[1] = VV(2);
1694    vv[2] = VV(3);
1695  }
1696
1697  double tc = tt - _TOC;
1698  xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
1699
1700  xc[4] = _clock_bias;
1701  xc[5] = _clock_drift;
1702  xc[6] = _clock_driftrate;
1703
1704  // dotC  = _clock_drift + _clock_driftrate*tc
1705  //       - 4.442807633e-10*_e*sqrt(a0)*cos(E) * dEdM * n;
1706
1707  // Relativistic Correction
1708  // -----------------------
1709  // correspondent to BDS ICD and to SSR standard
1710    xc[3] -= 4.442807633e-10 * _e * sqrt(a0) *sin(E);
1711  // correspondent to IGS convention
1712  // xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c;
1713
1714  return success;
1715}
1716
1717// RINEX Format String
1718//////////////////////////////////////////////////////////////////////////////
1719QString t_ephBDS::toString(double version) const {
1720
1721  QString rnxStr = rinexDateStr(_TOC-14.0, _prn, version);
1722
1723  QTextStream out(&rnxStr);
1724
1725  out << QString("%1%2%3\n")
1726    .arg(_clock_bias,      19, 'e', 12)
1727    .arg(_clock_drift,     19, 'e', 12)
1728    .arg(_clock_driftrate, 19, 'e', 12);
1729
1730  QString fmt = version < 3.0 ? "   %1%2%3%4\n" : "    %1%2%3%4\n";
1731
1732  out << QString(fmt)
1733    .arg(double(_AODE), 19, 'e', 12)
1734    .arg(_Crs,          19, 'e', 12)
1735    .arg(_Delta_n,      19, 'e', 12)
1736    .arg(_M0,           19, 'e', 12);
1737
1738  out << QString(fmt)
1739    .arg(_Cuc,    19, 'e', 12)
1740    .arg(_e,      19, 'e', 12)
1741    .arg(_Cus,    19, 'e', 12)
1742    .arg(_sqrt_A, 19, 'e', 12);
1743
1744  double toes = 0.0;
1745  if (_TOEweek > -1.0) {// RINEX input
1746    toes = _TOEsec;
1747  }
1748  else {// RTCM stream input
1749    toes = _TOE.bdssec();
1750  }
1751  out << QString(fmt)
1752    .arg(toes,    19, 'e', 12)
1753    .arg(_Cic,    19, 'e', 12)
1754    .arg(_OMEGA0, 19, 'e', 12)
1755    .arg(_Cis,    19, 'e', 12);
1756
1757  out << QString(fmt)
1758    .arg(_i0,       19, 'e', 12)
1759    .arg(_Crc,      19, 'e', 12)
1760    .arg(_omega,    19, 'e', 12)
1761    .arg(_OMEGADOT, 19, 'e', 12);
1762
1763  double toew = 0.0;
1764  if (_TOEweek > -1.0) {// RINEX input
1765    toew = _TOEweek;
1766  }
1767  else {// RTCM stream input
1768    toew = double(_TOE.bdsw());
1769  }
1770  out << QString(fmt)
1771    .arg(_IDOT, 19, 'e', 12)
1772    .arg(0.0,   19, 'e', 12)
1773    .arg(toew,  19, 'e', 12)
1774    .arg(0.0,   19, 'e', 12);
1775
1776  out << QString(fmt)
1777    .arg(_URA,           19, 'e', 12)
1778    .arg(double(_SatH1), 19, 'e', 12)
1779    .arg(_TGD1,          19, 'e', 12)
1780    .arg(_TGD2,          19, 'e', 12);
1781
1782  double tots = 0.0;
1783  if (_TOEweek > -1.0) {// RINEX input
1784    tots = _TOT;
1785  }
1786  else {// RTCM stream input
1787    tots = _TOE.bdssec();
1788  }
1789  out << QString(fmt)
1790    .arg(tots,          19, 'e', 12)
1791    .arg(double(_AODC), 19, 'e', 12)
1792    .arg("",            19, QChar(' '))
1793    .arg("",            19, QChar(' '));
1794  return rnxStr;
1795}
Note: See TracBrowser for help on using the repository browser.