119 lines
2.9 KiB
Julia
119 lines
2.9 KiB
Julia
export oe_to_rθh, rθh_to_xyz, oe_to_xyz, xyz_to_oe, conv_T
|
||
|
||
function oe_to_rθh(oe::Vector,μ::Real) :: Vector
|
||
|
||
a,e,i,Ω,ω,ν = oe
|
||
return [a*(1-e^2)/(1+e*cos(ν)),
|
||
0,
|
||
0,
|
||
(μ/sqrt(μ*a*(1-e^2)))*e*sin(ν),
|
||
(μ/sqrt(μ*a*(1-e^2)))*(1+e*cos(ν)),
|
||
0]
|
||
|
||
end
|
||
|
||
function rθh_to_xyz(rθh_vec::Vector,oe::Vector)
|
||
|
||
a,e,i,Ω,ω,ν = oe
|
||
θ = ω+ν
|
||
cΩ,sΩ,ci,si,cθ,sθ = cos(Ω),sin(Ω),cos(i),sin(i),cos(θ),sin(θ)
|
||
DCM = [cΩ*cθ-sΩ*ci*sθ -cΩ*sθ-sΩ*ci*cθ sΩ*si;
|
||
sΩ*cθ+cΩ*ci*sθ -sΩ*sθ+cΩ*ci*cθ -cΩ*si;
|
||
si*sθ si*cθ ci]
|
||
DCM = kron(Matrix(I,2,2),DCM)
|
||
return DCM*rθh_vec
|
||
|
||
end
|
||
|
||
function oe_to_xyz(oe::Vector,μ::Real)
|
||
|
||
return rθh_to_xyz(oe_to_rθh(oe,μ),oe)
|
||
|
||
end
|
||
|
||
function xyz_to_oe(cart_vec::Vector,μ::Real)
|
||
|
||
r_xyz, v_xyz = cart_vec[1:3],cart_vec[4:6]
|
||
r = norm(r_xyz)
|
||
h_xyz = cross(r_xyz,v_xyz) #km^2/s
|
||
h = norm(h_xyz) #km^2/s
|
||
ξ = dot(v_xyz,v_xyz)/2 - μ/r #km^2 s^-2
|
||
a = -μ/(2ξ) #km
|
||
e_xyz = cross(v_xyz,h_xyz)/μ - r_xyz/r
|
||
e = norm(e_xyz)
|
||
if h_xyz[3]/h < 1.
|
||
i = acos(h_xyz[3]/h) #rad
|
||
else
|
||
i = acos(1.)
|
||
end
|
||
n_xyz = cross([0,0,1],h_xyz)
|
||
if dot(n_xyz,[1,0,0])/norm(n_xyz) < 1.
|
||
Ω = acos(dot(n_xyz,[1,0,0])/norm(n_xyz))
|
||
else
|
||
Ω = acos(1.)
|
||
end
|
||
if dot(n_xyz,e_xyz)/(norm(n_xyz)*e) < 1.
|
||
ω = acos(dot(n_xyz,e_xyz)/(norm(n_xyz)*e))
|
||
else
|
||
ω = acos(1.)
|
||
end
|
||
if abs((dot(r_xyz,e_xyz))/(r*norm(e_xyz))) < 1.
|
||
ν = acos((dot(r_xyz,e_xyz))/(r*norm(e_xyz)))
|
||
else
|
||
ν = acos(1.)
|
||
end
|
||
Ω = dot(n_xyz,[0,1,0]) > 0. ? Ω : -Ω
|
||
ω = dot(e_xyz,[0,0,1]) > 0. ? ω : -ω
|
||
ν = dot(r_xyz,v_xyz) > 0. ? ν : -ν
|
||
return [a,e,i,Ω,ω,ν]
|
||
|
||
end
|
||
|
||
"""
|
||
Converts a series of thrust vectors from R,Θ,H frame to cartesian
|
||
"""
|
||
function conv_T(Tm::Vector{Float64},
|
||
Ta::Vector{Float64},
|
||
Tb::Vector{Float64},
|
||
init_state::Vector{Float64},
|
||
m::Float64,
|
||
craft::Sc,
|
||
time::Float64,
|
||
μ::Float64)
|
||
|
||
Txs = Float64[]
|
||
Tys = Float64[]
|
||
Tzs = Float64[]
|
||
n::Int = length(Tm)
|
||
|
||
for i in 1:n
|
||
mag, α, β = Tm[i], Ta[i], Tb[i]
|
||
if mag > 1 || mag < 0
|
||
@error "ΔV input is too high: $mag"
|
||
elseif α > π || α < -π
|
||
@error "α angle is incorrect: $α"
|
||
elseif β > π/2 || β < -π/2
|
||
@error "β angle is incorrect: $β"
|
||
end
|
||
end
|
||
|
||
state = init_state
|
||
for i in 1:n
|
||
mag, α, β = Tm[i], Ta[i], Tb[i]
|
||
thrust_rθh = mag * [cos(β)*sin(α), cos(β)*cos(α), sin(β)]
|
||
_,_,i,Ω,ω,ν = xyz_to_oe(state, μ)
|
||
θ = ω+ν
|
||
cΩ,sΩ,ci,si,cθ,sθ = cos(Ω),sin(Ω),cos(i),sin(i),cos(θ),sin(θ)
|
||
DCM = [cΩ*cθ-sΩ*ci*sθ -cΩ*sθ-sΩ*ci*cθ sΩ*si;
|
||
sΩ*cθ+cΩ*ci*sθ -sΩ*sθ+cΩ*ci*cθ -cΩ*si;
|
||
si*sθ si*cθ ci ]
|
||
Tx, Ty, Tz = DCM*thrust_rθh
|
||
|
||
state = prop_one([Tx, Ty, Tz], state, craft, μ, time/n)[1]
|
||
push!(Txs, Tx)
|
||
push!(Tys, Ty)
|
||
push!(Tzs, Tz)
|
||
end
|
||
return Txs, Tys, Tzs
|
||
end
|