www.gusucode.com > Simulink,飞行器控制源码程序 > Simulink,飞行器控制源码程序/Aerial_recovery_demo/param.m
clear all clc P.ts = 0.2; %========================================================================= % physical parameters P.g = 9.8; % (m/s) gravity constant P.rho = 1.268200; % air density P.sound_speed = 340; % sound speed (m/s) %========================================================================= % wind parameters (currently not working with wind) P.wind.n = 0; % North component of wind P.wind.e = 0; % East component of wind %========================================================================= % mothership configuration P.mothership.dir = -1; % -1=conter-clockwise; 1=clockwise V = 50; % mothership airspeed in m/s R = 300; % mothership radius in m c = [0; 0]; % center of mothership orbit in m h = 1000; % altitude of mothership in m P.mothership.n = c(1) + R; % initial North position of mothership P.mothership.e = c(2); % initial East position of mothership P.mothership.d = -h; % initial Down position of mothership P.mothership.V = V; % initial airspeed of mothership P.mothership.chi = -pi/2; % initial heading set up for counterclockwise orbit P.mothership.phi = 0; % initial roll angle P.mothership.gam = 0; % intial flight path angle P.mothership.phibar = 45*(pi/180); % (rad) roll angle constraint P.mothership.phidotbar = 100*(pi/180); % (rad/sec) roll rate constraint P.mothership.R = V^2/P.g/tan(P.mothership.phibar)+5; % (m) min turn radius P.mothership.gambar = 20*(pi/180); % (rad) flight path constraint P.mothership.gamdotbar = 60*(pi/180); % (rad/sec) flight path rate constraint P.mothership.T_guidance = P.ts; % sample rate for mothership guidance loop P.mothership.k_roll = 10; % roll attitude hold proportional gain P.mothership.k_h = 1; % proportional gain for altitude P.mothership.k_hdot = 1; % derivative gain for altitude P.mothership.k_orbit = 0.01; % proportional gain for tracking oribits P.mothership.radius = R; P.mothership.size = 0.2; % size for display %========================================================================= % cable parameters P.cable.N = 15; % number of the cable links P.cable.length = 900; % cabel length in meters P.cable_spring_1 = 0.05; % spring constant of cable P.cable_spring_2 = 0.001; % velocity constraint spring constant of cable P.cable.mass = 1; % cable mass P.cable.straighten = 50; % force constatn that straightens the cable P.cable.constraint_length = P.cable.length/P.cable.N; if (P.cable.N>=2) for i=1:(P.cable.N-1) P.cable.link.n0(i) = P.mothership.n; P.cable.link.e0(i) = P.mothership.e; P.cable.link.d0(i) = P.mothership.d + P.cable.length * (i)/P.cable.N ; P.cable.link.ndot0(i) = 0; P.cable.link.edot0(i) = 0; P.cable.link.ddot0(i) = 0; end end P.Cf_last_joint = 0.5; P.Cn_last_joint = 0.27; %P.cable.rho = 10; %cable density P.cable.d = 0.01; %cable diameter %========================================================================= % drogue configuration P.drogue.n = P.mothership.n; P.drogue.e = P.mothership.e; P.drogue.d = P.mothership.d + P.cable.length; P.drogue.ndot = 0; P.drogue.edot = 0; P.drogue.ddot = 0; P.drogue.phi = 0; P.drogue.theta = 0; P.drogue.psi = P.mothership.chi; P.drogue.S = 0.258900; % wing area of drogue P.drogue.mass = 5; % 5 mass of drogue in kg P.drogue.phibar = 45*pi/180; % maximum roll angle of drogue 30*pi/180 P.drogue.phidotbar = 10*pi/180; % maximum roll rate of drogue P.drogue.thetabar = 20*pi/180; % 20 maximum pitch angle of drogue P.drogue.thetadotbar = 5*pi/180; % maximum pitch rate of drogue P.drogue.Vdotbar = 1; % maximum acceleration of drogue P.drogue.size = 0.15; % size for display P.drogue.markers = [... % visual markers in the body frame 0, 0, 0;... 0, 50*P.drogue.size, 0;... 0, -50*P.drogue.size, 0;... 0, 0, -50*P.drogue.size/2;... ]; P.drogue.marker_size = 1; P.drogue.phi_d = 15*pi/180; P.drogue.theta_d = 15*pi/180; P.drogue.k_phi = 5; P.drogue.k_theta = 5; %========================================================================= % drogue autopilot parameters P.drogue.T_guidance = P.ts; % sample rate for guidance algorithm %========================================================================= % drogue aerodynamic coefficients for linear force model P.drogue.CD_ctrl_flag = 0; % flag of using drag coefficient control P.drogue.CL_0 = 0.280000; P.drogue.CD_0 = 0.5; %0.030000; P.drogue.CDdotbar = 0.05; P.drogue.CD_bar = 1.2; P.drogue.CD_bar_lower = 0.3; P.drogue.k_CD = 0.01; %========================================================================= % MAV configuration D = 500; P.mav.n = P.mothership.n - D*cos(P.mothership.chi); P.mav.e = P.mothership.e - D*sin(P.mothership.chi); P.mav.d = P.drogue.d; P.mav.chi = P.mothership.chi; P.mav.V = P.mothership.V/3; P.mav.phi = 0; P.mav.gam = 0; P.mav.az = 0; P.mav.el = 0; % MAV performance parameters P.mav.Vbar_up = 17; % (m/s) upper limit on velocity P.mav.Vbar_low = 10; % (m/s) lower limit on velocity P.mav.Vdotbar = 1; % (m/s^2) limit on velocity increase P.mav.phibar = 45*(pi/180); % (rad) roll angle constraint P.mav.phidotbar = 100*(pi/180); % (rad/sec) roll rate constraint P.mav.R = P.mav.V^2/P.g/tan(P.mav.phibar)+5; % (m) min turn radius P.mav.gambar = 20*(pi/180); % (rad) flight path constraint P.mav.gamdotbar = 60*(pi/180); % (rad/sec) flight path rate constraint % MAV gimbal parameters P.mav.gim.elbar = 60*(pi/180); % maximum gimbal elevation angle P.mav.gim.eldotbar = 660*(pi/180); % maximum rate of change of the gimbal elevation angle P.mav.gim.azbar = 60*(pi/180); % maximum gimbal azimuth angle P.mav.gim.azdotbar = 330*(pi/180); % maximum rate of change of the gimbal azimuth angle P.mav.gim.az = 0*(pi/180); % strapdown azimuth angle P.mav.gim.el = 0*(pi/180); % strapdown elevation angle % size for display P.mav.size = 0.1; %========================================================================= % camera parameters (on MAV) P.cam.pix = 480; % size of (square) pixel array P.cam.fov = 70*(pi/180); % field of view of camera P.cam.f = (P.cam.pix/2)/tan(P.cam.fov/2); % focal range P.cam.pixelnoise = 0; % (pixels) - variance of the pixel noise % %========================================================================= % MAV autopilot parameters P.mav.T_guidance = P.ts; % sample rate for guidance algorithm P.mav.T_cam = 1*P.mav.T_guidance; % sample rate for camera (needs to be integer value of T_guidance in current implimentation) P.mav.k_V = 10; % proportional constant for airspeed P.mav.k_roll = 10; % roll attitude hold proportional gain P.mav.k_pitch = 10; % pitch gain used in direct pursuit algorithm P.mav.k_yaw = 1; % yaw gain used in direct pursuit algorithm P.mav.k_az = 1; % proportional constant for azimuth angle P.mav.k_el = 1; % proportional constant for elevation angle P.mav.navigation_ratio = 3.5; % PRONAV navigation constants P.mav.sigmoid_gain = 0.2; % sigmoid gain for roll control P.mav.tau = 1/1; % bandwidth of differentiator for line of sight rate and distance rate %--------------------------------------------------------------------- % MAV Kalman filter parameters P.mav.ekf.paz0 = .1; % initial estimate of estimation error of eta_az in radians P.mav.ekf.pel0 = .1; % initial estimate of estimation error of eta_el in radians P.mav.ekf.q_az = .01; % process noise on eta_az P.mav.ekf.q_el = .01; % process noise on eta_el P.mav.ekf.r_eps_x = 1; % measurement noise on x-pixel P.mav.ekf.r_eps_y = 1; % measurement noise on y-pixel %========================================================================= % parameters for drawing UAVs P.drawuav.scale = 0.2; % plot scale for UAV % the origin is at the center of the fuselage P.drawuav.h_fuse = 6.5; % fuselage height (cm) P.drawuav.w_fuse = 9.5; % fuselage width (cm) P.drawuav.l_fuse = 55.5; % fuselage length (cm) % wing coefficients P.drawuav.w_recess = 6.5; P.drawuav.w_height = 5.5; P.drawuav.w_width = 71.5; P.drawuav.w_tipwidth = 25; P.drawuav.w_back = 30; % rudder coefficients P.drawuav.r_height = 21; P.drawuav.r_top = 10; %%%%%%%% parameters for drawing uav %%%%%%%%%%%%%%%% P.uavscale = 0.2; % plot scale for UAV % the origin is at the center of the fuselage P.h_fuse = 6.5; % fuselage height (cm) P.w_fuse = 9.5; % fuselage width (cm) P.l_fuse = 55.5; % fuselage length (cm) % wing coefficients P.w_recess = 6.5; P.w_height = 5.5; P.w_width = 71.5; P.w_tipwidth = 25; P.w_back = 30; % rudder coefficients P.r_height = 21; P.r_top = 10; % colors P.myred = [1, 0, 0]; P.mygreen = [0, 1, 0]; P.myblue = [0, 0, 1]; P.myyellow = [1,1,0]; P.mymagenta = [0, 1, 1]; % Plot parameters P.fig_cable = zeros(1,P.cable.N); P.fig_joint = zeros(1,(P.cable.N-1)); P.joint_size = 5; H = 350; P.x_max = H; P.x_min = -H; P.y_max = H; P.y_min = -H; P.z_max = 1050; P.z_min = 50; % 0