def __init__(self, Ts): # steady state wind defined in the inertial frame self._steady_state = np.array([[3.0], [2.0], [0.0]]) #given low altitude and light turbulance Lu = 200 Lv = Lu Lw = 50 sig_u = 1.06 sig_v = sig_u sig_w = 0.7 Va = 0.0 a1 = sig_u * np.sqrt(2 * Va / Lu) a2 = sig_v * np.sqrt(3 * Va / Lv) a3 = sig_v * np.power(Va / Lv, 3.0 / 2.0) a4 = sig_w * np.sqrt(3 * Va / Lw) a5 = sig_w * np.power(Va / Lw, 3.0 / 2.0) b1 = Va / Lu b2 = Va / Lv b3 = Va / Lw self.u_w = transfer_function(num=np.array([[a1]]), den=np.array([[1, b1]]), Ts=Ts) self.v_w = transfer_function(num=np.array([[a2, a3]]), den=np.array([[1, 2 * b2, b2**2.0]]), Ts=Ts) self.w_w = transfer_function(num=np.array([[a4, a5]]), den=np.array([[1, 2 * b3, b3**2.0]]), Ts=Ts) self._Ts = Ts
def __init__(self, Ts): # steady state wind defined in the inertial frame self._steady_state = np.array([0, 0, 0]) #NED sig_u = 1.06 sig_v = sig_u sig_w = 0.7 Va = 20 Lu = 200 Lv = Lu Lw = 50 a1 = sig_u*np.sqrt(2*Va/Lu) a2 = sig_v*np.sqrt(3*Va/Lv) a3 = sig_v*np.sqrt(3*Va/Lv) * Va/(np.sqrt(3)*Lv) a4 = sig_w*np.sqrt(3*Va/Lw) a5 = sig_w*np.sqrt(3*Va/Lw) * Va/(np.sqrt(3)*Lw) b1 = Va/Lu b2 = Va/Lv b3 = Va/Lw self.u_w = transfer_function(num=np.array([[a1]]), den=np.array([[1, b1]]), Ts=Ts) self.v_w = transfer_function(num=np.array([[a2, a3]]), den=np.array([[1, 2*b2, b2**2.0]]), Ts=Ts) self.w_w = transfer_function(num=np.array([[a4, a5]]), den=np.array([[1, 2*b3, b3**2.0]]), Ts=Ts) self._Ts = Ts
def __init__(self, Ts): # steady state wind defined in the inertial frame Va = 20. Lu = 200. Lv = Lu Lw = 50. sigma_u = 2.12 sigma_v = sigma_u sigma_w = 1.4 a1 = sigma_u*np.sqrt(2.0*Va/(np.pi*Lu)) a2 = sigma_v*np.sqrt(3.0*Va/(np.pi*Lv)) a3 = (Va/(np.sqrt(3.0)*Lv))*a2 a4 = sigma_w*np.sqrt(3.0*Va/(np.pi*Lw)) a5 = (Va/(np.sqrt(3.0)*Lw))*a4 b1 = Va/Lu b2 = Va/Lv b3 = Va/Lw self._steady_state = [[1],[-0.5],[0.1]] self.u_w = transfer_function(num=np.array([[a1]]), den=np.array([[1, b1]]), Ts=Ts) self.v_w = transfer_function(num=np.array([[a2, a3]]), den=np.array([[1, 2*b2, b2**2.0]]), Ts=Ts) self.w_w = transfer_function(num=np.array([[a4, a5]]), den=np.array([[1, 2*b3, b3**2.0]]), Ts=Ts) self._Ts = Ts
def __init__(self, Ts): # steady state wind defined in the inertial frame # a1, b1, a2, a3, b2, a4, a5, b3 = pc() Lu, Lv, Lw = 200, 200, 50 sigma_u, sigma_v = 1.06, 1.06 sigma_w = .7 V_a = 20 c_u = sigma_u * root(2 * V_a / Lu / np.pi) c_v = sigma_v * root(3 * V_a / Lv / np.pi) c_w = sigma_w * root(3 * V_a / Lw / np.pi) a1 = c_u b1 = V_a / Lu a2 = c_v a3 = c_v * V_a / Lv / root(3) b2 = V_a / Lv a4 = c_w a5 = c_w * V_a / Lw / root(3) b3 = V_a / Lw self._steady_state = np.array([[0, 0, 0]]) self.u_w = transfer_function(num=np.array([[a1]]), den=np.array([[1, b1]]), Ts=Ts) self.v_w = transfer_function(num=np.array([[a2, a3]]), den=np.array([[1, 2 * b2, b2**2.0]]), Ts=Ts) self.w_w = transfer_function(num=np.array([[a4, a5]]), den=np.array([[1, 2 * b3, b3**2.0]]), Ts=Ts) self._Ts = Ts
def __init__(self, Ts, Va0=0): # steady state wind defined in the inertial frame self._steady_state = np.array([[0], [0], [0]]) #medium altitude, light turbulence (alitutde 600 meters) sigma_u = 1.5 #m/s sigma_v = sigma_u sigma_w = 1.5 #m/s Lw = 533 #m Lu = 533 #m Lv = Lu a1 = sigma_u * np.sqrt(2 * Va0 / (np.pi * Lu)) b1 = Va0 / Lu a2 = sigma_v * np.sqrt(3 * Va0 / (np.pi * Lv)) a3 = a2 * Va0 / (np.sqrt(3) * Lv) b2 = 2 * Va0 / Lv b3 = (Va0 / Lv)**2 a4 = sigma_w * np.sqrt(3 * Va0 / (np.pi * Lw)) a5 = a4 * Va0 / (np.sqrt(3) * Lw) b4 = 2 * Va0 / Lw b5 = (Va0 / Lw)**2 self.u_w = transfer_function(num=np.array([[a1]]), den=np.array([[1, b1]]), Ts=Ts) self.v_w = transfer_function(num=np.array([[a2, a3]]), den=np.array([[1, b2, b3]]), Ts=Ts) self.w_w = transfer_function(num=np.array([[a4, a5]]), den=np.array([[1, b4, b5]]), Ts=Ts) self._Ts = Ts
def __init__(self, Ts): # steady state wind defined in the inertial frame self._steady_state = np.array([[windy.Vw_ss_n], [windy.Vw_ss_e], [windy.Vw_ss_d]]) Va = 25 a1 = windy.sig_u * np.sqrt(2 * Va / (np.pi * windy.L_u)) b1 = Va / windy.L_u a2 = windy.sig_v * np.sqrt(3 * Va / (np.pi * windy.L_v)) a3 = Va / (np.sqrt(3) * windy.L_v) b2 = Va / windy.L_v a4 = windy.sig_w * np.sqrt(3 * Va / (np.pi * windy.L_w)) a5 = Va / (np.sqrt(3) * windy.L_v) b3 = Va / windy.L_w # Dryden Gust Params: self.u_w = transfer_function(num=np.array([[a1]]), den=np.array([[1, b1]]), Ts=Ts) self.v_w = transfer_function(num=np.array([[a2, a3]]), den=np.array([[1, 2 * b2, b2**2.0]]), Ts=Ts) self.w_w = transfer_function(num=np.array([[a4, a5]]), den=np.array([[1, 2 * b3, b3**2.0]]), Ts=Ts) self._Ts = Ts
def __init__(self, Ts): # steady state wind defined in the inertial frame self._steady_state = self.u_w = transfer_function(num=np.array([[a1]]), den=np.array([[1, b1]]), Ts=Ts) self.v_w = transfer_function(num=np.array([[a2, a3]]), den=np.array([[1, 2*b2, b2**2.0]]), Ts=Ts) self.w_w = transfer_function(num=np.array([[a4, a5]]), den=np.array([[1, 2*b3, b3**2.0]]), Ts=Ts) self._Ts = Ts
def __init__(self, ts_control): # instantiate lateral controllers self.roll_from_aileron = pd_control_with_rate(kp=AP.roll_kp, kd=AP.roll_kd, limit=np.radians(45)) self.course_from_roll = pi_control(kp=AP.course_kp, ki=AP.course_ki, Ts=ts_control, limit=np.radians(30)) self.sideslip_from_rudder = pi_control(kp=AP.sideslip_kp, ki=AP.sideslip_ki, Ts=ts_control, limit=np.radians(45)) self.yaw_damper = transfer_function( num=np.array([[AP.yaw_damper_kp, 0]]), den=np.array([[1, 1 / AP.yaw_damper_tau_r]]), Ts=ts_control) # instantiate lateral controllers self.pitch_from_elevator = pd_control_with_rate(kp=AP.pitch_kp, kd=AP.pitch_kd, limit=np.radians(45)) self.altitude_from_pitch = pi_control(kp=AP.altitude_kp, ki=AP.altitude_ki, Ts=ts_control, limit=np.radians(30)) self.airspeed_from_throttle = pi_control(kp=AP.airspeed_throttle_kp, ki=AP.airspeed_throttle_ki, Ts=ts_control, limit=1.0) self.commanded_state = msg_state()
def __init__(self, ts_control): # instantiate lateral controllers self.roll_from_aileron = pdControlWithRate(kp=AP.roll_kp, kd=AP.roll_kd, limit=AP.delta_a_max) self.course_from_roll = piControl(kp=AP.course_kp, ki=AP.course_ki, Ts=ts_control, limit=AP.roll_max) self.yaw_damper = transfer_function( num=np.array([[AP.yaw_damper_kp, 0]]), den=np.array([[1, 1 / AP.yaw_damper_tau_r]]), Ts=ts_control) # instantiate lateral controllers self.pitch_from_elevator = pdControlWithRate(kp=AP.pitch_kp, kd=AP.pitch_kd, limit=AP.delta_e_max) self.altitude_from_pitch = piControl(kp=AP.altitude_kp, ki=AP.altitude_ki, Ts=ts_control, limit=AP.pitch_max) self.airspeed_from_throttle = piControl(kp=AP.airspeed_throttle_kp, ki=AP.airspeed_throttle_ki, Ts=ts_control, limit=AP.throttle_max) self.commanded_state = msg_state()
def __init__(self, Ts): # steady state wind defined in the inertial frame # wn we wd self._steady_state = np.array([[0], [0], [0]]) sigma_u = 1.06 sigma_v = 1.06 sigma_w = 0.7 Lu = 200 Lv = 200 Lw = 50 Va = 25 a1 = sigma_u * math.sqrt(2 * Va / Lu / np.pi) b1 = Va / Lu a2 = sigma_v * math.sqrt(3 * Va / Lv / np.pi) a3 = sigma_v * math.sqrt(3 * Va / Lv / np.pi) * (Va / (math.sqrt(3) * Lv)) b2 = Va / Lv a4 = sigma_w * math.sqrt(3 * Va / Lw / np.pi) a5 = sigma_w * math.sqrt(3 * Va / Lw / np.pi) * (Va / (math.sqrt(3) * Lw)) b3 = Va / Lw self.u_w = transfer_function(num=np.array([[a1]]), den=np.array([[1, b1]]), Ts=Ts) self.v_w = transfer_function(num=np.array([[a2, a3]]), den=np.array([[1, 2 * b2, b2**2.0]]), Ts=Ts) self.w_w = transfer_function(num=np.array([[a4, a5]]), den=np.array([[1, 2 * b3, b3**2.0]]), Ts=Ts) self._Ts = Ts
def __init__(self, ts_control): #Can i just use the PID control class with 0 for certain gain? # instantiate lateral controllers self.roll_from_aileron = pid_control( #was pd_control_with_rate kp=AP.roll_kp, kd=AP.roll_kd, limit_h=np.radians(45), limit_l=-np.radians(45)) self.course_from_roll = pid_control( # was pi kp=AP.course_kp, ki=AP.course_ki, Ts=ts_control, limit_h=np.radians(30), limit_l=-np.radians(30)) self.sideslip_from_rudder = pid_control( # was pi kp=AP.sideslip_kp, ki=AP.sideslip_ki, Ts=ts_control, limit_h=np.radians(45), limit_l=-np.radians(45)) self.yaw_damper = transfer_function( num=np.array([[AP.yaw_damper_kp, 0]]), den=np.array([[1, 1 / AP.yaw_damper_tau_r]]), Ts=ts_control) # instantiate longitudinal controllers self.pitch_from_elevator = pid_control( # was pd_control with rate kp=AP.pitch_kp, kd=AP.pitch_kd, limit_h=np.radians(45), limit_l=-np.radians(45)) self.altitude_from_pitch = pid_control( # was pi kp=AP.altitude_kp, ki=AP.altitude_ki, Ts=ts_control, limit_h=np.radians(30), limit_l=-np.radians(30)) self.airspeed_from_throttle = pid_control( # was pi kp=AP.airspeed_throttle_kp, ki=AP.airspeed_throttle_ki, Ts=ts_control, limit_h=1.0, limit_l=0.0) self.commanded_state = StateMsg()
def compute_tf_model(mav, trim_state, trim_input, display=False): # euler states e_state = euler_state(trim_state) theta = e_state.item(7) # control surface inputs delta_e_star = trim_input[0, 0] delta_t_star = trim_input[3, 0] # additional mav variables Va = mav._Va Va_star = mav._Va Vg = mav._Vg alpha_star = mav._alpha b = MAV.b rho = MAV.rho S = MAV.S_wing mass = MAV.mass c = MAV.c Jy = MAV.Jy g = MAV.gravity # -------------- Transfer Functions -------------- # phi to delta_a a_phi_1 = -0.5 * rho * Va**2 * S * b * MAV.C_p_p * b / (2.0 * Va) a_phi_2 = 0.5 * rho * Va**2 * S * b * MAV.C_p_delta_a T_phi_delta_a = transfer_function(num=np.array([[a_phi_2]]), den=np.array([[1, a_phi_1, 0]]), Ts=Ts) # chi to phi T_chi_phi = transfer_function(num=np.array([[g / Vg]]), den=np.array([[1, 0]]), Ts=Ts) # beta to delta_r a_b_1 = -rho * Va * S / (2 * mass) * MAV.C_Y_beta a_b_2 = rho * Va * S / (2 * mass) * MAV.C_Y_delta_r T_beta_delta_r = transfer_function(num=np.array([[a_b_2]]), den=np.array([[1, a_b_1]]), Ts=Ts) #theta to delta_e a_t_1 = -rho * Va**2 * c * S / (2 * Jy) * MAV.C_m_q * c / (2.0 * Va) a_t_2 = -rho * Va**2 * c * S / (2 * Jy) * MAV.C_m_alpha a_t_3 = rho * Va**2 * c * S / (2 * Jy) * MAV.C_m_delta_e T_theta_delta_e = transfer_function(num=np.array([[a_t_3]]), den=np.array([[1, a_t_1, a_t_2]]), Ts=Ts) # h to theta T_h_theta = transfer_function(num=np.array([[Va]]), den=np.array([[1, 0]]), Ts=Ts) # h to Va T_h_Va = transfer_function(num=np.array([[theta]]), den=np.array([[1, 0]]), Ts=Ts) # Va to delta_t # book approach (non-finite difference) # a_v_1 = rho*Va_star*S/mass*(MAV.C_D_0 + MAV.C_D_alpha*alpha_star + MAV.C_D_delta_e*delta_e_star) + rho*MAV.S_prop/mass*MAV.C_prop*Va_star # a_v_2 = rho*MAV.S_prop/mass*MAV.C_prop*MAV.k_motor**2*delta_t_star # a_v_3 = g # finite difference approach a_v_1 = rho * Va_star * S / mass * ( MAV.C_D_0 + MAV.C_D_alpha * alpha_star + MAV.C_D_delta_e * delta_e_star) - 1.0 / mass * dT_dVa(mav, Va, delta_t_star) a_v_2 = 1.0 / mass * dT_ddelta_t(mav, Va, delta_t_star) a_v_3 = g * np.cos(theta - alpha_star) T_Va_delta_t = transfer_function(num=np.array([[a_v_2]]), den=np.array([[1, a_v_1]]), Ts=Ts) # Va to theta T_Va_theta = transfer_function(num=np.array([[-a_v_3]]), den=np.array([[1, a_v_1]]), Ts=Ts) if display: print("Calculated Transfer Function Forms") print("T_phi_delta_a: ") T_phi_delta_a.print() print("T_chi_phi: ") T_chi_phi.print() print("T_beta_delta_r: ") T_beta_delta_r.print() print("T_theta_delta_e: ") T_theta_delta_e.print() print("T_h_theta: ") T_h_theta.print() print("T_h_Va: ") T_h_Va.print() print("T_Va_delta_t: ") T_Va_delta_t.print() print("T_Va_theta: ") T_Va_theta.print() return T_phi_delta_a, T_chi_phi, T_theta_delta_e, T_h_theta, T_h_Va, T_Va_delta_t, T_Va_theta, T_beta_delta_r
def compute_tf_model(mav, trim_state, trim_input): # trim values [e0, e1, e2, e3] = trim_state[6:10] [phi, theta, psi] = Quaternion2Euler(e0, e1, e2, e3) Va_star = mav._Va alpha_star = mav._alpha theta_star = theta chi_star = mav._chi delta_e_star = trim_input[1] delta_t_star = trim_input[2] #T_phi_delta_a a_phi_1 = -1 / 2 * p.rho * mav._Va**2 * p.S_wing * p.b * p.C_p_p * p.b / ( 2 * mav._Va) a_phi_2 = 1 / 2 * p.rho * mav._Va**2 * p.S_wing * p.b * p.C_p_delta_a num = np.array([[a_phi_2]]) den = np.array([[1, a_phi_1, 0]]) T_phi_delta_a = transfer_function(num, den, Ts) #T_chi_phi num = np.array([[p.gravity / mav._Vg]]) den = np.array([[1, 0]]) T_chi_phi = transfer_function(num, den, Ts) #T_beta_delta_r a_beta_1 = -p.rho * mav._Va * p.S_wing / (2 * p.mass) * p.C_Y_beta a_beta_2 = p.rho * mav._Va * p.S_wing / (2 * p.mass) * p.C_Y_delta_r num = np.array([[a_beta_2]]) den = np.array([[1, a_beta_1]]) T_beta_delta_r = transfer_function(num, den, Ts) #T_theta_delta_e a_theta_1 = -p.rho * mav._Va**2 * p.c * p.S_wing / ( 2 * p.Jy) * p.C_m_q * p.c / (2 * mav._Va) a_theta_2 = -p.rho * mav._Va**2 * p.c * p.S_wing / (2 * p.Jy) * p.C_m_alpha a_theta_3 = p.rho * mav._Va**2 * p.c * p.S_wing / (2 * p.Jy) * p.C_m_delta_e num = np.array([[a_theta_3]]) den = np.array([[1, a_theta_1, a_theta_2]]) T_theta_delta_e = transfer_function(num, den, Ts) #T_h_theta num = np.array([[mav._Vg]]) den = np.array([[1, 0]]) T_h_theta = transfer_function(num, den, Ts) #T_h_Va num = np.array([[theta]]) den = np.array([[1, 0]]) T_h_Va = transfer_function(num, den, Ts) #T_Va_delta_t a_v_1 = p.rho*Va_star*p.S_wing/p.mass*(p.C_D_0 + p.C_D_alpha*alpha_star + p.C_D_delta_e*delta_e_star) \ + p.rho*p.S_prop/p.mass*p.C_prop*Va_star a_v_2 = p.rho * p.S_prop / p.mass * p.C_prop * p.k_motor**2 * delta_t_star num = np.array([[a_v_2]]) den = np.array([[1, a_v_1]]) T_Va_delta_t = transfer_function(num, den, Ts) #T_Va_theta a_v_3 = p.gravity * np.cos(theta_star - chi_star) num = np.array([[-a_v_3]]) den = np.array([[1, a_v_1]]) T_Va_theta = transfer_function(num, den, Ts) # finite difference approach a_v_1 = p.rho*Va_star*p.S_wing/p.mass*(p.C_D_0 + p.C_D_alpha*alpha_star + p.C_D_delta_e*delta_e_star) \ - 1.0/p.mass*dT_dVa(mav, mav._Va, delta_t_star) a_v_2 = 1.0 / p.mass * dT_ddelta_t(mav, mav._Va, delta_t_star) a_v_3 = p.gravity * np.cos(theta - alpha_star) T_Va_delta_t = transfer_function(num=np.array([[a_v_2]]), den=np.array([[1, a_v_1]]), Ts=Ts) return T_phi_delta_a, T_chi_phi, T_theta_delta_e, T_h_theta, T_h_Va, T_Va_delta_t, T_Va_theta, T_beta_delta_r
def compute_tf_model(self, mav, trim_state, trim_input): # trim values u_star = trim_state[3] v_star = trim_state[4] w_star = trim_state[5] e_vec = trim_state[6:10] [phi, theta, psi] = Quaternion2Euler(e_vec) delta_e_star = trim_input[1][0] delta_t_star = trim_input[3][0] th_star = theta #is this right? #parameters rho = MAV.rho S = MAV.S_wing b = MAV.b C_p_p = MAV.C_p_p Va = mav._Va C_p_delta_a = MAV.C_p_delta_a g = MAV.gravity Vg = mav._Vg #this is in the body frame. Is that right? (its a magnitude so it shouldn't matter right?) c = MAV.c Jy = MAV.Jy C_m_q = MAV.C_m_q C_m_a = MAV.C_m_alpha #is this the right variable? C_m_delta_e = MAV.C_m_delta_e Va_star = Va #is this right since it is right after the trim function? mass = MAV.mass C_D_0 = MAV.C_D_0 C_D_al = MAV.C_D_alpha al_star = mav._alpha C_D_delta_e = MAV.C_D_delta_e S_prop = MAV.S_prop C_prop = MAV.C_prop k_motor = MAV.k_motor chi_star = mav.msg_true_state.chi C_Y_B = MAV.C_Y_beta C_Y_delta_r = MAV.C_Y_delta_r #phi to delta_a self.a_phi_1 = -0.5 * rho * Va**2 * S * b * C_p_p * b / (2.0 * Va) self.a_phi_2 = 0.5 * rho * Va**2 * S * b * C_p_delta_a num = np.array([[self.a_phi_2]]) den = np.array([[1, self.a_phi_1, 0]]) T_phi_delta_a = transfer_function(num, den, Ts) #chi to phi num = np.array([[g / Vg]]) den = np.array([[1, 0]]) T_chi_phi = transfer_function(num, den, Ts) #theta to delta_e self.a_th_1 = -rho * Va**2 * c * S / (2.0 * Jy) * C_m_q * c / (2.0 * Va) self.a_th_2 = -rho * Va**2 * c * S / (2.0 * Jy) * C_m_a self.a_th_3 = rho * Va**2 * c * S / (2.0 * Jy) * C_m_delta_e num = np.array([[self.a_th_3, 0.0]]) den = np.array([[1, self.a_th_1, self.a_th_2]]) T_theta_delta_e = transfer_function(num, den, Ts) #h to theta num = np.array([[Va]]) den = np.array([[1, 0]]) T_h_theta = transfer_function(num, den, Ts) #h to Va num = np.array([[theta]]) den = np.array([[1, 0]]) T_h_Va = transfer_function(num, den, Ts) #Va to delta_t self.a_v_1 = rho * Va_star * S / mass * ( C_D_0 + C_D_al * al_star + C_D_delta_e * delta_e_star) + rho * S_prop / mass * C_prop * Va_star self.a_v_2 = rho * S_prop / mass * C_prop * k_motor**2 * delta_t_star num = np.array([[self.a_v_2]]) den = np.array([[1, self.a_v_1]]) T_Va_delta_t = transfer_function(num, den, Ts) #Va to theta self.a_v_3 = g * np.cos(th_star - chi_star) num = np.array([[self.a_v_3]]) den = np.array([[1, self.a_v_1]]) T_Va_theta = transfer_function(num, den, Ts) #beta to delta_r self.a_B_1 = -rho * Va * S / (2.0 * mass) * C_Y_B self.a_B_2 = rho * Va * S / (2.0 * mass) * C_Y_delta_r num = np.array([[self.a_B_2]]) den = np.array([[1, self.a_B_1]]) T_beta_delta_r = transfer_function(num, den, Ts) return T_phi_delta_a, T_chi_phi, T_theta_delta_e, T_h_theta, T_h_Va, T_Va_delta_t, T_Va_theta, T_beta_delta_r