Changeset 3145 in ntrip for trunk/BNS/bnseph.cpp


Ignore:
Timestamp:
Mar 25, 2011, 5:13:32 PM (11 years ago)
Author:
mervart
Message:
 
File:
1 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}
Note: See TracChangeset for help on using the changeset viewer.