四旋翼飞行器MATLAB仿真程序_包括simulink仿真系统和程序.zip

%---------------------------------------------------------- function draw_results_3D(phi,theta,psi) % Global variables declaration global SwitchProc1 SwitchCompl % used for the Selection of the Procedures global GRAF chbox % Radio Button global EDIT % Edit Text on Robox Window global EIXOS EIXOS1 EIXOS2 EIXOS3 EIXOS4 EIXOS5 EIXOS6 global FIGU FIGU1 FIGU2 FIGU3 FIGU4 % Figure parameters global LINHA XLIMI YLIMI global x y z global roll pitch yaw tout global OS4movie % ------------------------------------------------------------------------- az3d = -41; % Azimuth for the 3-D view [degrees] el3d = 48; % Elevation for the 3-D view [degrees] R1 = 0.15; % OS4 Rotor radius [m] R2 = 0.37; % OS4 base [m] R3 = 0.01; L1 = 0.1; % OS4 Avionics Box length [m] H1 = 0.1; % OS4 Avionics Box height [m] ang1= 5*pi/180; % OS4 motor structure inclination angle [rd] hlinha = R2*tan(ang1); h2inha = 0.06 - hlinha; cth=cos(pi-theta); sth=sin(pi-theta); cph=cos(pi+phi); sph=sin(pi+phi); cps=cos(-psi+pi/2); sps=sin(-psi+pi/2); cps1=cos(-psi+pi); sps1=sin(-psi+pi); % 3D View % ------- % OS4 Avionics Box aux=sqrt(2); py = [L1/aux 0 -L1/aux 0 L1/aux]; % box botton px = [0 -L1/aux 0 L1/aux 0]; pz = [-L1/2 -L1/2 -L1/2 -L1/2 -L1/2]; npx = px*(cps1*cth + sps1*sph*sth) + py*sps1*cph + pz*(-cps1*sth + sps1*sph*cth); npy = px*(-sps1*cth + cps1*sph*sth) + py*cps1*cph + pz*(sps1*sth + cps1*sph*cth); npz = px*(sth*cph) - py*sph + pz*(cth*cph); efe=patch(npy, npx, npz,[0.5 0.7 0.5],'Parent',EIXOS3); py = [L1/aux 0 -L1/aux 0 L1/aux]; % box top px = [0 -L1/aux 0 L1/aux 0]; pz = [L1/2 L1/2 L1/2 L1/2 L1/2]; npx = px*(cps1*cth + sps1*sph*sth) + py*sps1*cph + pz*(-cps1*sth + sps1*sph*cth); npy = px*(-sps1*cth + cps1*sph*sth) + py*cps1*cph + pz*(sps1*sth + cps1*sph*cth); npz = px*(sth*cph) - py*sph + pz*(cth*cph); efe=patch(npy, npx, npz,[0.5 0.7 0.5],'Parent',EIXOS3); py = [L1/aux L1/aux 0 0 L1/aux]; % box sides px = [0 0 -L1/aux -L1/aux 0]; pz = [-L1/2 L1/2 L1/2 -L1/2 -L1/2]; npx = px*(cps1*cth + sps1*sph*sth) + py*sps1*cph + pz*(-cps1*sth + sps1*sph*cth); npy = px*(-sps1*cth + cps1*sph*sth) + py*cps1*cph + pz*(sps1*sth + cps1*sph*cth); npz = px*(sth*cph) - py*sph + pz*(cth*cph); efe=patch(npy, npx, npz,[0.5 0.7 0.5],'Parent',EIXOS3); py = [0 0 -L1/aux -L1/aux 0]; px = [-L1/aux -L1/aux 0 0 -L1/aux]; pz = [-L1/2 L1/2 L1/2 -L1/2 -L1/2]; npx = px*(cps1*cth + sps1*sph*sth) + py*sps1*cph + pz*(-cps1*sth + sps1*sph*cth); npy = px*(-sps1*cth + cps1*sph*sth) + py*cps1*cph + pz*(sps1*sth + cps1*sph*cth); npz = px*(sth*cph) - py*sph + pz*(cth*cph); efe=patch(npy, npx, npz,[0.5 0.7 0.5],'Parent',EIXOS3); py = [-L1/aux -L1/aux 0 0 -L1/aux]; px = [0 0 L1/aux L1/aux 0]; pz = [-L1/2 L1/2 L1/2 -L1/2 -L1/2]; npx = px*(cps1*cth + sps1*sph*sth) + py*sps1*cph + pz*(-cps1*sth + sps1*sph*cth); npy = px*(-sps1*cth + cps1*sph*sth) + py*cps1*cph + pz*(sps1*sth + cps1*sph*cth); npz = px*(sth*cph) - py*sph + pz*(cth*cph); efe=patch(npy, npx, npz,[0.5 0.7 0.5],'Parent',EIXOS3); py = [0 0 L1/aux L1/aux 0]; px = [L1/aux L1/aux 0 0 L1/2]; pz = [-L1/2 L1/2 L1/2 -L1/2 -L1/2]; npx = px*(cps1*cth + sps1*sph*sth) + py*sps1*cph + pz*(-cps1*sth + sps1*sph*cth); npy = px*(-sps1*cth + cps1*sph*sth) + py*cps1*cph + pz*(sps1*sth + cps1*sph*cth); npz = px*(sth*cph) - py*sph + pz*(cth*cph); efe=patch(npy, npx, npz,[0.5 0.7 0.5],'Parent',EIXOS3); % OS4 Structure bars and Rotors % Rotor # 4 npx = 0*(cps1*cth + sps1*sph*sth) + R2*sps1*cph + (hlinha + h2inha + L1/2)*(-cps1*sth + sps1*sph*cth); npy = 0*(-sps1*cth + cps1*sph*sth) + R2*cps1*cph + (hlinha + h2inha + L1/2)*(sps1*sth + cps1*sph*cth); npz = 0*(sth*cph) - (R2)*sph + (hlinha + h2inha + L1/2)*(cth*cph); npxo = 0*(cps1*cth + sps1*sph*sth) + 0*sps1*cph + (h2inha + L1/2)*(-cps1*sth + sps1*sph*cth); npyo = 0*(-sps1*cth + cps1*sph*sth) + 0*cps1*cph + (h2inha + L1/2)*(sps1*sth + cps1*sph*cth); npzo = 0*(sth*cph) - 0*sph + (h2inha + L1/2)*(cth*cph); xxc = [npxo; npx]; zzc = [npzo; npz]; yyc = [npyo; npy]; como = 0; for tt=0:0.1:6.28, como = como + 1; xo(como) = R1*cos(tt)*(cps1*cth + sps1*sph*sth) + R1*sin(tt)*sps1*cph + 0*(-cps1*sth + sps1*sph*cth) + npx; yo(como) = R1*cos(tt)*(-sps1*cth + cps1*sph*sth) + R1*sin(tt)*cps1*cph + 0*(sps1*sth + cps1*sph*cth) + npy; zo(como) = R1*cos(tt)*(sth*cph) - R1*sin(tt)*sph + 0*(cth*cph) + npz; end efe=patch(yo,xo,zo,[0.9 0.9 0.9],'LineStyle','-','Parent',EIXOS3); LINHA=line(yyc, xxc, zzc,'Parent',EIXOS3,'Color',[0 0 0],'LineStyle','-', 'LineWidth',2); if npx < 0, aqux = npx-0.06; else, aqux = npx+0.06; end if npy < 0, aquy = npy-0.06; else, aquy = npy+0.06; end aquz = npz + 0.16; text(aquy,aqux,aquz,'4','FontSize',10,'Parent',EIXOS3); % Rotor # 2 npx = 0*(cps1*cth + sps1*sph*sth) - R2*sps1*cph + (hlinha + h2inha + L1/2)*(-cps1*sth + sps1*sph*cth); npy = 0*(-sps1*cth + cps1*sph*sth) - R2*cps1*cph + (hlinha + h2inha + L1/2)*(sps1*sth + cps1*sph*cth); npz = 0*(sth*cph) - (-R2)*sph + (hlinha + h2inha + L1/2)*(cth*cph); npxo = 0*(cps1*cth + sps1*sph*sth) + 0*sps1*cph + (h2inha + L1/2)*(-cps1*sth + sps1*sph*cth); npyo = 0*(-sps1*cth + cps1*sph*sth) + 0*cps1*cph + (h2inha + L1/2)*(sps1*sth + cps1*sph*cth); npzo = 0*(sth*cph) - 0*sph + (h2inha + L1/2)*(cth*cph); xxc = [npxo; npx]; zzc = [npzo; npz]; yyc = [npyo; npy]; como = 0; for tt=0:0.1:6.28, como = como + 1; xo(como) = R1*cos(tt)*(cps1*cth + sps1*sph*sth) + R1*sin(tt)*sps1*cph + 0*(-cps1*sth + sps1*sph*cth) + npx; yo(como) = R1*cos(tt)*(-sps1*cth + cps1*sph*sth) + R1*sin(tt)*cps1*cph + 0*(sps1*sth + cps1*sph*cth) + npy; zo(como) = R1*cos(tt)*(sth*cph) - R1*sin(tt)*sph + 0*(cth*cph) + npz; end efe=patch(yo,xo,zo,[0.9 0.9 0.9],'LineStyle','-','Parent',EIXOS3); LINHA=line(yyc, xxc, zzc,'Parent',EIXOS3,'Color',[0 0 0],'LineStyle','-', 'LineWidth',2); if npx < 0, aqux = npx-0.06; else, aqux = npx+0.06; end if npy < 0, aquy = npy-0.06; else, aquy = npy+0.06; end aquz = npz + 0.16; text(aquy,aqux,aquz,'2','FontSize',10,'Parent',EIXOS3); % Rotor # 3 npx = -R2*(cps1*cth + sps1*sph*sth) + 0*sps1*cph + (hlinha + h2inha + L1/2)*(-cps1*sth + sps1*sph*cth); npy = -R2*(-sps1*cth + cps1*sph*sth) + 0*cps1*cph + (hlinha + h2inha + L1/2)*(sps1*sth + cps1*sph*cth); npz = -R2*(sth*cph) - 0*sph + (hlinha + h2inha + L1/2)*(cth*cph); npxo = 0*(cps1*cth + sps1*sph*sth) + 0*sps1*cph + (h2inha + L1/2)*(-cps1*sth + sps1*sph*cth); npyo = 0*(-sps1*cth + cps1*sph*sth) + 0*cps1*cph + (h2inha + L1/2)*(sps1*sth + cps1*sph*cth); npzo = 0*(sth*cph) - 0*sph + (h2inha + L1/2)*(cth*cph); xxc = [npxo; npx]; zzc = [npzo; npz]; yyc = [npyo; npy]; como = 0; for tt=0:0.1:6.28, como = como + 1; xo(como) = R1*cos(tt)*(cps1*cth + sps1*sph*sth) + R1*sin(tt)*sps1*cph + 0*(-cps1*sth + sps1*sph*cth) + npx; yo(como) = R1*cos(tt)*(-sps1*cth + cps1*sph*sth) + R1*sin(tt)*cps1*cph + 0*(sps1*sth + cps1*sph*cth) + npy; zo(como) = R1*cos(tt)*(sth*cph) - R1*sin(tt)*sph + 0*(cth*cph) + npz; end efe=patch(yo,xo,zo,[0.9 0.9 0.9],'LineStyle','-','Parent',EIXOS3); LINHA=line(yyc, xxc, zzc,'Parent',EIXOS3,'Color',[0 0 0],'LineStyle','-', 'LineWidth',2); if npx < 0, aqux = npx-0.06; else, aqux = npx+0.06; end if npy < 0, aquy = npy-0.06; else, aquy = npy+0.06; end aquz = npz + 0.16; text(aquy,aqux,aquz,'3','FontSize',10,'Parent',EIXOS3); % Rotor # 1 npx = R2*(cps1*cth + sps1*sph*sth) + 0*sps1*cph + (hlinha + h2inha + L1/2)*(-cps1*sth + sps1*sph*cth); npy = R2*(-sps1*cth + cps1*sph*sth)