Ok, now open loop is working, sc mass changed to state, and other updates
This commit is contained in:
Connor
@@ -3,234 +3,80 @@ export prop
|
||||
"""
|
||||
Maximum ΔV that a spacecraft can impulse for a given single time step
|
||||
"""
|
||||
function max_ΔV(duty_cycle::T,
|
||||
function max_ΔV(duty_cycle::Float64,
|
||||
num_thrusters::Int,
|
||||
max_thrust::T,
|
||||
tf::T,
|
||||
t0::T,
|
||||
mass::S) where {T <: Real, S <: Real}
|
||||
max_thrust::Float64,
|
||||
tf::Float64,
|
||||
t0::Float64,
|
||||
mass::T) where T <: Real
|
||||
return duty_cycle*num_thrusters*max_thrust*(tf-t0)/mass
|
||||
end
|
||||
|
||||
"""
|
||||
This function propagates the spacecraft forward in time 1 Sim-Flanagan step (of variable length of time),
|
||||
applying a thrust in the center.
|
||||
"""
|
||||
function prop_one(thrust_unit::Vector{<:Real},
|
||||
state::Vector{<:Real},
|
||||
duty_cycle::Float64,
|
||||
num_thrusters::Int,
|
||||
max_thrust::Float64,
|
||||
mass::T,
|
||||
mass_flow_rate::Float64,
|
||||
μ::Float64,
|
||||
time::Float64) where T<:Real
|
||||
|
||||
ΔV = max_ΔV(duty_cycle, num_thrusters, max_thrust, time, 0., mass) * thrust_unit
|
||||
halfway = laguerre_conway(state, μ, time/2) + [0., 0., 0., ΔV...]
|
||||
return laguerre_conway(halfway, μ, time/2), mass - mass_flow_rate*norm(thrust_unit)*time
|
||||
|
||||
end
|
||||
|
||||
"""
|
||||
A convenience function for using spacecraft. Note that this function outputs a sc instead of a mass
|
||||
"""
|
||||
function prop_one(ΔV_unit::Vector{T},
|
||||
state::Vector{S},
|
||||
function prop_one(ΔV_unit::Vector{<:Real},
|
||||
state::Vector{<:Real},
|
||||
craft::Sc,
|
||||
μ::Float64,
|
||||
time::Float64) where {T <: Real,S <: Real}
|
||||
state, mass = prop_one(ΔV_unit, state, craft.duty_cycle, craft.num_thrusters, craft.max_thrust,
|
||||
craft.mass, craft.mass_flow_rate, μ, time)
|
||||
return state, Sc(mass, craft.mass_flow_rate, craft.max_thrust, craft.num_thrusters, craft.duty_cycle)
|
||||
time::Float64)
|
||||
|
||||
for direction in ΔV_unit
|
||||
if abs(direction) > 1.0
|
||||
println(direction)
|
||||
error("ΔV is impossibly high")
|
||||
end
|
||||
end
|
||||
ΔV = max_ΔV(craft.duty_cycle, craft.num_thrusters, craft.max_thrust, time, 0., state[7]) * ΔV_unit
|
||||
halfway = laguerre_conway(state, μ, time/2) + [zeros(3); ΔV]
|
||||
final = laguerre_conway(halfway, μ, time/2)
|
||||
return [final; state[7] - craft.mass_flow_rate*norm(ΔV_unit)*time]
|
||||
|
||||
end
|
||||
|
||||
|
||||
"""
|
||||
This propagates over a given time period, with a certain number of intermediate steps
|
||||
The propagator function
|
||||
"""
|
||||
function prop(ΔVs::Matrix{T},
|
||||
state::Vector{Float64},
|
||||
duty_cycle::Float64,
|
||||
num_thrusters::Int,
|
||||
max_thrust::Float64,
|
||||
mass::Float64,
|
||||
mass_flow_rate::Float64,
|
||||
craft::Sc,
|
||||
μ::Float64,
|
||||
time::Float64) where T <: Real
|
||||
|
||||
if size(ΔVs)[2] != 3 throw(ErrorException("ΔV input is wrong size")) end
|
||||
n = size(ΔVs)[i]
|
||||
|
||||
for i in 1:n
|
||||
state, mass = prop_one(ΔVs[i,:], state, duty_cycle, num_thrusters, max_thrust, mass,
|
||||
mass_flow_rate, μ, time/n)
|
||||
end
|
||||
|
||||
return state, mass
|
||||
|
||||
end
|
||||
|
||||
"""
|
||||
The same function, using Scs
|
||||
"""
|
||||
function prop(ΔVs::Matrix{T},
|
||||
state::Vector{S},
|
||||
craft::Sc,
|
||||
μ::Float64,
|
||||
time::Float64) where {T <: Real, S <: Real}
|
||||
|
||||
if size(ΔVs)[2] != 3 throw(ErrorException("ΔV input is wrong size")) end
|
||||
n = size(ΔVs)[1]
|
||||
|
||||
x_states = [state[1]]
|
||||
y_states = [state[2]]
|
||||
z_states = [state[3]]
|
||||
dx_states = [state[4]]
|
||||
dy_states = [state[5]]
|
||||
dz_states = [state[6]]
|
||||
masses = [craft.mass]
|
||||
x_states = Vector{T}()
|
||||
y_states = Vector{T}()
|
||||
z_states = Vector{T}()
|
||||
dx_states = Vector{T}()
|
||||
dy_states = Vector{T}()
|
||||
dz_states = Vector{T}()
|
||||
masses = Vector{T}()
|
||||
|
||||
push!(x_states, state[1])
|
||||
push!(y_states, state[2])
|
||||
push!(z_states, state[3])
|
||||
push!(dx_states, state[4])
|
||||
push!(dy_states, state[5])
|
||||
push!(dz_states, state[6])
|
||||
push!(masses, state[7])
|
||||
|
||||
for i in 1:n
|
||||
state, craft = prop_one(ΔVs[i,:], state, craft, μ, time/n)
|
||||
state = prop_one(ΔVs[i,:], state, craft, μ, time/n)
|
||||
push!(x_states, state[1])
|
||||
push!(y_states, state[2])
|
||||
push!(z_states, state[3])
|
||||
push!(dx_states, state[4])
|
||||
push!(dy_states, state[5])
|
||||
push!(dz_states, state[6])
|
||||
push!(masses, craft.mass)
|
||||
push!(masses, state[7])
|
||||
if state[7] < craft.dry_mass
|
||||
println(state[7])
|
||||
error("Mass is too low")
|
||||
end
|
||||
end
|
||||
|
||||
return [x_states, y_states, z_states, dx_states, dy_states, dz_states], masses, state
|
||||
return [x_states, y_states, z_states, dx_states, dy_states, dz_states, masses], state
|
||||
|
||||
end
|
||||
|
||||
function prop_nlsolve(ΔVs::Matrix{T},
|
||||
state::Vector{S},
|
||||
craft::Sc,
|
||||
μ::Float64,
|
||||
time::Float64) where {T <: Real, S <: Real}
|
||||
|
||||
n = size(ΔVs)[1]
|
||||
try
|
||||
for i in 1:n
|
||||
state, craft = prop_one(ΔVs[i,:], state, craft, μ, time/n)
|
||||
end
|
||||
return state
|
||||
catch
|
||||
return [0., 0., 0., 0., 0., 0.]
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
|
||||
function prop_simple(ΔVs::AbstractMatrix,
|
||||
state::AbstractVector,
|
||||
craft::Sc,
|
||||
μ::Float64,
|
||||
time::Float64)
|
||||
|
||||
if size(ΔVs)[2] != 3 throw(ErrorException("ΔV input is wrong size")) end
|
||||
n = size(ΔVs)[1]
|
||||
|
||||
for i in 1:n
|
||||
state, craft = prop_one(ΔVs[i,:], state, craft, μ, time/n)
|
||||
end
|
||||
|
||||
return state
|
||||
|
||||
end
|
||||
|
||||
function prop_one_simple(Tx, Ty, Tz, x, y, z, dx, dy, dz, t, μ)
|
||||
|
||||
# perform laguerre_conway once
|
||||
r0_mag = √(x^2 + y^2 + z^2)
|
||||
v0_mag = √(dx^2 + dy^2 + dz^2)
|
||||
σ0 = ([x, y, z] ⋅ [dx, dy, dz])/√(μ)
|
||||
a = 1 / ( 2/r0_mag - v0_mag^2/μ )
|
||||
coeff = 1 - r0_mag/a
|
||||
|
||||
if a > 0 # Elliptical
|
||||
ΔM = ΔE_new = √(μ) / sqrt(a^3) * t/2
|
||||
ΔE = 1000
|
||||
while abs(ΔE - ΔE_new) > 1e-10
|
||||
ΔE = ΔE_new
|
||||
F = ΔE - ΔM + σ0 / √(a) * (1-cos(ΔE)) - coeff * sin(ΔE)
|
||||
dF = 1 + σ0 / √(a) * sin(ΔE) - coeff * cos(ΔE)
|
||||
d2F = σ0 / √(a) * cos(ΔE) + coeff * sin(ΔE)
|
||||
sign = dF >= 0 ? 1 : -1
|
||||
ΔE_new = ΔE - 5*F / ( dF + sign * √(abs((5-1)^2*dF^2 - 5*(5-1)*F*d2F )))
|
||||
end
|
||||
F = 1 - a/r0_mag * (1-cos(ΔE))
|
||||
G = a * σ0/ √(μ) * (1-cos(ΔE)) + r0_mag * √(a) / √(μ) * sin(ΔE)
|
||||
r = a + (r0_mag - a) * cos(ΔE) + σ0 * √(a) * sin(ΔE)
|
||||
Ft = -√(a)*√(μ) / (r*r0_mag) * sin(ΔE)
|
||||
Gt = 1 - a/r * (1-cos(ΔE))
|
||||
else # Hyperbolic or Parabolic
|
||||
ΔN = √(μ) / sqrt(-a^3) * t/2
|
||||
ΔH = 0
|
||||
ΔH_new = t/2 < 0 ? -1 : 1
|
||||
while abs(ΔH - ΔH_new) > 1e-10
|
||||
ΔH = ΔH_new
|
||||
F = -ΔN - ΔH + σ0 / √(-a) * (cos(ΔH)-1) + coeff * sin(ΔH)
|
||||
dF = -1 + σ0 / √(-a) * sin(ΔH) + coeff * cos(ΔH)
|
||||
d2F = σ0 / √(-a) * cos(ΔH) + coeff * sin(ΔH)
|
||||
sign = dF >= 0 ? 1 : -1
|
||||
ΔH_new = ΔH - 5*F / ( dF + sign * √(abs((5-1)^2*dF^2 - 5*(5-1)*F*d2F )))
|
||||
end
|
||||
F = 1 - a/r0_mag * (1-cos(ΔH))
|
||||
G = a * σ0/ √(μ) * (1-cos(ΔH)) + r0_mag * √(-a) / √(μ) * sin(ΔH)
|
||||
r = a + (r0_mag - a) * cos(ΔH) + σ0 * √(-a) * sin(ΔH)
|
||||
Ft = -√(-a)*√(μ) / (r*r0_mag) * sin(ΔH)
|
||||
Gt = 1 - a/r * (1-cos(ΔH))
|
||||
end
|
||||
|
||||
# add the thrust vector
|
||||
x,y,z,dx,dy,dz = [F*[x,y,z] + G*[dx,dy,dz]; Ft*[x,y,z] + Gt*[dx,dy,dz] + [Tx, Ty, Tz]]
|
||||
|
||||
#perform again
|
||||
r0_mag = √(x^2 + y^2 + z^2)
|
||||
v0_mag = √(dx^2 + dy^2 + dz^2)
|
||||
σ0 = ([x, y, z] ⋅ [dx, dy, dz])/√(μ)
|
||||
a = 1 / ( 2/r0_mag - v0_mag^2/μ )
|
||||
coeff = 1 - r0_mag/a
|
||||
|
||||
if a > 0 # Elliptical
|
||||
ΔM = ΔE_new = √(μ) / sqrt(a^3) * t/2
|
||||
ΔE = 1000
|
||||
while abs(ΔE - ΔE_new) > 1e-10
|
||||
ΔE = ΔE_new
|
||||
F = ΔE - ΔM + σ0 / √(a) * (1-cos(ΔE)) - coeff * sin(ΔE)
|
||||
dF = 1 + σ0 / √(a) * sin(ΔE) - coeff * cos(ΔE)
|
||||
d2F = σ0 / √(a) * cos(ΔE) + coeff * sin(ΔE)
|
||||
sign = dF >= 0 ? 1 : -1
|
||||
ΔE_new = ΔE - 5*F / ( dF + sign * √(abs((5-1)^2*dF^2 - 5*(5-1)*F*d2F )))
|
||||
end
|
||||
F = 1 - a/r0_mag * (1-cos(ΔE))
|
||||
G = a * σ0/ √(μ) * (1-cos(ΔE)) + r0_mag * √(a) / √(μ) * sin(ΔE)
|
||||
r = a + (r0_mag - a) * cos(ΔE) + σ0 * √(a) * sin(ΔE)
|
||||
Ft = -√(a)*√(μ) / (r*r0_mag) * sin(ΔE)
|
||||
Gt = 1 - a/r * (1-cos(ΔE))
|
||||
else # Hyperbolic or Parabolic
|
||||
ΔN = √(μ) / sqrt(-a^3) * t/2
|
||||
ΔH = 0
|
||||
ΔH_new = t/2 < 0 ? -1 : 1
|
||||
while abs(ΔH - ΔH_new) > 1e-10
|
||||
ΔH = ΔH_new
|
||||
F = -ΔN - ΔH + σ0 / √(-a) * (cos(ΔH)-1) + coeff * sin(ΔH)
|
||||
dF = -1 + σ0 / √(-a) * sin(ΔH) + coeff * cos(ΔH)
|
||||
d2F = σ0 / √(-a) * cos(ΔH) + coeff * sin(ΔH)
|
||||
sign = dF >= 0 ? 1 : -1
|
||||
ΔH_new = ΔH - 5*F / ( dF + sign * √(abs((5-1)^2*dF^2 - 5*(5-1)*F*d2F )))
|
||||
end
|
||||
F = 1 - a/r0_mag * (1-cos(ΔH))
|
||||
G = a * σ0/ √(μ) * (1-cos(ΔH)) + r0_mag * √(-a) / √(μ) * sin(ΔH)
|
||||
r = a + (r0_mag - a) * cos(ΔH) + σ0 * √(-a) * sin(ΔH)
|
||||
Ft = -√(-a)*√(μ) / (r*r0_mag) * sin(ΔH)
|
||||
Gt = 1 - a/r * (1-cos(ΔH))
|
||||
end
|
||||
|
||||
return [F*[x,y,z] + G*[dx,dy,dz]; Ft*[x,y,z] + Gt*[dx,dy,dz]]
|
||||
|
||||
end
|
||||
Reference in New Issue
Block a user