[Fortran] 纯文本查看 复制代码
SUBROUTINE cal_foot_point(obs_prn,fileid,obs_year,obs_month,
. obs_day,obs_hour, obs_minute,obs_second,x,y,z)
implicit none
type cansu1
real*8 a0,a1,a2,a3
end type
type cansu2
integer prn,toc,year,month,day,hour,minute
real*8 second
real*8 svcb,svcd,svcdr
end type
type(cansu1)::p1
type(cansu2)::p2
real*8 :: IODE,crs,delta_n,M0,
. cuc,e,cus,sqrt_a,
. toe,cic,omega,cis,
. i0,crc,w,omegadot,
. idot,l2code,gpsweek,l2pdataflag,
. svaccuracy,svhealth, tgd,iodc,
. transtime,fitinterval,
. svcb,svcd,svcdr,obs_second,
. n0,
n
. m,
. es,
. fs,
. t,
. r,
. u,
. i,
. x0,y0,
. x,y,z
!sec,omega_dot,fi0
character*80 buffer
integer :: j
integer :: nav_prn,obs_prn,nav_year,obs_year,nav_month,obs_month,
. nav_day,obs_day,nav_hour,obs_hour,
. obs_minute,nav_minute
real*8,parameter:: omega_e=7.2921151467d-5
character*15 inputfile,outputfile,outputffile
character*20 c
! integer, parameter ::fileid=101
logical alive
integer ::err=0,fileid
buffer=' '
x=0d0
y=0d0
z=0d0
do while (.true.)
read(fileid,"(A79)") buffer
if (buffer(61:61)=='E') exit
end do
do while(.true.)
if (err/=0) exit
read(fileid,"(a)",iostat=err) buffer
if (buffer(1:2)/=" ") then
read(buffer,"(i2)") nav_prn
if (nav_prn==obs_prn) then
read(buffer,"(3x,i2)") nav_year
if (nav_year==obs_year) then
read(buffer,"(6x,i2)") nav_month
if (nav_month==obs_month) then
read(buffer,"(9x,i2)") nav_day
if (nav_day==obs_day) then
read(buffer,"(12x,2i2)") nav_hour,nav_minute
c if (abs(nav_hour-obs_hour)==1 .or.
c . nav_hour==obs_hour) then
if (abs(nav_hour*60+nav_minute)-
. (obs_hour*60+obs_minute)<70) then
read(buffer,"(I2,1x,I2.2,4(1X,I2),
. F5.1,3D19.12)") p2%prn,
. p2%year,p2%month,p2%day,p2%hour,
. p2%minute,p2%second,svcb,svcd,svcdr
do j=1 ,7
read(fileid,"(3X,4F19.12)")
. p1%a0,p1%a1,p1%a2,p1%a3
select case (j)
case (1)
IODE=p1%a0
crs=p1%a1
delta_n=p1%a2
M0=p1%a3
case(2)
cuc=p1%a0
e=p1%a1
cus=p1%a2
sqrt_a=p1%a3
case(3)
toe=p1%a0
cic=p1%a1
omega=p1%a2
cis=p1%a3
case(4)
i0=p1%a0
crc=p1%a1
w=p1%a2
omegadot=p1%a3
case (5)
idot=p1%a0
l2code=p1%a1
gpsweek=p1%a2
l2pdataflag=p1%a3
case (6)
svaccuracy=p1%a0
svhealth=p1%a1
tgd=p1%a2
iodc=p1%a3
case (7)
transtime=p1%a0
fitinterval=p1%a1
end select
end do
call g_gps(obs_year,obs_month,obs_day,obs_hour,
. obs_minute,obs_second,t)
call calu_fs(svcb,svcd,svcdr,cuc,cus,crc,crs,cic,
. cis,i0,e,sqrt_A,delta_n,m0,toe,t,w,r,u,i)
call cal_orit_scoord(r,u,x0,y0)
call cal_sjdju(omega_e,omegadot,omega,t,
. toe,i,x0,y0,x,y,z)
rewind(unit=fileid)
return
else
rewind(unit=fileid)
return
end if
end if
end if
end if
end if
end if
end do
rewind(unit=fileid)
! close(unit=fileid)
end
SUBROUTINE calu_fs(a0,a1,a2,fcuc,fcus,fcrc,fcrs,fcic,fcis, fi0,
. fe,fsqrt_A,fdelta_n,fm0,ftoe,ft,fw,fr,fu,fi)
implicit none
real*8,parameter:: pi=3.1415926535897932D0
real*8 es,n0,m,n,fsqrt_A,fdelta_n,fm0,ft,ftoe,fs,fe,fw,fu,
. delta_u,delta_r,delta_i,fcuc,fcus,fcrc,fcrs,fcic,fcis,fi0,
. fr,fi,a0,a1,a2,i,j,cf,sf
real,parameter ::GM=3.986005e+14
n0=sqrt(GM)/fsqrt_A**3
n=n0+fdelta_n
m=fm0+n*(ft-ftoe)
es=m+fe*sin(m)
do while (abs(es-j)>1.0d-12)
j=es
es=m+fe*sin(es)
end do
cf=(cos(es)-fe)/(1-fe*cos(es))
sf=sqrt(1-fe**2)*sin(es)/(1-fe*cos(es))
if (cf>0 .and. sf>0) then
fs=atan(sqrt(1-fe**2)*sin(es)/(cos(es)-fe))
else if (cf<0) then
fs=pi+atan(sqrt(1-fe**2)*sin(es)/(cos(es)-fe))
else if (cf>0 .and. sf<0) then
fs=2*pi+atan(sqrt(1-fe**2)*sin(es)/(cos(es)-fe))
end if
c fs=sqrt((1+fe)/(1-fe))*tan(es/2)
c fs=2*ATAN(fs)
fu=fw+fs
delta_u=fcuc*cos(2*fu)+fcus*sin(2*fu)
delta_r=fcrc*cos(2*fu)+fcrs*sin(2*fu)
delta_i=fcic*cos(2*fu)+fcis*sin(2*fu)
fu=fu+delta_u
fr=fsqrt_A**2*(1-fe*cos(es))+delta_r
fi=fi0+delta_i
return
end
SUBROUTINE cal_sjdju(omega_e,omega_dot,omega,t,toe,i,x0,y0,x,y,z)
real*8 omega_dot,omega,t,toe,i,x0,y0,x,y,z,l,omega_e
l=omega+(omega_dot-omega_e)*t-omega_dot*toe
x=x0*cos(l)-y0*sin(l)*cos(i)
y=x0*sin(l)+y0*cos(l)*cos(i)
z=y0*sin(i)
return
end
SUBROUTINE g_gps(y,m,d,h,min,sec,ftow)
implicit none
real*8 sec,ftow,jd,hh
integer y,m,d,h,min,fwn
if (y>0 .and. y<80) then
y=y+2000
else
y=y+1900
end if
if (m<=2) then
y=y-1
m=m+12
else
y=y
m=m
end if
jd=INT(365.25*y)+INT(30.6001*(m+1))+d+1720981.5
fwn=INT((jd-2444244.5)/7)
ftow=(jd-2444244.5-7*fwn)*86400.0+h*3600+min*60+sec
if (m>12) then
y=y+1
m=m-12
else
y=y
m=m
end if
if (y>2000 ) then
y=y-2000
else
y=y-1900
end if
return
end
SUBROUTINE cal_orit_scoord(fr,fu,x0,y0)
real*8 fr,fu,x0,y0
x0=fr*cos(fu)
y0=fr*sin(fu)
z0=0
return
end