subroutine SGP4( IFLAG, TSINCE, & M0, NODE0, OMEGA0, E0, I0, N0, BSTAR, PV, ISTAT ) ! ! Version of 1980-11-03 ! ! Modern Fortran, B. Knapp, 1999-08-05 ! implicit none ! ! Input real*8 TSINCE, M0, NODE0, OMEGA0, E0, I0, N0, BSTAR ! Output real*8 PV(6) integer ISTAT !Returned non-zero if user should call SDP4 instead ! ! Input/Output integer IFLAG !Set to 1 on input for new element set ! ! ! WGS-72 physical and geopotential constants real*8 J2, J3, J4 parameter (J2=1.082616D-3, J3=-0.253881D-5, J4=-1.65597E-6) real*8 KE parameter (KE=0.743669161D-1) real*8 CK2, A3OVK2, CK4 parameter (CK2=.5D0*J2, A3OVK2=-J3/CK2, CK4=-.375D0*J4) ! ! Control parameters real*8 KMPER, SIMPHT, Q0, S0, S1, EPS parameter (KMPER=6378.135D0, SIMPHT=220.D0/KMPER, Q0=120.D0/KMPER, & S0=20.D0/KMPER, S1=78.D0/KMPER, EPS=1.D-6) ! ! Misc. constants real*8 TOTHRD, TWOPI parameter (TOTHRD=2.D0/3.D0, TWOPI=2.D0*3.141592653589793D0) ! ! Static variables real*8 PERIGE, MDOT, OMGDOT, N0DOT, NODCF, C1, C4, C5, & T2COF, T3COF, T4COF, T5COF, OMGCOF, ETA, DELM0, SINM0, & A0DP, N0DP, LCOF, MCOF, AYCOF, X3THM1, X1MTH2, X7THM1, & COSI0, SINI0, D2, D3, D4 save PERIGE, MDOT, OMGDOT, N0DOT, NODCF, C1, C4, C5, & T2COF, T3COF, T4COF, T5COF, OMGCOF, ETA, DELM0, SINM0, & A0DP, N0DP, LCOF, MCOF, AYCOF, X3THM1, X1MTH2, X7THM1, & COSI0, SINI0, D2, D3, D4 ! ! Local variables integer I real*8 DELA2, A1, THETA2, BETA02, BETA0, DEL1, A0, DEL0, & S, S4, PINVSQ, XI, ETASQ, EETA, PSISQ, COEF, COEF1, & C3, THETA4, TEMP0, TEMP1, TEMP2, TEMP3, HDOT1, C1SQ real*8 OMEGA, MP, NODE, TEMPA, TEMPE, TEMPL, TEMPF, A, E, N, AB, & AXN, AYN, CAPU, EPW, EPWNEW, SINEPW, COSEPW, ECOSE, ESINE, & ELSQ, TEMPS, PL, R, U, RDOT, RFDOT, BETAL, COSU, SINU, & COS2U, SIN2U, RK, UK, NODEK, IK, RDOTK, RFDOTK, COSUK, SINUK, & COSIK, SINIK, COSNOK, SINNOK, MX, MY, UX, UY, UZ, VX, VY, VZ ! ! ! Initialize output do I=1,6 PV(I)=0.D0 enddo ! if (IFLAG .ne. 0) then ! Recover original mean motion (N0DP) and semimajor axis (A0DP) ! from input elements A1 = (KE/N0)**TOTHRD COSI0 = cos(I0) THETA2 = COSI0**2 X3THM1 = 3.D0*THETA2-1.D0 BETA02 = 1.D0-E0**2 BETA0 = sqrt(BETA02) DELA2 = 1.5D0*CK2*X3THM1/(BETA0*BETA02) DEL1 = DELA2/A1**2 A0 = A1*(1.D0-DEL1*(1.D0/3.D0+DEL1*(1.D0+134.D0/81.D0*DEL1))) DEL0 = DELA2/A0**2 N0DP = N0/(1.D0+DEL0) ! ! User should call SDP4 if period is greater than 225 min if (TWOPI/N0DP .ge. 225.D0) then ISTAT = 1 return endif ! Initialization for new element set A0DP = A0/(1.D0-DEL0) PERIGE = A0DP*(1.D0-E0)-1.D0 S = min(max(S0,PERIGE-S1),S1) !S0 <= S <= S1 S4 = 1.D0+S PINVSQ = 1.D0/(A0DP*BETA02)**2 XI = 1.D0/(A0DP-S4) ETA = A0DP*XI*E0 ETASQ = ETA**2 EETA = E0*ETA PSISQ = abs(1.D0-ETASQ) COEF = ((Q0-S)*XI)**4 COEF1 = COEF/(sqrt(PSISQ)*PSISQ**3) C1 = BSTAR*COEF1*N0DP* & (A0DP*(1.D0+1.5D0*ETASQ+EETA*(4.D0+ETASQ))+ & 0.75D0*CK2*XI/PSISQ*X3THM1*(8.D0+3.D0*ETASQ*(8.D0+ETASQ))) SINI0 = sin(I0) C3 = COEF*XI*A3OVK2*N0DP*SINI0/E0 X1MTH2 = 1.D0-THETA2 C4 = 2.D0*N0DP*COEF1*A0DP*BETA02*(ETA* & (2.D0+.5D0*ETASQ)+E0*(.5D0+2.D0*ETASQ)-2.D0*CK2*XI/ & (A0DP*PSISQ)*(-3.D0*X3THM1*(1.D0-2.D0*EETA+ETASQ* & (1.5D0-.5D0*EETA))+.75D0*X1MTH2*(2.D0*ETASQ-EETA* & (1.D0+ETASQ))*cos(2.D0*OMEGA0))) C5 = 2.D0*COEF1*A0DP*BETA02* & (1.D0+2.75D0*(ETASQ+EETA)+EETA*ETASQ) THETA4 = THETA2**2 TEMP1 = 3.D0*CK2*PINVSQ*N0DP TEMP2 = TEMP1*CK2*PINVSQ TEMP3 = 1.25D0*CK4*PINVSQ**2*N0DP MDOT = N0DP+.5D0*TEMP1*BETA0*X3THM1+.0625D0*TEMP2*BETA0* & (13.D0-78.D0*THETA2+137.D0*THETA4) OMGDOT = -.5D0*TEMP1*(1.D0-5.D0*THETA2)+ & 0.0625D0*TEMP2*(7.D0-114.D0*THETA2+ & 395.D0*THETA4)+TEMP3*(3.D0-36.D0*THETA2+49.D0*THETA4) HDOT1 = -TEMP1*COSI0 N0DOT = HDOT1+(.5D0*TEMP2*(4.D0-19.D0*THETA2)+2.D0*TEMP3* & (3.D0-7.D0*THETA2))*COSI0 OMGCOF = BSTAR*C3*cos(OMEGA0) MCOF = -TOTHRD*COEF*BSTAR/EETA NODCF = 3.5D0*BETA02*HDOT1*C1 T2COF = 1.5D0*C1 LCOF = .125D0*A3OVK2*SINI0*(3.D0+5.D0*COSI0)/(1.D0+COSI0) AYCOF = .25D0*A3OVK2*SINI0 DELM0 = (1.D0+ETA*cos(M0))**3 SINM0 = sin(M0) X7THM1 = 7.D0*THETA2-1.D0 ! For perigee less than 220 kilometers, the equations are ! truncated to linear variation in sqrt A and quadratic ! variation in mean anomaly. Also, the C3 term, the ! delta OMEGA term, and the delta M term are dropped. if (PERIGE .ge. SIMPHT) then C1SQ = C1**2 D2 = 4.D0*A0DP*XI*C1SQ TEMP0 = D2*XI*C1/3.D0 D3 = (17.D0*A0DP+S4)*TEMP0 D4 = .5D0*TEMP0*A0DP*XI*(221.D0*A0DP+31.D0*S4)*C1 T3COF = D2+2.D0*C1SQ T4COF = .25D0*(3.D0*D3+C1*(12.D0*D2+10.D0*C1SQ)) T5COF = .2D0*(3.D0*D4+12.D0*C1*D3+6.D0*D2**2+ & 15.D0*C1SQ*(2.D0*D2+C1SQ)) endif IFLAG = 0 endif ! Update for secular gravity and atmospheric drag MP = M0+MDOT*TSINCE OMEGA = OMEGA0+OMGDOT*TSINCE NODE = NODE0+(N0DOT+NODCF*TSINCE)*TSINCE TEMPE = C4*TSINCE TEMPA = 1.D0-C1*TSINCE TEMPL = T2COF if (PERIGE .ge. SIMPHT) then TEMPF = MCOF*((1.D0+ETA*cos(MP))**3-DELM0)+OMGCOF*TSINCE MP = MP+TEMPF OMEGA = OMEGA-TEMPF TEMPE = TEMPE+C5*(sin(MP)-SINM0) TEMPA = TEMPA-(D2+(D3+D4*TSINCE)*TSINCE)*TSINCE**2 TEMPL = TEMPL+(T3COF+(T4COF+T5COF*TSINCE)*TSINCE)*TSINCE endif A = A0DP*TEMPA**2 N = KE/sqrt(A**3) E = E0-TEMPE*BSTAR TEMPL = TEMPL*TSINCE**2 ! Long period periodics AXN = E*cos(OMEGA) AB = A*(1.D0-E**2) AYN = AYCOF/AB+E*sin(OMEGA) ! Solve Kepler's equation CAPU = mod(LCOF*AXN/AB+MP+OMEGA+N0DP*TEMPL,TWOPI) if (CAPU .lt. 0.D0) CAPU = CAPU+TWOPI EPWNEW = CAPU do I=1,10 EPW = EPWNEW SINEPW = sin(EPW) COSEPW = cos(EPW) ESINE = AXN*SINEPW-AYN*COSEPW ECOSE = AXN*COSEPW+AYN*SINEPW EPWNEW = (CAPU+ESINE-EPW)/(1.D0-ECOSE)+EPW if (abs(EPWNEW-EPW) .le. EPS) goto 1 enddo 1 continue ! Short period preliminary quantities ELSQ = AXN**2+AYN**2 TEMPS = 1.D0-ELSQ PL = A*TEMPS R = A*(1.D0-ECOSE) RDOT = KE*sqrt(A)*ESINE/R RFDOT = KE*sqrt(PL)/R BETAL = sqrt(TEMPS) TEMP3 = ESINE/(1.D0+BETAL) COSU = (COSEPW-AXN+AYN*TEMP3)*A/R SINU = (SINEPW-AYN-AXN*TEMP3)*A/R U = atan2(SINU,COSU) IF (U .lt. 0.D0) U = U+TWOPI SIN2U = 2.*SINU*COSU COS2U = 2.*COSU**2-1.D0 TEMP1 = CK2/PL TEMP2 = TEMP1/PL ! Update for short periodics RK = R*(1.D0-1.5D0*TEMP2*BETAL*X3THM1)+.5D0*TEMP1*X1MTH2*COS2U UK = U-.25D0*TEMP2*X7THM1*SIN2U NODEK = NODE+1.5D0*TEMP2*COSI0*SIN2U IK = I0+1.5D0*TEMP2*COSI0*SINI0*COS2U RDOTK = RDOT-N*TEMP1*X1MTH2*SIN2U RFDOTK = RFDOT+N*TEMP1*(X1MTH2*COS2U+1.5D0*X3THM1) ! Orientation vectors SINUK = sin(UK) COSUK = cos(UK) SINIK = sin(IK) COSIK = cos(IK) SINNOK = sin(NODEK) COSNOK = cos(NODEK) MX = -SINNOK*COSIK MY = COSNOK*COSIK UX = MX*SINUK+COSNOK*COSUK UY = MY*SINUK+SINNOK*COSUK UZ = SINIK*SINUK VX = MX*COSUK-COSNOK*SINUK VY = MY*COSUK-SINNOK*SINUK VZ = SINIK*COSUK ! Position and Velocity PV(1) = RK*UX PV(2) = RK*UY PV(3) = RK*UZ PV(4) = RDOTK*UX+RFDOTK*VX PV(5) = RDOTK*UY+RFDOTK*VY PV(6) = RDOTK*UZ+RFDOTK*VZ ! ISTAT = 0 return end