def compute_tf_model(
trim_state,
trim_input): #computes the transfer function linearized at trim
# trim values
mav = mav_dynamics(SIM.ts_simulation)
mav.set_state(trim_state)
mav._update_velocity_data()
Va_trim = mav._Va
alpha_trim = mav._alpha
beta_trim = mav._beta
phi, theta_trim, psi = Quaternion2Euler(trim_state[6:10])
delta_t_trim = trim_input[3, 0]
_dT_dVa = dT_dVa(Va_trim, delta_t_trim)
_dT_ddelta_t = dT_ddelta_t(Va_trim, delta_t_trim)
# define transfer function constants
a_phi1 = -0.25 * MAV.rho * mav._Va * MAV.S_wing * (MAV.b**2) * MAV.C_p_p
a_phi2 = .5 * MAV.rho * (mav._Va**2) * MAV.S_wing * MAV.b * MAV.C_p_delta_a
a_theta1 = -MAV.rho * mav._Va * (MAV.c**
2) * MAV.S_wing * MAV.C_m_q / (4 * MAV.Jy)
a_theta2 = -MAV.rho * (mav._Va**2) * MAV.c * MAV.S_wing * MAV.C_m_alpha / (
2 * MAV.Jy)
a_theta3 = MAV.rho * (
mav._Va**2) * MAV.c * MAV.S_wing * MAV.C_m_delta_e / (2 * MAV.Jy)
a_V1 = (MAV.rho * Va_trim * MAV.S_wing /
MAV.mass) * (MAV.C_D_0 + MAV.C_D_alpha * alpha_trim +
MAV.C_D_delta_e) - _dT_dVa / MAV.mass
a_V2 = _dT_ddelta_t / MAV.mass
a_V3 = MAV.gravity * np.cos(theta_trim - alpha_trim)
return Va_trim, alpha_trim, beta_trim, theta_trim, a_phi1, a_phi2, a_theta1, a_theta2, a_theta3, a_V1, a_V2, a_V3
def dT_ddelta_t(mav, Va, delta_t):
# returns the derivative of motor thrust with respect to delta_t
h = 0.001
f_f, _ = mav.calcMotorDynamics(Va, delta_t + h)
f_b, _ = mav.calcMotorDynamics(Va, delta_t - h)
dThrust = (f_f - f_b) / (2.0 * h)
return dThrust
if __name__ == "__main__":
from chap4.mav_dynamics import mav_dynamics
from chap5.trim import compute_trim
dyn = mav_dynamics(0.02)
Va = 25.
gamma = 0. * np.pi / 180.0
trim_state, trim_input = compute_trim(dyn, Va, gamma)
dyn._state = trim_state
# trim_euler = euler_state(trim_state)
# trim_quat = quaternion_state(trim_euler)
# print('trim: \n',trim_state)
# print('euler: \n',trim_euler)
# print('quat: \n',trim_quat)
# A_lat = getALat(dyn,trim_state,trim_input)
# A_lon, B_lon, A_lat, B_lat = compute_ss_model(dyn, trim_state, trim_input)
# print('A_lon: \n',A_lon)
# print('B_lon: \n',B_lon)
from chap4.mav_viewer import mav_viewer
from chap4.data_viewer import data_viewer
from chap4.mav_dynamics import mav_dynamics
from chap4.wind_simulation import wind_simulation
from chap5.trim import compute_trim
from chap5.compute_models import compute_model
from tools.signals import signals
# initialize the visualization
mav_view = mav_viewer() # initialize the mav viewer
data_view = data_viewer() # initialize view of data plots
# initialize elements of the architecture
wind = wind_simulation(SIM.ts_simulation)
mav = mav_dynamics(SIM.ts_simulation)
# use compute_trim function to compute trim state and trim input
Va0 = 25
gamma = 0 #10.0*np.pi/180.
delta0 = np.array([[0], [0], [0],
[0.5]]) # [delta_e, delta_a, delta_r, delta_t]
trim_state, trim_input = compute_trim(mav, delta0, Va0, gamma)
mav.set_state(trim_state) # set the initial state of the mav to the trim state
delta = trim_input # set input to constant trim input
# # compute the state space model linearized about trim
compute_model(trim_state, trim_input)
# this signal will be used to excite modes
input_signal = signals(amplitude=.05, duration=0.01, start_time=2.0)
from chap5.compute_models import compute_ss_model, compute_tf_model
import time
# initialize the visualization
VIDEO = False # True==write video, False==don't write video
mav_view = mav_viewer() # initialize the mav viewer
data_view = data_viewer() # initialize view of data plots
if VIDEO == True:
video = video_writer(video_name="chap5_video.avi",
bounding_box=(0, 0, 1000, 1000),
output_rate=SIM.ts_video)
# initialize elements of the architecture
wind = wind_simulation(SIM.ts_simulation)
mav = mav_dynamics(SIM.ts_simulation,[P.pn0,P.pe0,P.pd0])
# use compute_trim function to compute trim state and trim input
Va = 25.
gamma = 0.*np.pi/180.
trim_state, trim_input = compute_trim(mav, Va, gamma)
# OUTPUT TO FILE
filename = "../chap5/trim_results.py"
file = open(filename,'w')
file.write("import numpy as np\n\n")
# trim state
file.write("trim_state = np.array([")
trim_state.tofile(file,sep=",")
file.write("])\n\n")
# trim inputs