def _adapt_full_state_to_dynamic_system(self, full_state):
pos = EarthPosition(full_state.position.x_earth,
full_state.position.y_earth,
full_state.position.height,
full_state.position.lat, full_state.position.lon)
att = EulerAttitude(full_state.attitude.theta, full_state.attitude.phi,
full_state.attitude.psi)
vel = BodyVelocity(full_state.velocity.u, full_state.velocity.v,
full_state.velocity.w, att)
ang_vel = BodyAngularVelocity(full_state.angular_vel.p,
full_state.angular_vel.q,
full_state.angular_vel.r, att)
accel = BodyAcceleration(full_state.acceleration.u_dot,
full_state.acceleration.v_dot,
full_state.acceleration.w_dot, att)
ang_accel = BodyAngularAcceleration(full_state.angular_accel.p_dot,
full_state.angular_accel.q_dot,
full_state.angular_accel.r_dot,
att)
full_state = AircraftState(pos, att, vel, ang_vel, accel, ang_accel)
return full_state
from systems.odeint_system import OdeIntSystem
from state.aircraft_state import AircraftState
from state.velocity import BodyVelocity
from state.attitude import EulerAttitude
from simulation.simtools import Target
from simulation.flightsim import TakeoffSim
#from simulation.simulator import Simulation
from ground_station.ground_station import GeodeticWaypoint, FlightPlan
from simulation.simcraft.glider import ElectricGlider
from simulation.simcraft.rocket import Missile
from simulation.autopilot import PDAutopilot, LeastSquaresAutopilot
GROUND_LEVEL = 5000.0
FLIGHT_LEVEL = 6000.0
init_pos = EarthPosition(0.0, 0.0, FLIGHT_LEVEL)
INITIAL_HEADING = 0.0
TRIM_SPEED = 55.0
atmosphere = ISA1976()
gravity = VerticalConstant()
#wind = NoWind()
wind = Wind()
environment = Environment(atmosphere, gravity, wind)
aircraft = Missile()
controls0 = {
'delta_elevator': -0.01,
def __init__(self, pos=np.array([1000.0, -1000.0, -6000.0]), velocity=(0.0, 20.0, 0.0)):
self.initial_position = EarthPosition(*pos)
# attitude = EulerAttitude(0, 0, 0)
# self.velocity = NEDVelocity(*velocity, attitude)
self.velocity = np.array(velocity)
from state.position import EarthPosition, GeodeticPosition
from ground_station.ground_station import World, EarthWaypoint, GeodeticWaypoint
# start location
gs_loc = (45.805, -108.6, 5000)
# define path
loc0 = (45.806, -108.606, 5000)
hdg0 = 0.0
loc1 = (45.675, -108.7715, 5000)
hdg1 = 0.54
wps = [GeodeticWaypoint(loc0, hdg0), # x_earth=0.0, y_earth=0.0),
GeodeticWaypoint(loc1, hdg1)]
aircraft_hdg = hdg0
aircraft_loc = EarthPosition(0.0, 0.0, 6000, lat=45.8055, lon=-108.605)
w = World(gs_loc, wps, aircraft_loc, aircraft_hdg)
print('ground station at: ', w.gs_position.lat, w.gs_position.lon, w.gs_position.height)
print('ground station at: ', w.gs_position.x_earth, w.gs_position.y_earth, w.gs_position.height)
for i, pt in enumerate(w.waypoints):
print('wp ', i+1)
print('lat: ', pt.lat, ' lon: ', pt.lon, 'hdg: ', pt.heading)
print('x: ', pt.x_earth, ' y: ', pt.y_earth, 'z: ', pt.z_earth)
#plots.flight_map(w)
"""
- make world
-make aircraft-environment
def state(self, x=0.0, y=0.0, theta=0.0, phi=0.0, psi=0.0, u=0.0, v=0.0, w=0.0):
pos = EarthPosition(x, y, 1000)
att = EulerAttitude(theta, phi, psi)
vel = BodyVelocity(u=u, v=v, w=w, attitude=att)
return AircraftState(pos, att, vel, angular_vel=None,
acceleration=None, angular_accel=None)