Changeset 6214 in ntrip for branches/BNC_2.11.0


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

prevent usage of very old GLONASS navigation messages

Location:
branches/BNC_2.11.0/src/RTCM3
Files:
2 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)

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

    r5541 r6214  
    77#include <string>
    88#include "bnctime.h"
     9#include "bncconst.h"
    910extern "C" {
    1011#include "rtcm3torinex.h"
    … …  
    2526  virtual e_type type() const = 0;
    2627  virtual QString toString(double version) const = 0;
    27   virtual void position(int GPSweek, double GPSweeks,
     28  virtual t_irc position(int GPSweek, double GPSweeks,
    2829                        double* xc, double* vv) const = 0;
    2930  virtual int  IOD() const = 0;
    … …  
    3839  const QDateTime& receptDateTime() const {return _receptDateTime;}
    3940
    40   void position(int GPSweek, double GPSweeks,
     41  t_irc position(int GPSweek, double GPSweeks,
    4142                double& xx, double& yy, double& zz, double& cc) const {
    4243    double tmp_xx[4];
    4344    double tmp_vv[4];
    44     position(GPSweek, GPSweeks, tmp_xx, tmp_vv);
    45 
     45    if (position(GPSweek, GPSweeks, tmp_xx, tmp_vv) != success) {
     46        return failure;
     47    }
    4648    xx = tmp_xx[0];
    4749    yy = tmp_xx[1];
    4850    zz = tmp_xx[2];
    4951    cc = tmp_xx[3];
     52
     53    return success;
    5054  }
    5155
    … …  
    5357                              double version);
    5458
    55  protected: 
     59 protected:
    5660  QString   _prn;
    5761  bncTime   _TOC;
    … …  
    7377  void set(const gpsephemeris* ee);
    7478
    75   virtual void position(int GPSweek, double GPSweeks,
     79  virtual t_irc position(int GPSweek, double GPSweeks,
    7680                        double* xc,
    7781                        double* vv) const;
    … …  
    8488
    8589 private:
    86   double  _clock_bias;      // [s]   
    87   double  _clock_drift;     // [s/s] 
     90  double  _clock_bias;      // [s]
     91  double  _clock_drift;     // [s/s]
    8892  double  _clock_driftrate; // [s/s^2]
    8993
    90   double  _IODE;           
    91   double  _Crs;             // [m]   
     94  double  _IODE;
     95  double  _Crs;             // [m]
    9296  double  _Delta_n;         // [rad/s]
    93   double  _M0;              // [rad] 
    94 
    95   double  _Cuc;             // [rad] 
    96   double  _e;               //       
    97   double  _Cus;             // [rad] 
     97  double  _M0;              // [rad]
     98
     99  double  _Cuc;             // [rad]
     100  double  _e;               //
     101  double  _Cus;             // [rad]
    98102  double  _sqrt_A;          // [m^0.5]
    99103
    100   double  _TOEsec;          // [s]   
    101   double  _Cic;             // [rad] 
    102   double  _OMEGA0;          // [rad] 
    103   double  _Cis;             // [rad] 
    104 
    105   double  _i0;              // [rad] 
    106   double  _Crc;             // [m]   
    107   double  _omega;           // [rad] 
     104  double  _TOEsec;          // [s]
     105  double  _Cic;             // [rad]
     106  double  _OMEGA0;          // [rad]
     107  double  _Cis;             // [rad]
     108
     109  double  _i0;              // [rad]
     110  double  _Crc;             // [m]
     111  double  _omega;           // [rad]
    108112  double  _OMEGADOT;        // [rad/s]
    109113
    110114  double  _IDOT;            // [rad/s]
    111   double  _L2Codes;         // Codes on L2 channel 
     115  double  _L2Codes;         // Codes on L2 channel
    112116  double  _TOEweek;
    113117  double  _L2PFlag;         // L2 P data flag
    … …  
    115119  double  _ura;             // SV accuracy
    116120  double  _health;          // SV health
    117   double  _TGD;             // [s]   
    118   double  _IODC;           
    119 
    120   double  _TOT;             // Transmisstion time 
     121  double  _TGD;             // [s]
     122  double  _IODC;
     123
     124  double  _TOT;             // Transmisstion time
    121125  double  _fitInterval;     // Fit interval
    122126};
    … …  
    133137  virtual QString toString(double version) const;
    134138
    135   virtual void position(int GPSweek, double GPSweeks,
     139  virtual t_irc position(int GPSweek, double GPSweeks,
    136140                        double* xc,
    137141                        double* vv) const;
    … …  
    149153                                double* acc);
    150154
    151   mutable bncTime      _tt;  // time 
     155  mutable bncTime      _tt;  // time
    152156  mutable ColumnVector _xv;  // status vector (position, velocity) at time _tt
    153157
    154158  double  _gps_utc;
    155   double  _tau;              // [s]     
    156   double  _gamma;            //         
     159  double  _tau;              // [s]
     160  double  _gamma;            //
    157161  double  _tki;              // message frame time
    158162
    159   double  _x_pos;            // [km]     
    160   double  _x_velocity;       // [km/s]   
    161   double  _x_acceleration;   // [km/s^2] 
    162   double  _health;           // 0 = O.K. 
    163 
    164   double  _y_pos;            // [km]     
    165   double  _y_velocity;       // [km/s]   
    166   double  _y_acceleration;   // [km/s^2] 
    167   double  _frequency_number; // ICD-GLONASS data position 
    168 
    169   double  _z_pos;            // [km]     
    170   double  _z_velocity;       // [km/s]   
    171   double  _z_acceleration;   // [km/s^2] 
    172   double  _E;                // Age of Information [days]   
     163  double  _x_pos;            // [km]
     164  double  _x_velocity;       // [km/s]
     165  double  _x_acceleration;   // [km/s^2]
     166  double  _health;           // 0 = O.K.
     167
     168  double  _y_pos;            // [km]
     169  double  _y_velocity;       // [km/s]
     170  double  _y_acceleration;   // [km/s^2]
     171  double  _frequency_number; // ICD-GLONASS data position
     172
     173  double  _z_pos;            // [km]
     174  double  _z_velocity;       // [km/s]
     175  double  _z_acceleration;   // [km/s^2]
     176  double  _E;                // Age of Information [days]
    173177};
    174178
    … …  
    185189  void set(const galileoephemeris* ee);
    186190
    187   virtual void position(int GPSweek, double GPSweeks,
     191  virtual t_irc position(int GPSweek, double GPSweeks,
    188192                        double* xc,
    189193                        double* vv) const;
    … …  
    194198
    195199 private:
    196   double  _clock_bias;       //  [s]   
    197   double  _clock_drift;      //  [s/s] 
     200  double  _clock_bias;       //  [s]
     201  double  _clock_drift;      //  [s/s]
    198202  double  _clock_driftrate;  //  [s/s^2]
    199203
    200   double  _IODnav;             
    201   double  _Crs;              //  [m]   
     204  double  _IODnav;
     205  double  _Crs;              //  [m]
    202206  double  _Delta_n;          //  [rad/s]
    203   double  _M0;               //  [rad] 
    204 
    205   double  _Cuc;              //  [rad] 
    206   double  _e;                //         
    207   double  _Cus;              //  [rad] 
     207  double  _M0;               //  [rad]
     208
     209  double  _Cuc;              //  [rad]
     210  double  _e;                //
     211  double  _Cus;              //  [rad]
    208212  double  _sqrt_A;           //  [m^0.5]
    209213
    210   double  _TOEsec;           //  [s]   
    211   double  _Cic;              //  [rad] 
    212   double  _OMEGA0;           //  [rad] 
    213   double  _Cis;              //  [rad] 
    214 
    215   double  _i0;               //  [rad] 
    216   double  _Crc;              //  [m]   
    217   double  _omega;            //  [rad] 
     214  double  _TOEsec;           //  [s]
     215  double  _Cic;              //  [rad]
     216  double  _OMEGA0;           //  [rad]
     217  double  _Cis;              //  [rad]
     218
     219  double  _i0;               //  [rad]
     220  double  _Crc;              //  [m]
     221  double  _omega;            //  [rad]
    218222  double  _OMEGADOT;         //  [rad/s]
    219223
    … …  
    226230  double  _E5aHS;            //  E5a Health Status
    227231  double  _E5bHS;            //  E5a Health Status
    228   double  _BGD_1_5A;         //  group delay [s] 
    229   double  _BGD_1_5B;         //  group delay [s] 
     232  double  _BGD_1_5A;         //  group delay [s]
     233  double  _BGD_1_5B;         //  group delay [s]
    230234
    231235  double _TOT;               // [s]
Note: See TracChangeset for help on using the changeset viewer.