Changeset 798 in ntrip for trunk/BNS/bnsutils.cpp
- Timestamp:
- Apr 8, 2008, 5:15:07 PM (16 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/BNS/bnsutils.cpp
r775 r798 24 24 25 25 #include "bnsutils.h" 26 #include "bnseph.h" 26 27 27 28 using namespace std; … … 92 93 currTime.msec() / 1000.0; 93 94 } 95 96 // 97 //////////////////////////////////////////////////////////////////////////// 98 void satellitePosition(double GPSweeks, const gpsEph* ep, 99 double& X, double& Y, double& Z, double& dt) { 100 101 const static double omegaEarth = 7292115.1467e-11; 102 const static double gmWGS = 398.6005e12; 103 104 X = Y = Z = dt = 0.0; 105 106 double a0 = ep->sqrt_A * ep->sqrt_A; 107 if (a0 == 0) { 108 return; 109 } 110 111 double n0 = sqrt(gmWGS/(a0*a0*a0)); 112 double tk = GPSweeks - ep->TOE; 113 double n = n0 + ep->Delta_n; 114 double M = ep->M0 + n*tk; 115 double E = M; 116 double E_last; 117 do { 118 E_last = E; 119 E = M + ep->e*sin(E); 120 } while ( fabs(E-E_last)*a0 > 0.001 ); 121 double v = 2.0*atan( sqrt( (1.0 + ep->e)/(1.0 - ep->e) )*tan( E/2 ) ); 122 double u0 = v + ep->omega; 123 double sin2u0 = sin(2*u0); 124 double cos2u0 = cos(2*u0); 125 double r = a0*(1 - ep->e*cos(E)) + ep->Crc*cos2u0 + ep->Crs*sin2u0; 126 double i = ep->i0 + ep->IDOT*tk + ep->Cic*cos2u0 + ep->Cis*sin2u0; 127 double u = u0 + ep->Cuc*cos2u0 + ep->Cus*sin2u0; 128 double xp = r*cos(u); 129 double yp = r*sin(u); 130 double OM = ep->OMEGA0 + (ep->OMEGADOT - omegaEarth)*tk - 131 omegaEarth*ep->TOE; 132 133 double sinom = sin(OM); 134 double cosom = cos(OM); 135 double sini = sin(i); 136 double cosi = cos(i); 137 X = xp*cosom - yp*cosi*sinom; 138 Y = xp*sinom + yp*cosi*cosom; 139 Z = yp*sini; 140 141 double tc = GPSweeks - ep->TOC; 142 dt = ep->clock_bias + ep->clock_drift*tc + ep->clock_driftrate*tc*tc 143 - 4.442807633e-10 * ep->e * sqrt(a0) *sin(E); 144 }
Note:
See TracChangeset
for help on using the changeset viewer.