Ignore:
Timestamp:
Oct 7, 2014, 10:40:33 AM (10 years ago)
Author:
stuerze
Message:

prevent usage of very old GLONASS navigation messages

File:
1 edited

Legend:

Unmodified
Added
Removed

  • branches/BNC_2.11.0/src/RTCM3/ephemeris.cpp

    r5542 r6214  
    8888// Compute GPS Satellite Position (virtual)
    8989////////////////////////////////////////////////////////////////////////////
    90 void t_ephGPS::position(int GPSweek, double GPSweeks,
     90t_irc t_ephGPS::position(int GPSweek, double GPSweeks,
    9191                        double* xc,
    9292                        double* vv) const {
    … …  
    101101  double a0 = _sqrt_A * _sqrt_A;
    102102  if (a0 == 0) {
    103     return;
     103    return failure;
    104104  }
    105105
    … …  
    126126  double xp     = r*cos(u);
    127127  double yp     = r*sin(u);
    128   double OM     = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk - 
     128  double OM     = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
    129129                   omegaEarth*_TOEsec;
    130  
     130
    131131  double sinom = sin(OM);
    132132  double cosom = cos(OM);
    … …  
    135135  xc[0] = xp*cosom - yp*cosi*sinom;
    136136  xc[1] = xp*sinom + yp*cosi*cosom;
    137   xc[2] = yp*sini;                 
    138  
     137  xc[2] = yp*sini;
     138
    139139  double tc = tt - _TOC;
    140140  xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
    … …  
    144144  double tanv2 = tan(v/2);
    145145  double dEdM  = 1 / (1 - _e*cos(E));
    146   double dotv  = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2) 
     146  double dotv  = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
    147147               * dEdM * n;
    148148  double dotu  = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
    149149  double dotom = _OMEGADOT - omegaEarth;
    150150  double doti  = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
    151   double dotr  = a0 * _e*sin(E) * dEdM * n 
     151  double dotr  = a0 * _e*sin(E) * dEdM * n
    152152                + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
    153153  double dotx  = dotr*cos(u) - r*sin(u)*dotu;
    … …  
    167167  // -----------------------
    168168  xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c;
     169
     170  return success;
    169171}
    170172
    … …  
    274276  /static_cast<double>(1<<13))
    275277  GPSADDBITSFLOAT(8, _TGD, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
    276   GPSADDBITS(6, _health) 
     278  GPSADDBITS(6, _health)
    277279  GPSADDBITS(1, _L2PFlag)
    278280  GPSADDBITS(1, 0) /* GPS fit interval */
    … …  
    299301  ColumnVector vv = xv.rows(4,6);
    300302
    301   // Acceleration 
     303  // Acceleration
    302304  // ------------
    303305  static const double gmWGS = 398.60044e12;
    … …  
    319321  va(2) = vv(2);
    320322  va(3) = vv(3);
    321   va(4) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2) + acc[0]; 
    322   va(5) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1) + acc[1]; 
     323  va(4) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2) + acc[0];
     324  va(5) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1) + acc[1];
    323325  va(6) = (t1 + t2*(3.0-5.0*z2/(rho*rho))     ) * rr(3)            + acc[2];
    324326
    … …  
    328330// Compute Glonass Satellite Position (virtual)
    329331////////////////////////////////////////////////////////////////////////////
    330 void t_ephGlo::position(int GPSweek, double GPSweeks,
     332t_irc t_ephGlo::position(int GPSweek, double GPSweeks,
    331333                        double* xc, double* vv) const {
    332334
    … …  
    337339
    338340  double dtPos = bncTime(GPSweek, GPSweeks) - _tt;
     341
     342  if (fabs(dtPos) > 24*3600.0) {
     343          return failure;
     344  }
    339345
    340346  int nSteps  = int(fabs(dtPos) / nominalStep) + 1;
    … …  
    345351  acc[1] = _y_acceleration * 1.e3;
    346352  acc[2] = _z_acceleration * 1.e3;
    347   for (int ii = 1; ii <= nSteps; ii++) { 
     353  for (int ii = 1; ii <= nSteps; ii++) {
    348354    _xv = rungeKutta4(_tt.gpssec(), _xv, step, acc, glo_deriv);
    349355    _tt = _tt + step;
    … …  
    364370  double dtClk = bncTime(GPSweek, GPSweeks) - _TOC;
    365371  xc[3] = -_tau + _gamma * dtClk;
     372
     373  return success;
    366374}
    367375
    … …  
    382390
    383391  int ww  = ee->GPSWeek;
    384   int tow = ee->GPSTOW; 
     392  int tow = ee->GPSTOW;
    385393  updatetime(&ww, &tow, ee->tb*1000, 0);  // Moscow -> GPS
    386394
    387   // Check the day once more 
     395  // Check the day once more
    388396  // -----------------------
    389397  bool timeChanged = false;
    … …  
    418426    if (false && timeChanged && BNC_CORE->mode() == t_bncCore::batchPostProcessing) {
    419427      bncTime newHTime(ww, (double) tow);
    420       cout << "GLONASS " << ee->almanac_number <<  " Time Changed at " 
    421            << currentTime.datestr()         << " " << currentTime.timestr() 
     428      cout << "GLONASS " << ee->almanac_number <<  " Time Changed at "
     429           << currentTime.datestr()         << " " << currentTime.timestr()
    422430           << endl
    423            << "old: " << hTime.datestr()    << " " << hTime.timestr()       
     431           << "old: " << hTime.datestr()    << " " << hTime.timestr()
    424432           << endl
    425            << "new: " << newHTime.datestr() << " " << newHTime.timestr()   
     433           << "new: " << newHTime.datestr() << " " << newHTime.timestr()
    426434           << endl
    427            << "eph: " << ee->GPSWeek << " " << ee->GPSTOW << " " << ee->tb 
     435           << "eph: " << ee->GPSWeek << " " << ee->GPSTOW << " " << ee->tb
    428436           << endl
    429            << "ww, tow (old): " << ww_old << " " << tow_old 
     437           << "ww, tow (old): " << ww_old << " " << tow_old
    430438           << endl
    431            << "ww, tow (new): " << ww     << " " << tow 
     439           << "ww, tow (new): " << ww     << " " << tow
    432440           << endl << endl;
    433441    }
    … …  
    444452  _gamma             = ee->gamma;
    445453  _x_pos             = ee->x_pos;
    446   _x_velocity        = ee->x_velocity;     
     454  _x_velocity        = ee->x_velocity;
    447455  _x_acceleration    = ee->x_acceleration;
    448   _y_pos             = ee->y_pos;         
    449   _y_velocity        = ee->y_velocity;   
     456  _y_pos             = ee->y_pos;
     457  _y_velocity        = ee->y_velocity;
    450458  _y_acceleration    = ee->y_acceleration;
    451   _z_pos             = ee->z_pos;         
    452   _z_velocity        = ee->z_velocity;   
     459  _z_pos             = ee->z_pos;
     460  _z_velocity        = ee->z_velocity;
    453461  _z_acceleration    = ee->z_acceleration;
    454462  _health            = 0;
    … …  
    460468  _tt = _TOC;
    461469
    462   _xv(1) = _x_pos * 1.e3; 
    463   _xv(2) = _y_pos * 1.e3; 
    464   _xv(3) = _z_pos * 1.e3; 
    465   _xv(4) = _x_velocity * 1.e3; 
    466   _xv(5) = _y_velocity * 1.e3; 
    467   _xv(6) = _z_velocity * 1.e3; 
     470  _xv(1) = _x_pos * 1.e3;
     471  _xv(2) = _y_pos * 1.e3;
     472  _xv(3) = _z_pos * 1.e3;
     473  _xv(4) = _x_velocity * 1.e3;
     474  _xv(5) = _y_velocity * 1.e3;
     475  _xv(6) = _z_velocity * 1.e3;
    468476
    469477  _ok = true;
    … …  
    513521  GLONASSADDBITS(6, (static_cast<int>(_tki)/60)%60)
    514522  GLONASSADDBITS(1, (static_cast<int>(_tki)/30)%30)
    515   GLONASSADDBITS(1, _health) 
     523  GLONASSADDBITS(1, _health)
    516524  GLONASSADDBITS(1, 0)
    517525  unsigned long long timeofday = (static_cast<int>(_tt.gpssec()+3*60*60-_gps_utc)%86400);
    … …  
    581589
    582590  _TOEsec   = _TOC.gpssec();
    583   ////  _TOEsec   = ee->TOE;  //// TODO: 
     591  ////  _TOEsec   = ee->TOE;  //// TODO:
    584592  _Cic      = ee->Cic;
    585593  _OMEGA0   = ee->OMEGA0;
    … …  
    609617// Compute Galileo Satellite Position (virtual)
    610618////////////////////////////////////////////////////////////////////////////
    611 void t_ephGal::position(int GPSweek, double GPSweeks,
     619t_irc t_ephGal::position(int GPSweek, double GPSweeks,
    612620                        double* xc,
    613621                        double* vv) const {
    … …  
    621629  double a0 = _sqrt_A * _sqrt_A;
    622630  if (a0 == 0) {
    623     return;
     631    return failure;
    624632  }
    625633
    … …  
    646654  double xp     = r*cos(u);
    647655  double yp     = r*sin(u);
    648   double OM     = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk - 
     656  double OM     = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
    649657                  omegaEarth*_TOEsec;
    650  
     658
    651659  double sinom = sin(OM);
    652660  double cosom = cos(OM);
    … …  
    655663  xc[0] = xp*cosom - yp*cosi*sinom;
    656664  xc[1] = xp*sinom + yp*cosi*cosom;
    657   xc[2] = yp*sini;                 
    658  
     665  xc[2] = yp*sini;
     666
    659667  double tc = tt - _TOC;
    660668  xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
    … …  
    664672  double tanv2 = tan(v/2);
    665673  double dEdM  = 1 / (1 - _e*cos(E));
    666   double dotv  = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2) 
     674  double dotv  = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
    667675               * dEdM * n;
    668676  double dotu  = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
    669677  double dotom = _OMEGADOT - omegaEarth;
    670678  double doti  = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
    671   double dotr  = a0 * _e*sin(E) * dEdM * n 
     679  double dotr  = a0 * _e*sin(E) * dEdM * n
    672680                + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
    673681  double dotx  = dotr*cos(u) - r*sin(u)*dotu;
    … …  
    688696  //  xc(4) -= 4.442807633e-10 * _e * sqrt(a0) *sin(E);
    689697  xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c;
     698
     699  return success;
    690700}
    691701
    … …  
    806816      int    year, month, day, hour, min;
    807817      double sec;
    808      
     818
    809819      in >> _prn >> year >> month >> day >> hour >> min >> sec;
    810820
    … …  
    812822        _prn = QString("G%1").arg(_prn.toInt(), 2, 10, QLatin1Char('0'));
    813823      }
    814    
     824
    815825      if      (year <  80) {
    816826        year += 2000;
    … …  
    926936      int    year, month, day, hour, min;
    927937      double sec;
    928      
     938
    929939      in >> _prn >> year >> month >> day >> hour >> min >> sec;
    930940
    … …  
    932942        _prn = QString("R%1").arg(_prn.toInt(), 2, 10, QLatin1Char('0'));
    933943      }
    934    
     944
    935945      if      (year <  80) {
    936946        year += 2000;
    … …  
    985995  // ------------------------
    986996  _tt = _TOC;
    987   _xv.ReSize(6); 
    988   _xv(1) = _x_pos * 1.e3; 
    989   _xv(2) = _y_pos * 1.e3; 
    990   _xv(3) = _z_pos * 1.e3; 
    991   _xv(4) = _x_velocity * 1.e3; 
    992   _xv(5) = _y_velocity * 1.e3; 
    993   _xv(6) = _z_velocity * 1.e3; 
     997  _xv.ReSize(6);
     998  _xv(1) = _x_pos * 1.e3;
     999  _xv(2) = _y_pos * 1.e3;
     1000  _xv(3) = _z_pos * 1.e3;
     1001  _xv(4) = _x_velocity * 1.e3;
     1002  _xv(5) = _y_velocity * 1.e3;
     1003  _xv(6) = _z_velocity * 1.e3;
    9941004
    9951005  _ok = true;
    … …  
    10281038      int    year, month, day, hour, min;
    10291039      double sec;
    1030      
     1040
    10311041      in >> _prn >> year >> month >> day >> hour >> min >> sec;
    10321042
    … …  
    10341044        _prn = QString("E%1").arg(_prn.toInt(), 2, 10, QLatin1Char('0'));
    10351045      }
    1036    
     1046
    10371047      if      (year <  80) {
    10381048        year += 2000;
    … …  
    11131123}
    11141124
    1115 // 
     1125//
    11161126//////////////////////////////////////////////////////////////////////////////
    11171127QString t_eph::rinexDateStr(const bncTime& tt, const QString& prn,
    … …  
    11191129
    11201130  QString datStr;
    1121  
     1131
    11221132  unsigned year, month, day, hour, min;
    11231133  double   sec;
    11241134  tt.civil_date(year, month, day);
    11251135  tt.civil_time(hour, min, sec);
    1126  
     1136
    11271137  QTextStream out(&datStr);
    11281138
    … …  
    11551165
    11561166  QString rnxStr = rinexDateStr(_TOC, _prn, version);
    1157  
     1167
    11581168  QTextStream out(&rnxStr);
    1159  
     1169
    11601170  out << QString("%1%2%3\n")
    11611171    .arg(_clock_bias,      19, 'e', 12)
Note: See TracChangeset for help on using the changeset viewer.