plotquadrotor.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);           
  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(1.0*[-1, 1, -1, 1, -.1, 1.9]) % units are centimeters
    set(gca,'visible','off')
    fig_quadrotor = quadrotorPlot(pn, pe, pd, 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)
%    view(0,90)  % top down view to check heading hold loop
%    view(0,0)   % side view to check altitude hold loop
    

    % plot camera view
    
    handle=subplot('position',[0.61, 0.61, 0.3, 0.3]);
      tmp = C.cam_pix/2;
      set(handle,'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, pd, 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, pd, phi, theta, psi, C, handle, mode);
  % plot of quadrotor in contraption - rolling motion

  psi = -psi;
  
  %----quadrotor vertices------
  P = defineParameters;
  [Vert_quad, Face_quad, colors_quad]       = quadrotorVertFace(P);
  % rotate vertices by phi, theta, psi
  Vert_quad = rotateVert(Vert_quad, phi, theta, psi);
  % transform vertices from NED to XYZ
  R = [...
      0, 1, 0;...
      1, 0, 0;...
      0, 0, 1;...
      ];
  Vert_quad = Vert_quad*R;
  % translate vertices in XYZ
  Vert_quad = translateVert(Vert_quad, [pe; pn; -pd]);

  %----field-of-view vertices------
  [Vert_fov, Face_fov, colors_fov]          = fovVertFace(pe,pn,pd,phi,theta,psi,P,C);

  % 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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%