import time
# initialize the simulation time
# sim_time = SIM.start_time
sim_time = 0
end_time = 20
ts_simulation = 0.01
# initialize elements of the architecture
# wind = windSimulation(SIM.ts_simulation)
# mav = mavDynamics(SIM.ts_simulation)
# ctrl = autopilot(SIM.ts_simulation)
wind = windSimulation(ts_simulation)
mav = mavDynamics(ts_simulation)
ctrl = autopilot(ts_simulation)
# autopilot commands
from message_types.msg_autopilot import msgAutopilot
commands = msgAutopilot()
Va_command = signals(
dc_offset=25.0,
#amplitude=3.0,
amplitude=0.0,
start_time=2.0,
frequency=0.01)
h_command = signals(
dc_offset=100.0,
#amplitude=10.0,
import sys
sys.path.append('..')
import numpy as np
import parameters.aerosonde_parameters as MAV
import tools.signals as sigs
from tools.rotations import Quaternion2Rotation, Quaternion2Euler, Euler2Quaternion
import mav_dynamics
import mavsim_python_chap5_model_coef as chap5
import matplotlib.pyplot as plt
# Define sim parameters
dt = 0.01
uav = mav_dynamics.mavDynamics(dt)
T0 = 0
Tf = 10
n = int(np.floor((Tf - T0) / dt))
alpha = [uav._alpha * 180 / np.pi]
beta = [uav._beta * 180 / np.pi]
phi = [MAV.phi0 * 180 / np.pi]
theta = [MAV.theta0 * 180 / np.pi]
psi = [MAV.psi0 * 180 / np.pi]
gamma = [uav.true_state.gamma * 180 / np.pi]
t_history = [T0]
elev_doublet = sigs.signals(amplitude=0.2,
frequency=1.0,
start_time=1,
duration=1,
import numpy as np
from mav_dynamics import mavDynamics
import integrators as intg
import matplotlib.pyplot as plt
import aerosonde_parameters as MAV
import mavsim_python_chap5_model_coef as ch5
x = ch5.x_trim.flatten()
wind = np.array([[0], [0], [0], [0], [0], [0]])
delta = ch5.u_trim.flatten()
#establish aircraft for simulation
drone = mavDynamics(ch5.Ts)
u1 = drone._forces_moments(delta) #Aerodynamic enviornment
#u2 = Aerodynamics.forces_moments(drone, np.array([0, 0, 0, 0]), MAV)
#----------INTEGRATION
t = 0
dt = 0.0001
n = 500
for i in range(n):
drone.update(delta, wind)
# =============================================================================
# # Convert to numpy arrays, fix dimensions
# x_his = np.array(x_history)
# x_his = x_his.reshape((x_his.shape[0],x_his.shape[1]))
# t_his = np.array(t_history)
#
# #----------PLOTS