Changeset 4018 in ntrip for trunk/BNC


Ignore:
Timestamp:
Apr 22, 2012, 6:50:49 PM (13 years ago)
Author:
mervart
Message:
 
Location:
trunk/BNC
Files:
4 edited

Legend:

Unmodified
Added
Removed

  • trunk/BNC/RTCM3/ephemeris.cpp

    r4017 r4018  
    1515using namespace std;
    1616
    17 #define PI          3.1415926535898
    18 // Returns nearest integer value
    19 ////////////////////////////////////////////////////////////////////////////
    20 static double NearestInt(double fl, double * remain)
    21 {
    22   bool isneg = fl < 0.0;
    23   double intval;
    24   if(isneg) fl *= -1.0;
    25   intval = (double)((unsigned long)(fl+0.5));
    26   if(isneg) {fl *= -1.0; intval *= -1.0;}
    27   if(remain)
    28     *remain = fl-intval;
    29   return intval;
    30 } /* NearestInt() */
    31 
    3217// Returns CRC24
    3318////////////////////////////////////////////////////////////////////////////
    34 static unsigned long CRC24(long size, const unsigned char *buf)
    35 {
     19static unsigned long CRC24(long size, const unsigned char *buf) {
    3620  unsigned long crc = 0;
    37   int i;
    38 
    39   while(size--)
    40   {
     21  int ii;
     22  while (size--) {
    4123    crc ^= (*buf++) << (16);
    42     for(i = 0; i < 8; i++)
    43     {
     24    for(ii = 0; ii < 8; ii++) {
    4425      crc <<= 1;
    45       if(crc & 0x1000000)
     26      if (crc & 0x1000000) {
    4627        crc ^= 0x01864cfb;
     28      }
    4729    }
    4830  }
    4931  return crc;
    50 } /* CRC24 */
    51 
    52 //
    53 ////////////////////////////////////////////////////////////////////////////
    54 bool t_eph::isNewerThan(const t_eph* eph) const {
    55   if (_GPSweek >  eph->_GPSweek ||
    56       (_GPSweek == eph->_GPSweek && _GPSweeks > eph->_GPSweeks)) {
    57     return true;
    58   }
    59   else {
    60     return false;
    61   }
    6232}
    6333
    … …  
    6838  _prn = QString("G%1").arg(ee->satellite, 2, 10, QChar('0'));
    6939
    70   // TODO: check if following two lines are correct
    71   _GPSweek  = ee->GPSweek;
    72   _GPSweeks = ee->TOE;
    73 
    74   _TOW  = ee->TOW;
    75   _TOC  = ee->TOC;
    76   _TOE  = ee->TOE;
    77   _IODE = ee->IODE;
    78   _IODC = ee->IODC;
    79 
     40  _TOC.set(ee->GPSweek, ee->TOC);
    8041  _clock_bias      = ee->clock_bias;
    8142  _clock_drift     = ee->clock_drift;
    8243  _clock_driftrate = ee->clock_driftrate;
    8344
     45  _IODE     = ee->IODE;
    8446  _Crs      = ee->Crs;
    8547  _Delta_n  = ee->Delta_n;
    8648  _M0       = ee->M0;
     49
    8750  _Cuc      = ee->Cuc;
    8851  _e        = ee->e;
    8952  _Cus      = ee->Cus;
    9053  _sqrt_A   = ee->sqrt_A;
     54
     55  _TOEsec   = ee->TOE;
    9156  _Cic      = ee->Cic;
    9257  _OMEGA0   = ee->OMEGA0;
    9358  _Cis      = ee->Cis;
     59
    9460  _i0       = ee->i0;
    9561  _Crc      = ee->Crc;
    9662  _omega    = ee->omega;
    9763  _OMEGADOT = ee->OMEGADOT;
     64
    9865  _IDOT     = ee->IDOT;
    99 
     66  _L2Codes  = 0.0;
     67  _TOEweek  = ee->TOW;
     68  _L2PFlag  = 0.0;
     69
     70  _ura      = 0.0;
     71  _health   = ee->SVhealth;
    10072  _TGD      = ee->TGD;
     73  _IODC     = ee->IODC;
     74
     75  _TOT         = 0.9999e9;
     76  _fitInterval = 0.0;
    10177
    10278  _ok       = true;
    103 
    104   /* FIXME: convert URAindex and flags! */
    105   _ura = 0;
    106   _L2Codes = 0;
    107   _L2PFlag = 0;
    108   _health = ee->SVhealth;
    10979}
    11080
    … …  
    11282////////////////////////////////////////////////////////////////////////////
    11383void t_ephGPS::position(int GPSweek, double GPSweeks,
    114                         double* xc,
    115                         double* vv) const {
    116 
    117   static const double secPerWeek = 7 * 86400.0;
     84                        double* xc,
     85                        double* vv) const {
     86
     87
    11888  static const double omegaEarth = 7292115.1467e-11;
    11989  static const double gmWGS      = 398.6005e12;
    … …  
    12898
    12999  double n0 = sqrt(gmWGS/(a0*a0*a0));
    130   double tk = GPSweeks - _TOE;
    131   if (GPSweek != _GPSweek) { 
    132     tk += (GPSweek - _GPSweek) * secPerWeek;
    133   }
     100
     101  bncTime tt(GPSweek, GPSweeks);
     102  double tk = tt - bncTime(_TOEweek, _TOEsec);
     103
    134104  double n  = n0 + _Delta_n;
    135105  double M  = _M0 + n*tk;
    … …  
    150120  double yp     = r*sin(u);
    151121  double OM     = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
    152                    omegaEarth*_TOE;
     122                   omegaEarth*_TOEsec;
    153123 
    154124  double sinom = sin(OM);
    … …  
    160130  xc[2] = yp*sini;                 
    161131 
    162   double tc = GPSweeks - _TOC;
    163   if (GPSweek != _GPSweek) { 
    164     tc += (GPSweek - _GPSweek) * secPerWeek;
    165   }
     132  double tc = tt - _TOC;
    166133  xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
    167134
    … …  
    192159  // Relativistic Correction
    193160  // -----------------------
    194   //  xc(4) -= 4.442807633e-10 * _e * sqrt(a0) *sin(E);
    195161  xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c;
    196162}
    … …  
    198164// build up RTCM3 for GPS
    199165////////////////////////////////////////////////////////////////////////////
    200 #define GPSTOINT(type, value) static_cast<type>(NearestInt(value,0))
     166#define GPSTOINT(type, value) static_cast<type>(round(value))
    201167
    202168#define GPSADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
    … …  
    205171                       while(numbits >= 8) { \
    206172                       buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
     173
    207174#define GPSADDBITSFLOAT(a,b,c) {long long i = GPSTOINT(long long,(b)/(c)); \
    208175                             GPSADDBITS(a,i)};
    209176
    210 int t_ephGPS::RTCM3(unsigned char *buffer)
    211 {
     177int t_ephGPS::RTCM3(unsigned char *buffer) {
    212178
    213179  unsigned char *startbuffer = buffer;
    … …  
    267233  GPSADDBITS(12, 1019)
    268234  GPSADDBITS(6,_prn.right((_prn.length()-1)).toInt())
    269   GPSADDBITS(10, _GPSweek)
     235  GPSADDBITS(10, _TOC.gpsw())
    270236  GPSADDBITS(4, _ura)
    271237  GPSADDBITS(2,_L2Codes)
    272   GPSADDBITSFLOAT(14, _IDOT, PI/static_cast<double>(1<<30)
     238  GPSADDBITSFLOAT(14, _IDOT, M_PI/static_cast<double>(1<<30)
    273239  /static_cast<double>(1<<13))
    274240  GPSADDBITS(8, _IODE)
    275   GPSADDBITS(16, static_cast<int>(_TOC)>>4)
     241  GPSADDBITS(16, static_cast<int>(_TOC.gpssec())>>4)
    276242  GPSADDBITSFLOAT(8, _clock_driftrate, 1.0/static_cast<double>(1<<30)
    277243  /static_cast<double>(1<<25))
    … …  
    282248  GPSADDBITS(10, _IODC)
    283249  GPSADDBITSFLOAT(16, _Crs, 1.0/static_cast<double>(1<<5))
    284   GPSADDBITSFLOAT(16, _Delta_n, PI/static_cast<double>(1<<30)
     250  GPSADDBITSFLOAT(16, _Delta_n, M_PI/static_cast<double>(1<<30)
    285251  /static_cast<double>(1<<13))
    286   GPSADDBITSFLOAT(32, _M0, PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
     252  GPSADDBITSFLOAT(32, _M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
    287253  GPSADDBITSFLOAT(16, _Cuc, 1.0/static_cast<double>(1<<29))
    288254  GPSADDBITSFLOAT(32, _e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
    289255  GPSADDBITSFLOAT(16, _Cus, 1.0/static_cast<double>(1<<29))
    290256  GPSADDBITSFLOAT(32, _sqrt_A, 1.0/static_cast<double>(1<<19))
    291   GPSADDBITS(16, static_cast<int>(_TOE)>>4)
     257  GPSADDBITS(16, static_cast<int>(_TOEsec)>>4)
    292258  GPSADDBITSFLOAT(16, _Cic, 1.0/static_cast<double>(1<<29))
    293   GPSADDBITSFLOAT(32, _OMEGA0, PI/static_cast<double>(1<<30)
     259  GPSADDBITSFLOAT(32, _OMEGA0, M_PI/static_cast<double>(1<<30)
    294260  /static_cast<double>(1<<1))
    295261  GPSADDBITSFLOAT(16, _Cis, 1.0/static_cast<double>(1<<29))
    296   GPSADDBITSFLOAT(32, _i0, PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
     262  GPSADDBITSFLOAT(32, _i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
    297263  GPSADDBITSFLOAT(16, _Crc, 1.0/static_cast<double>(1<<5))
    298   GPSADDBITSFLOAT(32, _omega, PI/static_cast<double>(1<<30)
     264  GPSADDBITSFLOAT(32, _omega, M_PI/static_cast<double>(1<<30)
    299265  /static_cast<double>(1<<1))
    300   GPSADDBITSFLOAT(24, _OMEGADOT, PI/static_cast<double>(1<<30)
     266  GPSADDBITSFLOAT(24, _OMEGADOT, M_PI/static_cast<double>(1<<30)
    301267  /static_cast<double>(1<<13))
    302268  GPSADDBITSFLOAT(8, _TGD, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
    … …  
    358324                        double* xc, double* vv) const {
    359325
    360   static const double secPerWeek  = 7 * 86400.0;
    361326  static const double nominalStep = 10.0;
    362327
    … …  
    364329  memset(vv, 0, 3*sizeof(double));
    365330
    366   double dtPos = GPSweeks - _tt;
    367   if (GPSweek != _GPSweek) { 
    368     dtPos += (GPSweek - _GPSweek) * secPerWeek;
    369   }
     331  double dtPos = bncTime(GPSweek, GPSweeks) - _tt;
    370332
    371333  int nSteps  = int(fabs(dtPos) / nominalStep) + 1;
    … …  
    377339  acc[2] = _z_acceleration * 1.e3;
    378340  for (int ii = 1; ii <= nSteps; ii++) {
    379     _xv = rungeKutta4(_tt, _xv, step, acc, glo_deriv);
    380     _tt += step;
     341    _xv = rungeKutta4(_tt.gpssec(), _xv, step, acc, glo_deriv);
     342    _tt = _tt + step;
    381343  }
    382344
    … …  
    393355  // Clock Correction
    394356  // ----------------
    395   double dtClk = GPSweeks - _GPSweeks;
    396   if (GPSweek != _GPSweek) { 
    397     dtClk += (GPSweek - _GPSweek) * secPerWeek;
    398   }
     357  double dtClk = bncTime(GPSweek, GPSweeks) - _TOC;
    399358  xc[3] = -_tau + _gamma * dtClk;
    400359}
    … …  
    403362////////////////////////////////////////////////////////////////////////////
    404363int t_ephGlo::IOD() const {
    405   bncTime tGPS(_GPSweek, _GPSweeks);
    406   bncTime tMoscow = tGPS - _gps_utc + 3 * 3600.0;
     364  bncTime tMoscow = _TOC - _gps_utc + 3 * 3600.0;
    407365  return int(tMoscow.daysec() / 900);
    408366}
    … …  
    472430  _gps_utc = gnumleap(year, month, day);
    473431
    474   _GPSweek           = ww;
    475   _GPSweeks          = tow;
     432  _TOC.set(ww, tow);
    476433  _E                 = ee->E;
    477434  _tau               = ee->tau;
    … …  
    492449  // Initialize status vector
    493450  // ------------------------
    494   _tt = _GPSweeks;
     451  _tt = _TOC;
    495452
    496453  _xv(1) = _x_pos * 1.e3;
    … …  
    506463// build up RTCM3 for GLONASS
    507464////////////////////////////////////////////////////////////////////////////
    508 #define GLONASSTOINT(type, value) static_cast<type>(NearestInt(value,0))
     465#define GLONASSTOINT(type, value) static_cast<type>(round(value))
    509466
    510467#define GLONASSADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
    … …  
    549506  GLONASSADDBITS(1, _health)
    550507  GLONASSADDBITS(1, 0)
    551   unsigned long long timeofday = (static_cast<int>(_tt+3*60*60-_gps_utc)%86400);
     508  unsigned long long timeofday = (static_cast<int>(_tt.gpssec()+3*60*60-_gps_utc)%86400);
    552509  GLONASSADDBITS(7, timeofday/60/15)
    553510  GLONASSADDBITSFLOATM(24, _x_velocity*1000, 1000.0/static_cast<double>(1<<20))
    … …  
    597554  _prn = QString("E%1").arg(ee->satellite, 2, 10, QChar('0'));
    598555
    599   _GPSweek  = ee->Week;
    600   _GPSweeks = ee->TOE;
    601 
    602   _TOC    = ee->TOC;
    603   _TOE    = ee->TOE;
    604   _IODnav = ee->IODnav;
    605 
     556  _TOC.set(ee->Week, ee->TOC);
    606557  _clock_bias      = ee->clock_bias;
    607558  _clock_drift     = ee->clock_drift;
    608559  _clock_driftrate = ee->clock_driftrate;
    609560
     561  _IODnav   = ee->IODnav;
    610562  _Crs      = ee->Crs;
    611563  _Delta_n  = ee->Delta_n;
    612564  _M0       = ee->M0;
     565
    613566  _Cuc      = ee->Cuc;
    614567  _e        = ee->e;
    615568  _Cus      = ee->Cus;
    616569  _sqrt_A   = ee->sqrt_A;
     570
     571  _TOEsec   = ee->TOE;
    617572  _Cic      = ee->Cic;
    618573  _OMEGA0   = ee->OMEGA0;
    619574  _Cis      = ee->Cis;
     575
    620576  _i0       = ee->i0;
    621577  _Crc      = ee->Crc;
    622578  _omega    = ee->omega;
    623579  _OMEGADOT = ee->OMEGADOT;
     580
    624581  _IDOT     = ee->IDOT;
     582  _TOEweek  = ee->Week;
     583
    625584  _SISA     = ee->SISA;
     585  _E5aHS    = ee->E5aHS;
    626586  _BGD_1_5A = ee->BGD_1_5A;
    627587  _BGD_1_5B = ee->BGD_1_5B;
    628   _E5aHS    = ee->E5aHS;
     588
     589  _TOT      = 0.9999e9;
    629590
    630591  _ok = true;
    … …  
    634595////////////////////////////////////////////////////////////////////////////
    635596void t_ephGal::position(int GPSweek, double GPSweeks,
    636                         double* xc,
    637                         double* vv) const {
    638 
    639   static const double secPerWeek = 7 * 86400.0;
     597                        double* xc,
     598                        double* vv) const {
     599
    640600  static const double omegaEarth = 7292115.1467e-11;
    641601  static const double gmWGS      = 398.6005e12;
    … …  
    650610
    651611  double n0 = sqrt(gmWGS/(a0*a0*a0));
    652   double tk = GPSweeks - _TOE;
    653   if (GPSweek != _GPSweek) { 
    654     tk += (GPSweek - _GPSweek) * secPerWeek;
    655   }
     612
     613  bncTime tt(GPSweek, GPSweeks);
     614  double tk = tt - bncTime(_TOC.gpsw(), _TOEsec);
     615
    656616  double n  = n0 + _Delta_n;
    657617  double M  = _M0 + n*tk;
    … …  
    672632  double yp     = r*sin(u);
    673633  double OM     = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
    674                    omegaEarth*_TOE;
     634                  omegaEarth*_TOEsec;
    675635 
    676636  double sinom = sin(OM);
    … …  
    682642  xc[2] = yp*sini;                 
    683643 
    684   double tc = GPSweeks - _TOC;
    685   if (GPSweek != _GPSweek) { 
    686     tc += (GPSweek - _GPSweek) * secPerWeek;
    687   }
     644  double tc = tt - _TOC;
    688645  xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
    689646
    … …  
    720677// build up RTCM3 for Galileo
    721678////////////////////////////////////////////////////////////////////////////
    722 #define GALILEOTOINT(type, value) static_cast<type>(NearestInt(value, 0))
     679#define GALILEOTOINT(type, value) static_cast<type>(round(value))
    723680
    724681#define GALILEOADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
    … …  
    739696  GALILEOADDBITS(12, /*inav ? 1046 :*/ 1045)
    740697  GALILEOADDBITS(6, _prn.right((_prn.length()-1)).toInt())
    741   GALILEOADDBITS(12, _GPSweek)
     698  GALILEOADDBITS(12, _TOC.gpsw())
    742699  GALILEOADDBITS(10, _IODnav)
    743700  GALILEOADDBITS(8, _SISA)
    744   GALILEOADDBITSFLOAT(14, _IDOT, PI/static_cast<double>(1<<30)
     701  GALILEOADDBITSFLOAT(14, _IDOT, M_PI/static_cast<double>(1<<30)
    745702  /static_cast<double>(1<<13))
    746   GALILEOADDBITS(14, _TOC/60)
     703  GALILEOADDBITS(14, _TOC.gpssec()/60)
    747704  GALILEOADDBITSFLOAT(6, _clock_driftrate, 1.0/static_cast<double>(1<<30)
    748705  /static_cast<double>(1<<29))
    … …  
    752709  /static_cast<double>(1<<4))
    753710  GALILEOADDBITSFLOAT(16, _Crs, 1.0/static_cast<double>(1<<5))
    754   GALILEOADDBITSFLOAT(16, _Delta_n, PI/static_cast<double>(1<<30)
     711  GALILEOADDBITSFLOAT(16, _Delta_n, M_PI/static_cast<double>(1<<30)
    755712  /static_cast<double>(1<<13))
    756   GALILEOADDBITSFLOAT(32, _M0, PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
     713  GALILEOADDBITSFLOAT(32, _M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
    757714  GALILEOADDBITSFLOAT(16, _Cuc, 1.0/static_cast<double>(1<<29))
    758715  GALILEOADDBITSFLOAT(32, _e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
    759716  GALILEOADDBITSFLOAT(16, _Cus, 1.0/static_cast<double>(1<<29))
    760717  GALILEOADDBITSFLOAT(32, _sqrt_A, 1.0/static_cast<double>(1<<19))
    761   GALILEOADDBITS(14, _TOE/60)
     718  GALILEOADDBITS(14, _TOEsec/60)
    762719  GALILEOADDBITSFLOAT(16, _Cic, 1.0/static_cast<double>(1<<29))
    763   GALILEOADDBITSFLOAT(32, _OMEGA0, PI/static_cast<double>(1<<30)
     720  GALILEOADDBITSFLOAT(32, _OMEGA0, M_PI/static_cast<double>(1<<30)
    764721  /static_cast<double>(1<<1))
    765722  GALILEOADDBITSFLOAT(16, _Cis, 1.0/static_cast<double>(1<<29))
    766   GALILEOADDBITSFLOAT(32, _i0, PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
     723  GALILEOADDBITSFLOAT(32, _i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
    767724  GALILEOADDBITSFLOAT(16, _Crc, 1.0/static_cast<double>(1<<5))
    768   GALILEOADDBITSFLOAT(32, _omega, PI/static_cast<double>(1<<30)
     725  GALILEOADDBITSFLOAT(32, _omega, M_PI/static_cast<double>(1<<30)
    769726  /static_cast<double>(1<<1))
    770   GALILEOADDBITSFLOAT(24, _OMEGADOT, PI/static_cast<double>(1<<30)
     727  GALILEOADDBITSFLOAT(24, _OMEGADOT, M_PI/static_cast<double>(1<<30)
    771728  /static_cast<double>(1<<13))
    772729  GALILEOADDBITSFLOAT(10, _BGD_1_5A, 1.0/static_cast<double>(1<<30)
    … …  
    784741    GALILEOADDBITS(1, /*flags & MNFGALEPHF_E5ADINVALID*/0)
    785742  }
    786   _TOE = 0.9999E9;
    787   GALILEOADDBITS(20, _TOE)
     743  _TOEsec = 0.9999E9;
     744  GALILEOADDBITS(20, _TOEsec)
    788745
    789746  GALILEOADDBITS(/*inav ? 1 :*/ 3, 0) /* fill up */
    … …  
    846803      }
    847804
    848       bncTime hlpTime;
    849       hlpTime.set(year, month, day, hour, min, sec);
    850       _GPSweek  = hlpTime.gpsw();
    851       _GPSweeks = hlpTime.gpssec();
    852       _TOC      = _GPSweeks;
     805      _TOC.set(year, month, day, hour, min, sec);
    853806
    854807      if ( readDbl(line, pos[1], fieldLen, _clock_bias     ) ||
    … …  
    878831
    879832    else if ( iLine == 3 ) {
    880       if ( readDbl(line, pos[0], fieldLen, _TOE   )  ||
     833      if ( readDbl(line, pos[0], fieldLen, _TOEsec)  ||
    881834           readDbl(line, pos[1], fieldLen, _Cic   )  ||
    882835           readDbl(line, pos[2], fieldLen, _OMEGA0)  ||
    … …  
    896849
    897850    else if ( iLine == 5 ) {
    898       double dummy, TOEw;
    899       if ( readDbl(line, pos[0], fieldLen, _IDOT) ||
    900            readDbl(line, pos[1], fieldLen, dummy) ||
    901            readDbl(line, pos[2], fieldLen, TOEw ) ||
    902            readDbl(line, pos[3], fieldLen, dummy) ) {
     851      if ( readDbl(line, pos[0], fieldLen, _IDOT   ) ||
     852           readDbl(line, pos[1], fieldLen, _L2Codes) ||
     853           readDbl(line, pos[2], fieldLen, _TOEweek  ) ||
     854           readDbl(line, pos[3], fieldLen, _L2PFlag) ) {
    903855        return;
    904856      }
    … …  
    906858
    907859    else if ( iLine == 6 ) {
    908       double dummy;
    909       if ( readDbl(line, pos[0], fieldLen, dummy  ) ||
     860      if ( readDbl(line, pos[0], fieldLen, _ura   ) ||
    910861           readDbl(line, pos[1], fieldLen, _health) ||
    911862           readDbl(line, pos[2], fieldLen, _TGD   ) ||
    … …  
    916867
    917868    else if ( iLine == 7 ) {
    918       double TOT;
    919       if ( readDbl(line, pos[0], fieldLen, TOT) ) {
     869      if ( readDbl(line, pos[0], fieldLen, _TOT)         ||
     870           readDbl(line, pos[1], fieldLen, _fitInterval) ) {
    920871        return;
    921872      }
    … …  
    972923      }
    973924
    974       bncTime hlpTime;
    975       hlpTime.set(year, month, day, hour, min, sec);
    976 
    977925      _gps_utc = gnumleap(year, month, day);
    978       hlpTime  = hlpTime + _gps_utc;
    979 
    980       _GPSweek  = hlpTime.gpsw();
    981       _GPSweeks = hlpTime.gpssec();
    982 
    983       _tki     = 0.0; // TODO ?
    984 
    985       double second_tot;
    986       if ( readDbl(line, pos[1], fieldLen, _tau      ) ||
    987            readDbl(line, pos[2], fieldLen, _gamma    ) ||
    988            readDbl(line, pos[3], fieldLen, second_tot) ) {
     926
     927      _TOC.set(year, month, day, hour, min, sec);
     928      _TOC  = _TOC + _gps_utc;
     929
     930      if ( readDbl(line, pos[1], fieldLen, _tau  ) ||
     931           readDbl(line, pos[2], fieldLen, _gamma) ||
     932           readDbl(line, pos[3], fieldLen, _tki  ) ) {
    989933        return;
    990934      }
    … …  
    1023967  // Initialize status vector
    1024968  // ------------------------
    1025   _tt = _GPSweeks;
    1026 
    1027    _xv.ReSize(6);
    1028 
     969  _tt = _TOC;
     970  _xv.ReSize(6);
    1029971  _xv(1) = _x_pos * 1.e3;
    1030972  _xv(2) = _y_pos * 1.e3;
    … …  
    10621004  QString rnxStr;
    10631005 
    1064   bncTime tt(_GPSweek, _GPSweeks);
    10651006  unsigned year, month, day, hour, min;
    1066   double sec;
    1067   tt.civil_date(year, month, day);
    1068   tt.civil_time(hour, min, sec);
     1007  double   sec;
     1008  _TOC.civil_date(year, month, day);
     1009  _TOC.civil_time(hour, min, sec);
    10691010 
    10701011  QTextStream out(&rnxStr);
    … …  
    11101051
    11111052  out << QString(fmt)
    1112     .arg(_TOE,    19, 'e', 12)
     1053    .arg(_TOEsec, 19, 'e', 12)
    11131054    .arg(_Cic,    19, 'e', 12)
    11141055    .arg(_OMEGA0, 19, 'e', 12)
    … …  
    11221063
    11231064  out << QString(fmt)
    1124     .arg(_IDOT, 19, 'e', 12)
    1125     .arg(0.0,  19, 'e', 12)
    1126     .arg(0.0,  19, 'e', 12)
    1127     .arg(0.0,  19, 'e', 12);
     1065    .arg(_IDOT,    19, 'e', 12)
     1066    .arg(_L2Codes, 19, 'e', 12)
     1067    .arg(_TOEweek, 19, 'e', 12)
     1068    .arg(_L2PFlag, 19, 'e', 12);
    11281069
    11291070  out << QString(fmt)
    1130     .arg(0.0,     19, 'e', 12)
     1071    .arg(_ura,    19, 'e', 12)
    11311072    .arg(_health, 19, 'e', 12)
    11321073    .arg(_TGD,    19, 'e', 12)
    … …  
    11341075
    11351076  out << QString(fmt)
    1136     .arg(0.0, 19, 'e', 12)
    1137     .arg(0.0, 19, 'e', 12)
     1077    .arg(_TOT,        19, 'e', 12)
     1078    .arg(_fitInterval, 19, 'e', 12)
    11381079    .arg("")
    11391080    .arg("");

  • trunk/BNC/RTCM3/ephemeris.h

    r4013 r4018  
    66#include <stdio.h>
    77#include <string>
     8#include "bnctime.h"
    89extern "C" {
    910#include "rtcm3torinex.h"
    … …  
    1516  enum e_type {unknown, GPS, GLONASS, Galileo};
    1617
    17   static bool earlierTime(const t_eph* eph1, const t_eph* eph2) {
    18     if      (eph1->_GPSweek < eph2->_GPSweek) {
    19       return true;
    20     }
    21     else if (eph1->_GPSweek == eph2->_GPSweek) {
    22       return eph1->_GPSweeks < eph2->_GPSweeks;
    23     }
    24     else {
    25       return false;
    26     }
    27   }
    28 
    2918  t_eph() {_ok = false;}
    3019  virtual ~t_eph() {};
    3120
     21  static bool earlierTime(const t_eph* eph1, const t_eph* eph2) {
     22    return eph1->_TOC < eph2->_TOC;
     23  }
     24
    3225  virtual e_type type() const = 0;
    33 
    3426  virtual QString toString(double version) const = 0;
    35 
    36   bool     ok() const {return _ok;}
    37   bool     isNewerThan(const t_eph* eph) const;
    38   QString  prn() const {return _prn;}
    39   void    setReceptDateTime(const QDateTime& dateTime) {
     27  virtual void position(int GPSweek, double GPSweeks,
     28                        double* xc, double* vv) const = 0;
     29  virtual int  IOD() const = 0;
     30  virtual int  RTCM3(unsigned char *) = 0;
     31
     32  bool ok() const {return _ok;}
     33  bncTime TOC() const {return _TOC;}
     34  bool isNewerThan(const t_eph* eph) const {
     35    return earlierTime(this, eph);
     36  }
     37  QString prn() const {return _prn;}
     38  void  setReceptDateTime(const QDateTime& dateTime) {
    4039    _receptDateTime = dateTime;
    4140  }
    4241  const QDateTime& receptDateTime() const {return _receptDateTime;}
    43 
    44   int    GPSweek()  const { return _GPSweek; }
    45   double GPSweeks() const { return _GPSweeks; }
    46 
    47   virtual void position(int GPSweek, double GPSweeks,
    48                         double* xc, double* vv) const = 0;
    4942
    5043  void position(int GPSweek, double GPSweeks,
    … …  
    5245    double tmp_xx[4];
    5346    double tmp_vv[4];
    54 
    5547    position(GPSweek, GPSweeks, tmp_xx, tmp_vv);
    5648
    … …  
    6153  }
    6254
    63   virtual int  IOD() const = 0;
    64  
    65   virtual int  RTCM3(unsigned char *) = 0;
    66 
    6755 protected: 
    6856  QString   _prn;
    69   int       _GPSweek;
    70   double    _GPSweeks;
     57  bncTime   _TOC;
    7158  QDateTime _receptDateTime;
    7259  bool      _ok;
    … …  
    8471  virtual QString toString(double version) const;
    8572
    86   double TOC() const {return _TOC;}
    87 
    8873  void set(const gpsephemeris* ee);
    8974
    … …  
    9782
    9883 private:
    99   double  _TOW;              //  [s]   
    100   double  _TOC;              //  [s]   
    101   double  _TOE;              //  [s]   
    102   double  _IODE;             
    103   double  _IODC;             
    104 
     84  double  _clock_bias;      // [s]   
     85  double  _clock_drift;     // [s/s] 
     86  double  _clock_driftrate; // [s/s^2]
     87
     88  double  _IODE;           
     89  double  _Crs;             // [m]   
     90  double  _Delta_n;         // [rad/s]
     91  double  _M0;              // [rad] 
     92
     93  double  _Cuc;             // [rad] 
     94  double  _e;               //       
     95  double  _Cus;             // [rad] 
     96  double  _sqrt_A;          // [m^0.5]
     97
     98  double  _TOEsec;          // [s]   
     99  double  _Cic;             // [rad] 
     100  double  _OMEGA0;          // [rad] 
     101  double  _Cis;             // [rad] 
     102
     103  double  _i0;              // [rad] 
     104  double  _Crc;             // [m]   
     105  double  _omega;           // [rad] 
     106  double  _OMEGADOT;        // [rad/s]
     107
     108  double  _IDOT;            // [rad/s]
     109  double  _L2Codes;         // Codes on L2 channel
     110  double  _TOEweek;
     111  double  _L2PFlag;         // L2 P data flag
     112
     113  double  _ura;             // SV accuracy
     114  double  _health;          // SV health
     115  double  _TGD;             // [s]   
     116  double  _IODC;           
     117
     118  double  _TOT;             // Transmisstion time
     119  double  _fitInterval;     // Fit interval
     120};
     121
     122class t_ephGlo : public t_eph {
     123 public:
     124  t_ephGlo() { _xv.ReSize(6); }
     125  t_ephGlo(float rnxVersion, const QStringList& lines);
     126
     127  virtual ~t_ephGlo() {}
     128
     129  virtual e_type type() const {return t_eph::GLONASS;}
     130
     131  virtual QString toString(double version) const;
     132
     133  virtual void position(int GPSweek, double GPSweeks,
     134                        double* xc,
     135                        double* vv) const;
     136
     137  virtual int  IOD() const;
     138
     139  virtual int  RTCM3(unsigned char *);
     140
     141  void set(const glonassephemeris* ee);
     142
     143  int  slotNum() const {return int(_frequency_number);}
     144
     145 private:
     146  static ColumnVector glo_deriv(double /* tt */, const ColumnVector& xv,
     147                                double* acc);
     148
     149  mutable bncTime      _tt;  // time
     150  mutable ColumnVector _xv;  // status vector (position, velocity) at time _tt
     151
     152  double  _gps_utc;
     153  double  _tau;              // [s]     
     154  double  _gamma;            //         
     155  double  _tki;              // message frame time
     156
     157  double  _x_pos;            // [km]     
     158  double  _x_velocity;       // [km/s]   
     159  double  _x_acceleration;   // [km/s^2]
     160  double  _health;           // 0 = O.K.
     161
     162  double  _y_pos;            // [km]     
     163  double  _y_velocity;       // [km/s]   
     164  double  _y_acceleration;   // [km/s^2]
     165  double  _frequency_number; // ICD-GLONASS data position
     166
     167  double  _z_pos;            // [km]     
     168  double  _z_velocity;       // [km/s]   
     169  double  _z_acceleration;   // [km/s^2]
     170  double  _E;                // Age of Information [days]   
     171};
     172
     173class t_ephGal : public t_eph {
     174 public:
     175  t_ephGal() { }
     176  t_ephGal(float rnxVersion, const QStringList& lines);
     177  virtual ~t_ephGal() {}
     178
     179  virtual QString toString(double version) const;
     180
     181  virtual e_type type() const {return t_eph::Galileo;}
     182
     183  void set(const galileoephemeris* ee);
     184
     185  virtual void position(int GPSweek, double GPSweeks,
     186                        double* xc,
     187                        double* vv) const;
     188
     189  virtual int  IOD() const { return static_cast<int>(_IODnav); }
     190
     191  virtual int  RTCM3(unsigned char *);
     192
     193 private:
    105194  double  _clock_bias;       //  [s]   
    106195  double  _clock_drift;      //  [s/s] 
    107196  double  _clock_driftrate;  //  [s/s^2]
    108197
     198  double  _IODnav;             
    109199  double  _Crs;              //  [m]   
    110200  double  _Delta_n;          //  [rad/s]
    111201  double  _M0;               //  [rad] 
     202
    112203  double  _Cuc;              //  [rad] 
    113204  double  _e;                //         
    114205  double  _Cus;              //  [rad] 
    115206  double  _sqrt_A;           //  [m^0.5]
     207
     208  double  _TOEsec;           //  [s]   
    116209  double  _Cic;              //  [rad] 
    117210  double  _OMEGA0;           //  [rad] 
    118211  double  _Cis;              //  [rad] 
     212
    119213  double  _i0;               //  [rad] 
    120214  double  _Crc;              //  [m]   
    121215  double  _omega;            //  [rad] 
    122216  double  _OMEGADOT;         //  [rad/s]
     217
    123218  double  _IDOT;             //  [rad/s]
    124 
    125   double  _TGD;              //  [s]   
    126   double _health;            //  SV health
    127   double _ura;               //  SV accuracy
    128   double _L2PFlag;           //  L2 P data flag
    129   double _L2Codes;           //  Codes on L2 channel
    130 };
    131 
    132 class t_ephGlo : public t_eph {
    133  public:
    134   t_ephGlo() { _xv.ReSize(6); }
    135   t_ephGlo(float rnxVersion, const QStringList& lines);
    136 
    137   virtual ~t_ephGlo() {}
    138 
    139   virtual e_type type() const {return t_eph::GLONASS;}
    140 
    141   virtual QString toString(double version) const;
    142 
    143   virtual void position(int GPSweek, double GPSweeks,
    144                         double* xc,
    145                         double* vv) const;
    146 
    147   virtual int  IOD() const;
    148 
    149   virtual int  RTCM3(unsigned char *);
    150 
    151   void set(const glonassephemeris* ee);
    152 
    153   int  slotNum() const {return int(_frequency_number);}
    154 
    155  private:
    156   static ColumnVector glo_deriv(double /* tt */, const ColumnVector& xv,
    157                                 double* acc);
    158 
    159   mutable double       _tt;  // time in seconds of GPSweek
    160   mutable ColumnVector _xv;  // status vector (position, velocity) at time _tt
    161 
    162   double  _gps_utc;
    163   double  _E;                // [days]   
    164   double  _tau;              // [s]     
    165   double  _gamma;            //         
    166   double  _x_pos;            // [km]     
    167   double  _x_velocity;       // [km/s]   
    168   double  _x_acceleration;   // [km/s^2]
    169   double  _y_pos;            // [km]     
    170   double  _y_velocity;       // [km/s]   
    171   double  _y_acceleration;   // [km/s^2]
    172   double  _z_pos;            // [km]     
    173   double  _z_velocity;       // [km/s]   
    174   double  _z_acceleration;   // [km/s^2]
    175   double  _health;           // 0 = O.K.
    176   double  _frequency_number; // ICD-GLONASS data position
    177   double  _tki;              // message frame time
    178 };
    179 
    180 class t_ephGal : public t_eph {
    181  public:
    182   t_ephGal() { }
    183   t_ephGal(float rnxVersion, const QStringList& lines);
    184   virtual ~t_ephGal() {}
    185 
    186   virtual QString toString(double version) const;
    187 
    188   virtual e_type type() const {return t_eph::Galileo;}
    189 
    190   double TOC() const {return _TOC;}
    191 
    192   void set(const galileoephemeris* ee);
    193 
    194   virtual void position(int GPSweek, double GPSweeks,
    195                         double* xc,
    196                         double* vv) const;
    197 
    198   virtual int  IOD() const { return static_cast<int>(_IODnav); }
    199 
    200   virtual int  RTCM3(unsigned char *);
    201 
    202  private:
    203   double  _IODnav;             
    204   double  _TOC;              //  [s]   
    205   double  _TOE;              //  [s]   
    206   double  _clock_bias;       //  [s]   
    207   double  _clock_drift;      //  [s/s] 
    208   double  _clock_driftrate;  //  [s/s^2]
    209   double  _Crs;              //  [m]   
    210   double  _Delta_n;          //  [rad/s]
    211   double  _M0;               //  [rad] 
    212   double  _Cuc;              //  [rad] 
    213   double  _e;                //         
    214   double  _Cus;              //  [rad] 
    215   double  _sqrt_A;           //  [m^0.5]
    216   double  _Cic;              //  [rad] 
    217   double  _OMEGA0;           //  [rad] 
    218   double  _Cis;              //  [rad] 
    219   double  _i0;               //  [rad] 
    220   double  _Crc;              //  [m]   
    221   double  _omega;            //  [rad] 
    222   double  _OMEGADOT;         //  [rad/s]
    223   double  _IDOT;             //  [rad/s]
     219  //
     220  double _TOEweek;
     221  // spare
     222
     223  int     _SISA;             //  Signal In Space Accuracy
     224  int     _E5aHS;            //  E5a Health Status
    224225  double  _BGD_1_5A;         //  group delay [s]
    225226  double  _BGD_1_5B;         //  group delay [s]
    226   int     _SISA;             //  Signal In Space Accuracy
    227   int     _E5aHS;            //  E5a Health Status
    228 
     227
     228  double _TOT;               // [s]
    229229};
    230230

  • trunk/BNC/bncephuser.cpp

    r3752 r4018  
    8181  if (_eph.contains(prn)) {
    8282    t_ephGPS* eLast = static_cast<t_ephGPS*>(_eph.value(prn)->last);
    83     if ( (eLast->GPSweek() <  gpseph.GPSweek) ||
    84          (eLast->GPSweek() == gpseph.GPSweek && 
    85           eLast->TOC()     <  gpseph.TOC) ) {
     83    bncTime toc(gpseph.GPSweek, gpseph.TOC);
     84    if (eLast->TOC() < toc) {
    8685      delete static_cast<t_ephGPS*>(_eph.value(prn)->prev);
    8786      _eph.value(prn)->prev = _eph.value(prn)->last;
    … …  
    110109    updatetime(&ww, &tow, gloeph.tb*1000, 0);  // Moscow -> GPS
    111110    t_ephGlo* eLast = static_cast<t_ephGlo*>(_eph.value(prn)->last);
    112     if (eLast->GPSweek() < ww ||
    113         (eLast->GPSweek()  == ww &&  eLast->GPSweeks() <  tow)) { 
     111    bncTime toc(ww, tow);
     112    if (eLast->TOC() < toc) {
    114113      delete static_cast<t_ephGlo*>(_eph.value(prn)->prev);
    115114      _eph.value(prn)->prev = _eph.value(prn)->last;
    … …  
    135134  if (_eph.contains(prn)) {
    136135    t_ephGal* eLast = static_cast<t_ephGal*>(_eph.value(prn)->last);
    137     if ( (eLast->GPSweek() <  galeph.Week) ||
    138          (eLast->GPSweek() == galeph.Week && 
    139           eLast->TOC()     <  galeph.TOC) ) {
     136    bncTime toc(galeph.Week, galeph.TOC);
     137    if (eLast->TOC() < toc) {
    140138      delete static_cast<t_ephGal*>(_eph.value(prn)->prev);
    141139      _eph.value(prn)->prev = _eph.value(prn)->last;

  • trunk/BNC/rinex/rnxnavfile.cpp

    r4013 r4018  
    223223      t_eph* eph = *it;
    224224
    225       bncTime ephTime(eph->GPSweek(), eph->GPSweeks());
    226       double dt = ephTime - tt;
     225      double dt = eph->TOC() - tt;
    227226
    228227      if (dt < 2*3600.0) {
Note: See TracChangeset for help on using the changeset viewer.