ENH: added basic propagator

Project.toml

@@ -8,6 +8,7 @@ Dierckx = "39dd38d3-220a-591b-8e3c-4c3a8c710a94"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 Markdown = "d6f4376e-aef5-505a-96c1-9c027394607a"
 Optim = "429524aa-4258-5aef-a3af-852621145aeb"
+OrdinaryDiffEq = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed"
 Revise = "295af30f-e4ad-537b-8983-00126c2a3abe"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 

src/models/Models.jl

@@ -2,6 +2,7 @@ module Models
 
 import Base: +, -, *, isapprox, convert, copy, show, push!, getindex, setindex!
 using LinearAlgebra
+using OrdinaryDiffEq
 using FlightMechanics
 
 export
@@ -76,7 +77,9 @@ SixDOFEulerFixedMass, SixDOFQuaternionFixedMass, SixDOFECEFQuaternionFixedMass,
 SixDOFAeroEulerFixedMass,
 get_state_equation, get_state_equation_ode_wrapper, get_x, get_x_count, get_state,
 # Results Store
-ResultsStore
+ResultsStore,
+# Propagator
+propagate_timestep, propagate
 
 include("attitude.jl")
 include("position.jl")
@@ -96,4 +99,5 @@ include("aerodynamics.jl")
 include("aircraft.jl")
 include("dynamic_systems.jl")
 include("results_store.jl")
+include("propagator.jl")
 end

src/models/fcs.jl

@@ -139,3 +139,13 @@ function get_controls(cs::StickPedalsLeverStream, t)
     lev = get_value(cs.thrust_lever_stream, t)
     return StickPedalsLeverControls(slon, slat, ped, lev)
 end
+
+
+function convert(::Type{ControlsStream}, c::StickPedalsLeverControls)
+    StickPedalsLeverStream(
+        ConstantInput(get_stick_lon(c)),
+        ConstantInput(get_stick_lat(c)),
+        ConstantInput(get_pedals(c)),
+        ConstantInput(get_thrust_lever(c)),
+        )
+end

src/models/propagator.jl

@@ -0,0 +1,97 @@
+# TODO: DOC
+function propagate_timestep(
+    ac, controls, env, state, aerostate, tspan, dynamic_system, solver; 
+    solver_options=Dict(),
+    )
+
+    # Controls, Forces, moments, mass, inertia and gyro effects are assumed
+    # to be constant for the time step
+    ac = calculate_aircraft(ac, controls, aerostate, state, get_gravity(env))
+
+    # Unpack properties to match OrdinaryDiffEq interface
+    ac_mass_props = get_mass_props(ac)
+    ac_pfm = get_pfm(ac)
+    h = get_gyro_effects(get_propulsion(ac))
+
+    mass = get_mass(ac_mass_props)
+    inertia = get_inertia(ac_mass_props)
+    forces = get_forces(ac_pfm)
+    moments = get_moments(ac_pfm)
+
+    p = [mass, inertia, forces, moments, h]
+
+    x0 = get_x(dynamic_system, state)
+
+    # Create ODE problem
+    prob = ODEProblem(
+        get_state_equation_ode_wrapper(dynamic_system),
+        x0,
+        tspan,
+        p,
+    )
+    sol = solve(prob, solver, save_everystep = false, save_start = false; solver_options...)
+
+    if ~(sol.retcode == :Success)
+        error("Propagate time step did not converge from $(tspan[1]) to $(tspan[2])")
+    end
+
+    dynamic_system.x[:] = sol.u[1]
+    # TODO: is it possible to obtain this from solve?
+    dynamic_system.x_dot[:] = get_state_equation(dynamic_system)(sol.u[1], p...)
+
+    # Obtain state from sol
+    state = convert(state, dynamic_system)
+
+    # As the position has changed, environment changes (must be updated)
+    # and aircraft changes (must be updated)
+    # Calculate environment and aerostate for the current state
+    env = calculate_environment(env, get_position(state))
+    aerostate = AeroState(state, env)
+    # Calculate aircraft
+    ac = calculate_aircraft(ac, controls, aerostate, state, get_gravity(env))
+
+    return sol, ac, env, state, aerostate
+end
+
+
+
+function propagate(
+    ac, env, state, controls_stream, tini, tfin, dt, dynamic_system, solver;
+    solver_options=Dict()
+    )
+
+    # Prepare initial time step
+    t0 = tini
+    t1 = t0 + dt
+
+    # Create results object
+    aerostate = AeroState(state, env)
+    results = ResultsStore(tini, ac, env, state, aerostate)
+
+    while t1 < tfin + 0.5*dt
+        # Get controls
+        controls = get_controls(controls_stream, t0)
+
+        tspan = (t0, t1)
+        sol, ac, env, state, aerostate = propagate_timestep(
+            ac,
+            controls,
+            env,
+            state,
+            aerostate,
+            tspan,
+            dynamic_system,
+            solver;
+            solver_options=solver_options,
+        )
+
+        # Store
+        push!(results, t1, ac, env, state, aerostate)
+
+        # Prepare for next time step
+        t0 = t1
+        t1 = t1 + dt
+    end
+
+    return results
+end

src/models/results_store.jl

@@ -1,19 +1,20 @@
 struct ResultsStore
     t :: Array{T, 1} where T <: Number
     ac :: Array{T, 1} where T <: Aircraft
-    fcs :: Array{T, 1} where T <: FCS
     env :: Array{T, 1} where T <: Environment
     state :: Array{T, 1} where T <: State
     aerostate :: Array{T, 1} where T <: AeroState
 end
 
-ResultsStore(t::Number, ac::Aircraft, fcs::FCS, env::Environment, state::State, aerostate::AeroState) = ResultsStore([t], [ac], [fcs], [env], [state], [aerostate])
+
+function ResultsStore(t::Number, ac::Aircraft, env::Environment, state::State, aerostate::AeroState)
+    ResultsStore([t], [ac], [env], [state], [aerostate])
+end
 
 
-function push!(results::ResultsStore, t, ac, fcs, env, state, aerostate)
+function push!(results::ResultsStore, t, ac, env, state, aerostate)
     push!(results.t, t)
     push!(results.ac, ac)
-    push!(results.fcs, fcs)
     push!(results.env, env)
     push!(results.state, state)
     push!(results.aerostate, aerostate)
@@ -25,9 +26,8 @@ function getindex(results::ResultsStore, ii)
 end
 
 function set_index!(results::ResultsStore, value, ii)
-    results.t[ii] = value[0]
-    results.ac[ii] = value[1]
-    results.fcs[ii] = value[2]
+    results.t[ii] = value[1]
+    results.ac[ii] = value[2]
     results.env[ii] = value[3]
     results.state[ii] = value[4]
     results.aerostate[ii] = value[5]

test/models/propagate_constant_inputs.jl

@@ -0,0 +1,108 @@
+using LinearAlgebra
+using OrdinaryDiffEq
+using FlightMechanics.Aircrafts
+using FlightMechanics.Models
+
+
+# Choose aircraft
+ac = F16()
+# Choose Flight Control System
+fcs = F16FCS()
+# Choose dynamic system
+dynamic_system = SixDOFEulerFixedMass()
+# Choose solver
+solver = RK4()
+solver_options = Dict()
+
+# Initilizations
+h = 1000.0 * M2FT
+tas = 250 * KT2MS
+psi = 0.0  # rad
+gamma = 0.0
+turn_rate = 0.0
+
+pos = EarthPosition(0, 0, -h)
+
+# Initilize environment
+env = Environment(pos, atmos = "ISAF16", wind = "NoWind", grav = "const")
+
+att = Attitude(-1, 0.1, 0.0)
+
+# Initilize FCS
+controls_0 = StickPedalsLeverControls(0.0, 0.5, 0.5, 0.8)
+
+# Trim a/c
+ac, aerostate, state, controls = steady_state_trim(
+    ac,
+    controls_0,
+    env,
+    tas,
+    pos,
+    psi,
+    gamma,
+    turn_rate,
+    0.0,
+    0.0,
+    show_trace = false,
+    )
+
+controls_stream = convert(ControlsStream, controls)
+
+times_to_test = [0.1, 1.0, 10.0, 100.0]  # seconds
+
+@testset "Simulation time $t s" for t=times_to_test
+    tini = 0.0
+    tfin = t
+    dt = 0.01
+
+    results = propagate(
+        ac,
+        env,
+        state,
+        controls_stream,
+        tini,
+        tfin,
+        dt,
+        dynamic_system,
+        solver;
+        solver_options=solver_options
+        )
+
+    @test isapprox(
+        results.state[1].attitude,
+        results.state[end].attitude,
+    )
+
+    @test isapprox(
+        results.state[1].velocity,
+        results.state[end].velocity,
+    )
+
+    @test isapprox(
+        results.state[1].angular_velocity,
+        results.state[end].angular_velocity,
+        atol=1e-11
+    )
+
+    @test isapprox(
+        results.state[1].acceleration,
+        results.state[end].acceleration,
+        atol=1e-10
+    )
+
+    @test isapprox(
+        results.state[1].angular_acceleration,
+        results.state[end].angular_acceleration,
+        atol=1e-12
+    )
+
+    @test isapprox(
+        get_height(results.state[1]),
+        get_height(results.state[end]),
+        )
+
+    @test isapprox(
+        norm(get_xyz_earth(results.state[1])[1:2] - get_xyz_earth(results.state[end])[1:2]),
+        tas * (tfin - tini),
+        )
+end

test/runtests.jl

@@ -17,6 +17,7 @@ using FlightMechanics
 @testset "aerostate" begin include("models/aero_state.jl") end
 @testset "dynamic system" begin include("models/dynamic_system.jl") end
 @testset "trimmer" begin include("models/trimmer.jl") end
+@testset "propagate_constant_inputs" begin include("models/propagate_constant_inputs.jl") end
 
 @testset "ac: c310" begin include("aircrafts/c310.jl") end
 @testset "ac: f16" begin include("aircrafts/f16.jl") end