class AppFixedWingPitchAttHolder(FixedWingUAV): fuse_l1 = 5. fuse_l2 = 2.5 fuse_l3 = 10. fuse_h = 3. fuse_w = 3. wing_w = 15. wing_l = 3. tail_h = 4. tailwing_w = 7.5 tailwing_l = 1.5 def __init__(self, x0, t0, config, ax): super(AppFixedWingPitchAttHolder, self).__init__(x0, t0, config) self.vertices = np.matrix(np.zeros((16, 3), dtype = np.double)) self.vertices[0, :] = [self.fuse_l1, 0.0, 0.0] self.vertices[1, :] = [self.fuse_l2, 0.5*self.fuse_w, 0.5*self.fuse_h] self.vertices[2, :] = [self.fuse_l2, -0.5*self.fuse_w, 0.5*self.fuse_h] self.vertices[3, :] = [self.fuse_l2, -0.5*self.fuse_w, -0.5*self.fuse_h] self.vertices[4, :] = [self.fuse_l2, 0.5*self.fuse_w, -0.5*self.fuse_h] self.vertices[5, :] = [-self.fuse_l3, 0., 0.] self.vertices[6, :] = [0, 0.5 * self.wing_w, 0.0] self.vertices[7, :] = [-self.wing_l, 0.5 * self.wing_w, 0.0] self.vertices[8, :] = [-self.wing_l, -0.5 * self.wing_w, 0.0] self.vertices[9, :] = [0, -0.5 * self.wing_w, 0.0] self.vertices[10, :] = [-(self.fuse_l3 - self.tailwing_l), 0.5 * self.tailwing_w, 0.] self.vertices[11, :] = [-self.fuse_l3, 0.5 * self.tailwing_w, 0.] self.vertices[12, :] = [-self.fuse_l3, -0.5 * self.tailwing_w, 0.] self.vertices[13, :] = [-(self.fuse_l3 - self.tailwing_l), -0.5 * self.tailwing_w, 0.] self.vertices[14, :] = [-(self.fuse_l3 - self.tailwing_l), 0.0, 0.] self.vertices[15, :] = [-self.fuse_l3, 0.0, -self.tail_h] self.vertices = 0.3048 * self.vertices self.nose = [[0, 1, 2], [0, 2, 3], [0, 3, 4], [0, 1, 4]] self.fuselage = [[5, 3, 2], [5, 2, 1], [5, 3, 4], [5, 4, 1]] self.wing = [[6, 7, 8, 9]] self.tail_wing = [[10, 11, 12, 13]] self.tail = [[5, 14, 15]] self.faces = [self.nose, self.fuselage, self.wing, self.tail_wing, self.tail] self.viewer = UAVViewer(ax, (self.rotate(x0[6:9]) + x0[0:3]), self.faces, ['r', 'g', 'g', 'g', 'y']) self.autopilot = Autopilot([], 1./200.) self.autopilot.delta_e_limit = self.attrs['autopilot']['delta_e_max_deg'] * np.pi/180 self.autopilot.pitch_hold_controller.limit = self.autopilot.delta_e_limit def update_view(self): vertices = self.rotate(self.dynamics.x[6:9]) + self.dynamics.x[0:3] self.viewer.update(vertices) def trim(self, Va, gamma, radius, max_iters): trimmed_state, trimmed_control_inputs = self.dynamics.trim(Va, gamma, radius, epsilon=1e-8, kappa=1e-6, max_iters=max_iters) #self.set_state(trimmed_state, 0.) self.set_control_inputs(trimmed_control_inputs) #TODO: move ki, tau, zeta to config if required def set_pitch(self, pitch_c, ki, tau, zeta): Va = np.linalg.norm(self.dynamics.x[3:6]) S = self.attrs['params']['S'] #b = self.attrs['params']['b'] c = self.attrs['params']['c'] rho = self.attrs['params']['rho'] Jy = self.attrs['params']['Jy'] Clong_coeffs = self.attrs['longitudinal_coeffs'] Cm_q = Clong_coeffs['Cm_q'] Cm_alpha = Clong_coeffs['Cm_q'] Cm_delta_e = Clong_coeffs['Cm_delta_e'] atheta_1 = -rho * Va * c * S * Cm_q * 0.5 * c atheta_2 = -rho * Va**2 * c *S * Cm_alpha * 0.5/Jy atheta_3 = rho * Va**2 * c *S * Cm_delta_e * 0.5/Jy self.autopilot.pitch_hold_controller.kp = self.attrs['autopilot']['delta_e_max_deg']/self.attrs['autopilot']['error_theta_max_deg'] * np.sign(atheta_3) self.autopilot.pitch_hold_controller.ki = ki omega_theta = np.sqrt(atheta_2 + self.attrs['autopilot']['delta_e_max_deg']/self.attrs['autopilot']['error_theta_max_deg'] * np.abs(atheta_3)) self.autopilot.pitch_hold_controller.kd = (2 * zeta * omega_theta - atheta_1)/atheta_3 self.autopilot.pitch_hold_controller.tau = tau control_inputs = self.get_control_inputs() #-0.018949908534872429 q = self.dynamics.x[10] control_inputs[0] = self.autopilot.compute_delta_e(pitch_c, self.dynamics.x[7], -q) self.set_control_inputs(control_inputs)
class AppFixedWingAirspeedHolder(FixedWingUAV): fuse_l1 = 5. fuse_l2 = 2.5 fuse_l3 = 10. fuse_h = 3. fuse_w = 3. wing_w = 15. wing_l = 3. tail_h = 4. tailwing_w = 7.5 tailwing_l = 1.5 def __init__(self, x0, t0, config, ax): super(AppFixedWingAirspeedHolder, self).__init__(x0, t0, config) self.vertices = np.matrix(np.zeros((16, 3), dtype = np.double)) self.vertices[0, :] = [self.fuse_l1, 0.0, 0.0] self.vertices[1, :] = [self.fuse_l2, 0.5*self.fuse_w, 0.5*self.fuse_h] self.vertices[2, :] = [self.fuse_l2, -0.5*self.fuse_w, 0.5*self.fuse_h] self.vertices[3, :] = [self.fuse_l2, -0.5*self.fuse_w, -0.5*self.fuse_h] self.vertices[4, :] = [self.fuse_l2, 0.5*self.fuse_w, -0.5*self.fuse_h] self.vertices[5, :] = [-self.fuse_l3, 0., 0.] self.vertices[6, :] = [0, 0.5 * self.wing_w, 0.0] self.vertices[7, :] = [-self.wing_l, 0.5 * self.wing_w, 0.0] self.vertices[8, :] = [-self.wing_l, -0.5 * self.wing_w, 0.0] self.vertices[9, :] = [0, -0.5 * self.wing_w, 0.0] self.vertices[10, :] = [-(self.fuse_l3 - self.tailwing_l), 0.5 * self.tailwing_w, 0.] self.vertices[11, :] = [-self.fuse_l3, 0.5 * self.tailwing_w, 0.] self.vertices[12, :] = [-self.fuse_l3, -0.5 * self.tailwing_w, 0.] self.vertices[13, :] = [-(self.fuse_l3 - self.tailwing_l), -0.5 * self.tailwing_w, 0.] self.vertices[14, :] = [-(self.fuse_l3 - self.tailwing_l), 0.0, 0.] self.vertices[15, :] = [-self.fuse_l3, 0.0, -self.tail_h] self.vertices = 0.3048 * self.vertices self.nose = [[0, 1, 2], [0, 2, 3], [0, 3, 4], [0, 1, 4]] self.fuselage = [[5, 3, 2], [5, 2, 1], [5, 3, 4], [5, 4, 1]] self.wing = [[6, 7, 8, 9]] self.tail_wing = [[10, 11, 12, 13]] self.tail = [[5, 14, 15]] self.faces = [self.nose, self.fuselage, self.wing, self.tail_wing, self.tail] self.viewer = UAVViewer(ax, (self.rotate(x0[6:9]) + x0[0:3]), self.faces, ['r', 'g', 'g', 'g', 'y']) self.autopilot = Autopilot([], 1./200.) self.autopilot.delta_a_limit = 15. * np.pi/180. self.autopilot.roll_hold_controller.limit = self.autopilot.delta_a_limit def update_view(self): vertices = self.rotate(self.dynamics.x[6:9]) + self.dynamics.x[0:3] self.viewer.update(vertices) def trim(self, Va, gamma, radius, max_iters): trimmed_state, trimmed_control_inputs = self.dynamics.trim(Va, gamma, radius, epsilon=1e-8, kappa=1e-6, max_iters=max_iters) #self.set_state(trimmed_state, 0.) #print trimmed_control_inputs self.set_control_inputs(trimmed_control_inputs) #TODO: vg = va (no wind is assumed. Add wind models and change appropriately later!) def set_airspeed(self, Va_c, zeta, Va_trim, delta_e_trim, alpha_trim): Va = np.linalg.norm(self.dynamics.x[3:6]) S = self.attrs['params']['S'] #b = self.attrs['params']['b'] c = self.attrs['params']['c'] rho = self.attrs['params']['rho'] Jy = self.attrs['params']['Jy'] Clong_coeffs = self.attrs['longitudinal_coeffs'] CD0 = Clong_coeffs['CD0'] CD_alpha = Clong_coeffs['CD_alpha'] CD_delta_e = Clong_coeffs['CD_delta_e'] C_prop = Clong_coeffs['C_prop'] S_prop = self.attrs['params']['S_prop'] mass = self.attrs['params']['mass'] aV_1 = rho * Va_trim * S * (CD0 + CD_alpha * alpha_trim + CD_delta_e * delta_e_trim)/mass + rho * S_prop * C_prop * Va_trim/mass Cm_alpha = Clong_coeffs['Cm_q'] Cm_delta_e = Clong_coeffs['Cm_delta_e'] atheta_2 = -rho * Va**2 * c *S * Cm_alpha * 0.5/Jy atheta_3 = rho * Va**2 * c *S * Cm_delta_e * 0.5/Jy omega_v2 = 0.5 kp_theta = self.attrs['autopilot']['delta_e_max_deg']/self.attrs['autopilot']['error_theta_max_deg'] * np.sign(atheta_3) K_theta_dc = (kp_theta * atheta_3)/(atheta_2 + kp_theta * atheta_3) self.autopilot.airspeed_hold_with_pitch_controller.kp = (aV_1 - 2.0*zeta*omega_v2)/(K_theta_dc * 9.81) self.autopilot.airspeed_hold_with_pitch_controller.ki = -omega_v2**2 /(K_theta_dc * 9.81) pitch_c = self.autopilot.compute_pitch_for_airspeed(Va_c, Va) return pitch_c #TODO: move ki, tau, zeta to config if required def set_pitch(self, pitch_c, ki, tau, zeta): Va = np.linalg.norm(self.dynamics.x[3:6]) S = self.attrs['params']['S'] #b = self.attrs['params']['b'] c = self.attrs['params']['c'] rho = self.attrs['params']['rho'] Jy = self.attrs['params']['Jy'] Clong_coeffs = self.attrs['longitudinal_coeffs'] Cm_q = Clong_coeffs['Cm_q'] Cm_alpha = Clong_coeffs['Cm_q'] Cm_delta_e = Clong_coeffs['Cm_delta_e'] atheta_1 = -rho * Va * c * S * Cm_q * 0.5 * c atheta_2 = -rho * Va**2 * c *S * Cm_alpha * 0.5/Jy atheta_3 = rho * Va**2 * c *S * Cm_delta_e * 0.5/Jy self.autopilot.pitch_hold_controller.kp = self.attrs['autopilot']['delta_e_max_deg']/self.attrs['autopilot']['error_theta_max_deg'] * np.sign(atheta_3) self.autopilot.pitch_hold_controller.ki = ki omega_theta = np.sqrt(atheta_2 + self.attrs['autopilot']['delta_e_max_deg']/self.attrs['autopilot']['error_theta_max_deg'] * np.abs(atheta_3)) print 'omega_theta: ', omega_theta self.autopilot.pitch_hold_controller.kd = (2 * zeta * omega_theta - atheta_1)/atheta_3 self.autopilot.pitch_hold_controller.tau = tau control_inputs = self.get_control_inputs() #-0.018949908534872429 q = self.dynamics.x[10] control_inputs[0] = self.autopilot.compute_delta_e(pitch_c, self.dynamics.x[7]) self.set_control_inputs(control_inputs)