plotquadrotor_old.m

四轴飞行器Matlab仿真分析,涉及四轴飞行器主要部件如Motor,camera,电池等

function [sys,x0,str,ts] = plotquadrotor(t,x,u,flag,C)
%
% modified 10/16/02 - Randy Beard
%          10/26/07 - RB
%          11/12/07 - RB
%          11/20/07 - RB
%          11/30/07 - RB
%          12/5/07  - RB

switch flag,

  %%%%%%%%%%%%%%%%%%
  % Initialization %
  %%%%%%%%%%%%%%%%%%
  case 0,
    [sys,x0,str,ts] = mdlInitializeSizes;

  %%%%%%%%%%
  % Update %
  %%%%%%%%%%
  case 2,                                                
    sys = mdlUpdate(t,x,u,C); 

  %%%%%%%%%%
  % Output %
  %%%%%%%%%%
  case 3,                                                
    sys = [];

  %%%%%%%%%%%%%
  % Terminate %
  %%%%%%%%%%%%%
  case 9,                                                
    sys = []; % do nothing

  %%%%%%%%%%%%%%%%%%%%
  % Unexpected flags %
  %%%%%%%%%%%%%%%%%%%%
  otherwise
    error(['unhandled flag = ',num2str(flag)]);
end

%end dsfunc

%
%=======================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=======================================================================
%
function [sys,x0,str,ts] = mdlInitializeSizes

sizes = simsizes;
sizes.NumContStates  = 0;
sizes.NumDiscStates  = 4;
sizes.NumOutputs     = 0;
sizes.NumInputs      = 12+4+4;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;


sys = simsizes(sizes);

x0 = zeros(sizes.NumDiscStates,1);
str = [];
ts  = [.1 0]; 

% end mdlInitializeSizes
%
%=======================================================================
% mdlUpdate
% Handle discrete state updates, sample time hits, and major time step
% requirements.
%=======================================================================
%
function xup = mdlUpdate(t,x,uu,C)

  initialize    = x(1);       % initial graphics
  fig_quadrotor = x(2);       % quadrotor figure handle
  fig_target    = x(3);       % target figure handle
  fig_cam_obj   = x(4);       % position figure handle
  NN = 0;
  pn            = uu(1+NN);       % inertial North position     
  pe            = uu(2+NN);       % inertial East position
  pd            = uu(3+NN);           
  h = -pd;                        % altitude
  u             = uu(4+NN);       
  v             = uu(5+NN);       
  w             = uu(6+NN);       
  phi           = uu(7+NN);       % roll angle     
  theta         = uu(8+NN);       % pitch angle     
  psi           = uu(9+NN);       % yaw angle     
  p             = uu(10+NN);       % roll rate
  q             = uu(11+NN);       % pitch rate     
  r             = uu(12+NN);       % yaw rate    
  NN = NN + 12;
  qx            = uu(1+NN);       % pixel x location
  qy            = uu(2+NN);       % pixel y location
  qsize         = uu(3+NN);       % pixel size
  qpsi          = uu(4+NN);       % target orientation in image
  NN = NN + 4;
  tn            = uu(1+NN);       % target x location
  te            = uu(2+NN);       % target y location
  td            = uu(3+NN);       % target z location
  tpsi          = uu(4+NN);       % target orientation in inertial frame
  

  if initialize==0, 
    % initialize the plot
    initialize = 1;
    figure(1), clf
    
    % plot quadrotor and rails
    % subplot(221)
    subplot('position',[0.01, 0.01, 0.6, 0.6]);
    axis(0.75*[-1, 1, -1, 1, -1, 1]) % units are centimeters
    set(gca,'visible','off')
    fig_quadrotor = quadrotorPlot(pn, pe, h, phi, theta, psi, C, [], 'normal');
    hold on
    fig_target = targetPlot(tn,te,td,-tpsi,C.target_size,[],'r','normal');
    title('Attitude (roll, pitch, yaw)')
    view(117,41)
    

    % plot camera view
    
    h=subplot('position',[0.61, 0.61, 0.3, 0.3]);
      tmp = C.cam_pix/2;
      set(h,'XAxisLocation','top','XLim',[-tmp,tmp],'YLim',[-tmp,tmp]);
      axis ij
      hold on
      fig_cam_obj = targetCamPlot(qx,qy,0,qpsi,qsize,[],'r','normal');
      xlabel('px (pixels)')
      ylabel('py (pixels)')
      title('Camera View')
    
    
  else  % do this at every time step

    % plot quadrotor
     quadrotorPlot(pn, pe, h, phi, theta, psi, C, fig_quadrotor);
     targetPlot(tn,te,td,-tpsi,C.target_size,fig_target);     
    % plot camera view
      targetCamPlot(qx,qy,0,qpsi,qsize,fig_cam_obj);

end
  
  xup = [initialize; fig_quadrotor; fig_target; fig_cam_obj];


%end mdlUpdate

%----------------------------------------------------------------------
%----------------------------------------------------------------------
% User defined functions
%----------------------------------------------------------------------
%----------------------------------------------------------------------

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function handle = quadrotorPlot(pn, pe, h, phi, theta, psi, C, handle, mode);
  % plot of quadrotor in contraption - rolling motion

  P = defineParameters;
  [Vert_quad, Face_quad, colors_quad]       = quadrotorVertFace(P);
  [Vert_fov, Face_fov, colors_fov]          = fovVertFace(pn,pe,h,phi,theta,psi,P,C);

  % rotate vertices by phi
  Vert_quad = rotateVert(Vert_quad, -phi, theta, psi);

  % translate vertices by re
  Vert_quad = translateVert(Vert_quad, [pn; pe; h]);

  % collect all vertices and faces
  V = [...
      Vert_quad;...
      Vert_fov;...
      ];
  F = [...
      Face_quad;...
      size(Vert_quad,1) + Face_fov;...
      ]; 
  patchcolors = [...
      colors_quad;...
      colors_fov;...
      ];

  if isempty(handle),
  handle = patch('Vertices', V, 'Faces', F,...
                 'FaceVertexCData',patchcolors,...
                 'FaceColor','flat',...
                 'EraseMode', mode);
  else
    set(handle,'Vertices',V,'Faces',F);
    drawnow
  end
 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% define the quadrotor parameters
function P = defineParameters

% parameters
P.r = 0.1;   % radius of the rotor
P.l = 0.1;   % length of connecting rods
P.lw = 0.01; % width of rod
P.w = 0.1;   % width of the center pod
P.w_rail = 0.01; % width of the rail
P.l_rail = 5; % length of the rail
P.N = 10;     % number of points defining rotor

% define colors for faces
P.myred = [1, 0, 0];
P.mygreen = [0, 1, 0];
P.myblue = [0, 0, 1];
P.myyellow = [1,1,0];

%%%%%%%%%%%%%%%%%%%%%%%
function Vert=rotateVert(Vert,phi,theta,psi);
  % define rotation matrix
  R_roll = [...
          1, 0, 0;...
          0, cos(phi), -sin(phi);...
          0, sin(phi), cos(phi)];
  R_pitch = [...
          cos(theta), 0, sin(theta);...
          0, 1, 0;...
          -sin(theta), 0, cos(theta)];
  R_yaw = [...
          cos(psi), -sin(psi), 0;...
          sin(psi), cos(psi), 0;...
          0, 0, 1];
  R = R_roll'*R_pitch'*R_yaw';

  % rotate vertices
  Vert = (R*Vert')';
  
% end rotateVert

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% translate vertices by column vector T
function Vert = translateVert(Vert, T)

  Vert = Vert + repmat(T', size(Vert,1),1);

% end translateVert

%%%%%%%%%%%%%%%%%%%%%%%
function [X,Y,Z]=rotateXYZ(X,Y,Z,phi,theta,psi);
  % define rotation matrix
  R_roll = [...
          1, 0, 0;...
          0, cos(phi), -sin(phi);...
          0, sin(phi), cos(phi)];
  R_pitch = [...
          cos(theta), 0, sin(theta);...
          0, 1, 0;...
          -sin(theta), 0, cos(theta)];
  R_yaw = [...
          cos(psi), -sin(psi), 0;...
          sin(psi), cos(psi), 0;...
          0, 0, 1];
  R = R_roll'*R_pitch'*R_yaw';

  % rotate vertices
  pts = [X, Y, Z]*R;
  X = pts(:,1);
  Y = pts(:,2);
  Z = pts(:,3);
  
% end rotateVert

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% translate vertices by column vector T
function [X,Y,Z] = translateXYZ(X,Y,Z,T)

  X = X + T(1)*ones(size(X));
  Y = Y + T(2)*ones(size(Y));
  Z = Z + T(3)*ones(size(Z));

% end translateXYZ

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Vert_quad, Face_quad, colors_quad] = quadrotorVertFace(P)
% Vertices = [x,y,z] position of each vertex
% Faces = defines how the vertices are connnected for form faces.  Each set
% of our vertices defines one face.


%--------- vertices and faces for center pod ------------
% vertices of the center pod
Vert_center = [...
    P.w/2, P.w/2, P.w/2;...
    P.w/2, P.w/2, -P.w/2;...
    P.w/2, -P.w/2, -P.w/2;...
    P.w/2, -P.w/2, P.w/2;...
    -P.w/2, P.w/2, P.w/2;...
    -P.w/2, P.w/2, -P.w/2;...
    -P.w/2, -P.w/2, -P.w/2;...
    -P.w/2, -P.w/2, P.w/2;...
    ];
% define faces of center pod
Face_center = [...
        1, 2, 3, 4;... % front
        5, 6, 7, 8;... % back
        1, 4, 8, 5;... % top
        8, 4, 3, 7;... % right 
        1, 2, 6, 5;... % left
        2, 3, 7, 6;... % bottom
        ];
    
%--------- vertices and faces for connecting rods ------------    
% vertices for front rod
Vert_rod_front = [...
    P.w/2, P.lw/2, 0;...
    P.w/2, -P.lw/2, 0;...
    P.w/2, 0, P.lw/2;...
    P.w/2, 0, -P.lw/2;...
    P.l+P.w/2, P.lw/2, 0;...
    P.l+P.w/2, -P.lw/2, 0;...
    P.l+P.w/2, 0, P.lw/2;...
    P.l+P.w/2, 0, -P.lw/2;...
    ];
Face_rod_front = 8 + [...
        1, 2, 6, 5;... % x-y face
        3, 4, 8, 7;... % x-z face
    ];
% vertices for right rod
Vert_rod_right = Vert_rod_front*[0,1,0;1,0,0;0,0,1];
Face_rod_right = 16 + [...
        1, 2, 6, 5;... 
        3, 4, 8, 7;... 
    ];
% vertices for back rod
Vert_rod_back = Vert_rod_front*[-1,0,0;0,-1,0;0,0,1];
Face_rod_back = 24 + [...
        1, 2, 6, 5;... 
        3, 4, 8, 7;... 
    ];
% vertices for left rod
Vert_rod_left = Vert_rod_front*[0,-1,0;-1,0,0;0,0,1];
Face_rod_left = 32 + [...
        1, 2, 6, 5;... 
        3, 4, 8, 7;... 
    ];

%--------- vertices and faces for rotors ------------  
Vert_rotor = [];
for i=1:P.N,
    Vert_rotor = [Vert_rotor; P.r*cos(2*pi*i/P.N), P.r*sin(2*pi*i/P.N), 0];
end
for i=1:P.N,
    Vert_rotor = [Vert_rotor; P.r/10*cos(2*pi*i/P.N), P.r/10*sin(2*pi*i/P.N), 0];
end
Face_rotor = [];
for i=1:P.N-1,
    Face_rotor = [Face_rotor; i, i+1, P.N+i+1, P.N+i];
end
Face_rotor = [Face_rotor; P.N, 1, P.N+1, 2*P.N];

% front rotor
Vert_rotor_front = Vert_rotor + repmat([P.w/2+P.l+P.r, 0, 0],2*P.N,1);
Face_rotor_front = 40 + Face_rotor;
% right rotor
Vert_rotor_right = Vert_rotor + repmat([0, P.w/2+P.l+P.r, 0],2*P.N,1);
Face_rotor_right = 40 + 2*P.N + Face_rotor;
% back rotor
Vert_rotor_back = Vert_rotor + repmat([-(P.w/2+P.l+P.r), 0, 0],2*P.N,1);
Face_rotor_back = 40 + 2*P.N + 2*P.N + Face_rotor;
% left rotor
Vert_rotor_left = Vert_rotor + repmat([0, -(P.w/2+P.l+P.r), 0],2*P.N,1);
Face_rotor_left = 40 + 2*P.N + 2*P.N + 2*P.N + Face_rotor;

% collect all of the vertices for the quadrotor into one matrix
Vert_quad = [...
    Vert_center; Vert_rod_front; Vert_rod_right; Vert_rod_back;...
    Vert_rod_left; Vert_rotor_front; Vert_rotor_right;...
    Vert_rotor_back; Vert_rotor_left...
    ];
% collect all of the faces for the quadrotor into one matrix
Face_quad = [...
    Face_center; Face_rod_front; Face_rod_right; Face_rod_back;...
    Face_rod_left; Face_rotor_front; Face_rotor_right;...
    Face_rotor_back; Face_rotor_left...
    ];

myred = [1, 0, 0];
mygreen = [0, 1, 0];
myblue = [0, 0, 1];
myyellow = [1,1,0];

colors_quad = [...
    myblue;... % fuselage front
    myblue;... % back
    myyellow;... % top
    myblue;... % right
    myblue;... % left
    myblue;... % bottom
    mygreen;... % rod front
    mygreen;... %
    mygreen;... % rod right
    mygreen;... %
    mygreen;... % rod back
    mygreen;... %
    mygreen;... % rod left
    mygreen;... %
    ];
for i=1:P.N,
    colors_quad = [colors_quad; mygreen];   % front rotor
end
for i=1:P.N,
    colors_quad = [colors_quad; myblue];  % right rotor
end
for i=1:P.N,
    colors_quad = [colors_quad; myblue];  % left rotor
end
for i=1:P.N,
    colors_quad = [colors_quad; myblue];  % back rotor
end

% end quadrotorVertFace

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Vert_rails, Face_rails, colors_rails] = railVertFace(P)
% Vertices = [x,y,z] position of each vertex
% Faces = defines how the vertices are connnected for form faces.  Each set
% of our vertices defines one face.

%-------vertices and faces for rails--------------
% vertices for right rail
Vert_rail_right = [...
    P.w/2+P.l, P.l_rail/2, P.w_rail;...
    P.w/2+P.l, P.l_rail/2, 0;...
    P.w/2+P.l, -P.l_rail/2, 0;...
    P.w/2+P.l, -P.l_rail/2, P.w_rail;...
    P.w/2+P.l+P.w_rail, P.l_rail/2, 0;...
    P.w/2+P.l+P.w_rail, -P.l_rail/2, 0;...
    ];
Face_rail_right = [...
        1, 2, 3, 4;... % x-y face
        5, 2, 3, 6;... % x-z face
    ];

% vertices for left rail
Vert_rail_left = Vert_rail_right*[-1,0,0;0,1,0;0,0,1];
Face_rail_left = 6 + [...
        1, 2, 3, 4;... % x-y face
        5, 2, 3, 6;... % x-z face
    ];

% collect vertices and faces
Vert_rails = [Vert_rail_right; Vert_rail_left];
Face_rails = [Face_rail_right; Face_rail_left];

colors_rails = [...
    P.myyellow;...% right rail
    P.myyellow;...%
    P.myyellow;...% left rail
    P.myyellow;...%
    ];

% end railVertFace

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Vert_fov, Face_fov, colors_fov] = fovVertFace(pn,pe,h,phi,theta,psi,P,C)

%-------vertices and faces for camera field-of-view --------------
% vertices 
Vert_fov = [...
    pn, pe, h;...
    pn+h*tan(theta+C.cam_fov/2), pe+h*tan(phi+C.cam_fov/2), 0;...
    pn+h*tan(theta+C.cam_fov/2), pe+h*tan(phi-C.cam_fov/2), 0;...
    pn-h*tan(-theta+C.cam_fov/2), pe+h*tan(phi+C.cam_fov/2), 0;...
    pn-h*tan(-theta+C.cam_fov/2), pe+h*tan(phi-C.cam_fov/2), 0;...
    ];
Vert_fov = rotateVert(Vert_fov,0,0,psi);
Face_fov = [...
        1, 1, 2, 2;... % x-y face
        1, 1, 3, 3;... % x-y face
        1, 1, 4, 4;... % x-y face
        1, 1, 5, 5;... % x-y face
        2, 3, 5, 4;... % x-y face
    ];

colors_fov = [P.myblue; P.myblue; P.myblue; P.myblue; P.myyellow];


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [X,Y,Z] = targetXYZ
  
  pts = [...
      2, 0, 0;...
      0, 2, 0;...
      0, 1, 0;...
      -2, 1, 0;...
      -2, -1, 0;...
      0, -1, 0;...
      0, -2, 0;...
      2, 0, 0;...
      ];
  X = pts(:,1);
  Y = pts(:,2);
  Z = pts(:,3);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function handle = targetPlot(tn,te,td,tpsi,size,handle,color,mode)

  % get the target data points
  [X,Y,Z] = targetXYZ;
  
  % scale by size
  X = X*size;
  Y = Y*size;
  Z = Z*size;
  
  % rotate by tpsi
  [X,Y,Z] = rotateXYZ(X,Y,Z,0,0,tpsi);
  
  % translate by [tn, te, td]
  [X,Y,Z] = translateXYZ(X,Y,Z,[tn,te,td]);
  
  if isempty(handle),
    handle = patch(X, Y, Z, color, 'EraseMode', mode);
  else
    set(handle,'XData',X,'YData',Y,'ZData',Z);
    drawnow
  end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function handle = targetCamPlot(tn,te,td,tpsi,size,handle,color,mode)

  % get the target data points
  [X,Y,Z] = targetXYZ;
  
  % scale by size
  X = X*size;
  Y = Y*size;
  Z = Z*size;
  
  % rotate by tpsi
  [X,Y,Z] = rotateXYZ(X,Y,Z,0,0,tpsi);
  
  % translate by [tn, te, td]
  [X,Y,Z] = translateXYZ(X,Y,Z,[tn,te,td]);
  
  if isempty(handle),
    handle = patch(X, Y, Z, color, 'EraseMode', mode);
  else
    set(handle,'XData',X,'YData',Y,'ZData',Z);
    drawnow
  end