Changeset 884 in ntrip for trunk/BNS/bnsutils.cpp
- Timestamp:
- May 8, 2008, 3:36:12 PM (16 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/BNS/bnsutils.cpp
r861 r884 108 108 } 109 109 110 // Satellite Position computed using broadcast ephemeris111 ////////////////////////////////////////////////////////////////////////////112 void satellitePosition(int GPSweek, double GPSweeks, const gpsEph* ep,113 double& X, double& Y, double& Z, double& dt,114 double& vX, double& vY, double& vZ) {115 116 const static double secPerWeek = 7 * 86400.0;117 const static double omegaEarth = 7292115.1467e-11;118 const static double gmWGS = 398.6005e12;119 120 X = Y = Z = dt = 0.0;121 122 double a0 = ep->sqrt_A * ep->sqrt_A;123 if (a0 == 0) {124 return;125 }126 127 double n0 = sqrt(gmWGS/(a0*a0*a0));128 double tk = GPSweeks - ep->TOE;129 if (GPSweek != ep->GPSweek) {130 tk += (GPSweek - ep->GPSweek) * secPerWeek;131 }132 double n = n0 + ep->Delta_n;133 double M = ep->M0 + n*tk;134 double E = M;135 double E_last;136 do {137 E_last = E;138 E = M + ep->e*sin(E);139 } while ( fabs(E-E_last)*a0 > 0.001 );140 double v = 2.0*atan( sqrt( (1.0 + ep->e)/(1.0 - ep->e) )*tan( E/2 ) );141 double u0 = v + ep->omega;142 double sin2u0 = sin(2*u0);143 double cos2u0 = cos(2*u0);144 double r = a0*(1 - ep->e*cos(E)) + ep->Crc*cos2u0 + ep->Crs*sin2u0;145 double i = ep->i0 + ep->IDOT*tk + ep->Cic*cos2u0 + ep->Cis*sin2u0;146 double u = u0 + ep->Cuc*cos2u0 + ep->Cus*sin2u0;147 double xp = r*cos(u);148 double yp = r*sin(u);149 double OM = ep->OMEGA0 + (ep->OMEGADOT - omegaEarth)*tk -150 omegaEarth*ep->TOE;151 152 double sinom = sin(OM);153 double cosom = cos(OM);154 double sini = sin(i);155 double cosi = cos(i);156 X = xp*cosom - yp*cosi*sinom;157 Y = xp*sinom + yp*cosi*cosom;158 Z = yp*sini;159 160 double tc = GPSweeks - ep->TOC;161 if (GPSweek != ep->GPSweek) {162 tc += (GPSweek - ep->GPSweek) * secPerWeek;163 }164 dt = ep->clock_bias + ep->clock_drift*tc + ep->clock_driftrate*tc*tc165 - 4.442807633e-10 * ep->e * sqrt(a0) *sin(E);166 167 // Velocity168 // --------169 double tanv2 = tan(v/2);170 double dEdM = 1 / (1 - ep->e*cos(E));171 double dotv = sqrt((1.0 + ep->e)/(1.0 - ep->e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)172 * dEdM * n;173 double dotu = dotv + (-ep->Cuc*sin2u0 + ep->Cus*cos2u0)*2*dotv;174 double dotom = ep->OMEGADOT - omegaEarth;175 double doti = ep->IDOT + (-ep->Cic*sin2u0 + ep->Cis*cos2u0)*2*dotv;176 double dotr = a0 * ep->e*sin(E) * dEdM * n177 + (-ep->Crc*sin2u0 + ep->Crs*cos2u0)*2*dotv;178 double dotx = dotr*cos(u) - r*sin(u)*dotu;179 double doty = dotr*sin(u) + r*cos(u)*dotu;180 181 vX = cosom *dotx - cosi*sinom *doty // dX / dr182 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA183 + yp*sini*sinom*doti; // dX / di184 185 vY = sinom *dotx + cosi*cosom *doty186 + xp*cosom*dotom - yp*cosi*sinom*dotom187 - yp*sini*cosom*doti;188 189 vZ = sini *doty + yp*cosi *doti;190 }191 192 110 // Transformation xyz --> radial, along track, out-of-plane 193 111 //////////////////////////////////////////////////////////////////////////// … … 202 120 rsw(3) = DotProduct(xyz, cross) / cross.norm_Frobenius(); 203 121 } 122 123 // Fourth order Runge-Kutta numerical integrator for ODEs 124 //////////////////////////////////////////////////////////////////////////// 125 ColumnVector rungeKutta4( 126 double xi, // the initial x-value 127 const ColumnVector& yi, // vector of the initial y-values 128 double dx, // the step size for the integration 129 ColumnVector (*der)(double x, const ColumnVector& y) 130 // A pointer to a function that computes the 131 // derivative of a function at a point (x,y) 132 ) { 133 134 ColumnVector k1 = der(xi , yi ) * dx; 135 ColumnVector k2 = der(xi+dx/2.0, yi+k1/2.0) * dx; 136 ColumnVector k3 = der(xi+dx/2.0, yi+k2/2.0) * dx; 137 ColumnVector k4 = der(xi+dx , yi+k3 ) * dx; 138 139 ColumnVector yf = yi + k1/6.0 + k2/3.0 + k3/3.0 + k4/6.0; 140 141 return yf; 142 }
Note:
See TracChangeset
for help on using the changeset viewer.