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)