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