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