Changeset 3145 in ntrip


Ignore:
Timestamp:
Mar 25, 2011, 5:13:32 PM (11 years ago)
Author:
mervart
Message:
 
Location:
trunk/BNS
Files:
2 edited

Legend:

Unmodified
Added
Removed
  • trunk/BNS/bnseph.cpp

    r3144 r3145  
    2525
    2626#define PI          3.1415926535898
     27const static double c_vel = 299792458.0;
    2728
    2829using namespace std;
     
    151152  else if (prn.indexOf('G') != -1) {
    152153    eph = new t_ephGPS();
     154    numlines = 8;
     155  }
     156  else if (prn.indexOf('E') != -1) {
     157    eph = new t_ephGal();
    153158    numlines = 8;
    154159  }
     
    503508  // -----------------------
    504509  //  xc(4) -= 4.442807633e-10 * _e * sqrt(a0) *sin(E);
    505   const static double c_vel = 299792458.0;
    506510  xc(4) -= 2.0 * DotProduct(xc.Rows(1,3),vv) / c_vel / c_vel;
    507511}
     
    766770  }
    767771}
     772
     773// Compute Galileo Satellite Position
     774////////////////////////////////////////////////////////////////////////////
     775void t_ephGal::position(int GPSweek, double GPSweeks, ColumnVector& xc,
     776                        ColumnVector& vv) const {
     777
     778  static const double secPerWeek = 7 * 86400.0;
     779  static const double omegaEarth = 7292115.1467e-11;
     780  static const double gmWGS      = 398.6005e12;
     781
     782  xc.ReSize(4); xc = 0.0;
     783  vv.ReSize(3); vv = 0.0;
     784
     785  double a0 = _sqrt_A * _sqrt_A;
     786  if (a0 == 0) {
     787    return;
     788  }
     789
     790  double n0 = sqrt(gmWGS/(a0*a0*a0));
     791  double tk = GPSweeks - _TOE;
     792  if (GPSweek != _GPSweek) { 
     793    tk += (GPSweek - _GPSweek) * secPerWeek;
     794  }
     795  double n  = n0 + _Delta_n;
     796  double M  = _M0 + n*tk;
     797  double E  = M;
     798  double E_last;
     799  do {
     800    E_last = E;
     801    E = M + _e*sin(E);
     802  } while ( fabs(E-E_last)*a0 > 0.001 );
     803  double v      = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) );
     804  double u0     = v + _omega;
     805  double sin2u0 = sin(2*u0);
     806  double cos2u0 = cos(2*u0);
     807  double r      = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
     808  double i      = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
     809  double u      = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
     810  double xp     = r*cos(u);
     811  double yp     = r*sin(u);
     812  double OM     = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
     813                   omegaEarth*_TOE;
     814 
     815  double sinom = sin(OM);
     816  double cosom = cos(OM);
     817  double sini  = sin(i);
     818  double cosi  = cos(i);
     819  xc[0] = xp*cosom - yp*cosi*sinom;
     820  xc[1] = xp*sinom + yp*cosi*cosom;
     821  xc[2] = yp*sini;                 
     822 
     823  double tc = GPSweeks - _TOC;
     824  if (GPSweek != _GPSweek) { 
     825    tc += (GPSweek - _GPSweek) * secPerWeek;
     826  }
     827  xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
     828
     829  // Velocity
     830  // --------
     831  double tanv2 = tan(v/2);
     832  double dEdM  = 1 / (1 - _e*cos(E));
     833  double dotv  = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
     834               * dEdM * n;
     835  double dotu  = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
     836  double dotom = _OMEGADOT - omegaEarth;
     837  double doti  = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
     838  double dotr  = a0 * _e*sin(E) * dEdM * n
     839                + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
     840  double dotx  = dotr*cos(u) - r*sin(u)*dotu;
     841  double doty  = dotr*sin(u) + r*cos(u)*dotu;
     842
     843  vv[0]  = cosom   *dotx  - cosi*sinom   *doty      // dX / dr
     844           - xp*sinom*dotom - yp*cosi*cosom*dotom   // dX / dOMEGA
     845                       + yp*sini*sinom*doti;        // dX / di
     846
     847  vv[1]  = sinom   *dotx  + cosi*cosom   *doty
     848           + xp*cosom*dotom - yp*cosi*sinom*dotom
     849                          - yp*sini*cosom*doti;
     850
     851  vv[2]  = sini    *doty  + yp*cosi      *doti;
     852
     853  // Relativistic Correction
     854  // -----------------------
     855  //  xc(4) -= 4.442807633e-10 * _e * sqrt(a0) *sin(E);
     856  xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / c_vel / c_vel;
     857}
     858
     859// Read Galileo Ephemeris
     860////////////////////////////////////////////////////////////////////////////
     861t_irc t_ephGal::read(const QStringList& lines) {
     862
     863  return success;
     864}
     865
     866// build up RTCM3 for Galileo
     867////////////////////////////////////////////////////////////////////////////
     868int t_ephGal::RTCM3(unsigned char *buffer) {
     869
     870  return 0;
     871}
  • trunk/BNS/bnseph.h

    r3045 r3145  
    115115};
    116116
     117class t_ephGal : public t_eph {
     118 public:
     119  t_ephGal() { }
     120  virtual ~t_ephGal() {}
     121  virtual BNS::t_irc read(const QStringList& lines);
     122  virtual void position(int GPSweek, double GPSweeks, ColumnVector& xc,
     123                        ColumnVector& vv) const;
     124  virtual int  IOD() const { return static_cast<int>(_IODnav); }
     125  virtual int  RTCM3(unsigned char *);
     126
     127 private:
     128  double  _IODnav;             
     129  double  _TOC;              //  [s]   
     130  double  _TOE;              //  [s]   
     131  double  _clock_bias;       //  [s]   
     132  double  _clock_drift;      //  [s/s] 
     133  double  _clock_driftrate;  //  [s/s^2]
     134  double  _Crs;              //  [m]   
     135  double  _Delta_n;          //  [rad/s]
     136  double  _M0;               //  [rad] 
     137  double  _Cuc;              //  [rad] 
     138  double  _e;                //         
     139  double  _Cus;              //  [rad] 
     140  double  _sqrt_A;           //  [m^0.5]
     141  double  _Cic;              //  [rad] 
     142  double  _OMEGA0;           //  [rad] 
     143  double  _Cis;              //  [rad] 
     144  double  _i0;               //  [rad] 
     145  double  _Crc;              //  [m]   
     146  double  _omega;            //  [rad] 
     147  double  _OMEGADOT;         //  [rad/s]
     148  double  _IDOT;             //  [rad/s]
     149  double  _BGD_1_5A;         //  group delay [s]
     150  int     _SISA;             //  Signal In Space Accuracy
     151  int     _E5aHS;            //  E5a Health Status
     152
     153};
     154
    117155class t_bnseph : public QThread {
    118156 Q_OBJECT
Note: See TracChangeset for help on using the changeset viewer.