Changeset 3145 in ntrip for trunk/BNS/bnseph.cpp
 Timestamp:
 Mar 25, 2011, 5:13:32 PM (11 years ago)
 File:

 1 edited
Legend:
 Unmodified
 Added
 Removed

trunk/BNS/bnseph.cpp
r3144 r3145 25 25 26 26 #define PI 3.1415926535898 27 const static double c_vel = 299792458.0; 27 28 28 29 using namespace std; … … 151 152 else if (prn.indexOf('G') != 1) { 152 153 eph = new t_ephGPS(); 154 numlines = 8; 155 } 156 else if (prn.indexOf('E') != 1) { 157 eph = new t_ephGal(); 153 158 numlines = 8; 154 159 } … … 503 508 //  504 509 // xc(4) = 4.442807633e10 * _e * sqrt(a0) *sin(E); 505 const static double c_vel = 299792458.0;506 510 xc(4) = 2.0 * DotProduct(xc.Rows(1,3),vv) / c_vel / c_vel; 507 511 } … … 766 770 } 767 771 } 772 773 // Compute Galileo Satellite Position 774 //////////////////////////////////////////////////////////////////////////// 775 void 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.1467e11; 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(EE_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.442807633e10 * _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 //////////////////////////////////////////////////////////////////////////// 861 t_irc t_ephGal::read(const QStringList& lines) { 862 863 return success; 864 } 865 866 // build up RTCM3 for Galileo 867 //////////////////////////////////////////////////////////////////////////// 868 int t_ephGal::RTCM3(unsigned char *buffer) { 869 870 return 0; 871 }
Note:
See TracChangeset
for help on using the changeset viewer.