plotquadstatevariables.asv

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

function [sys,x0,str,ts] = plotuavstatevariables(t,x,u,flag,Ts,C)
%
% modified 2/17/03 - Randy Beard
% modified 11/30/07 - RB

switch flag,

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

  %%%%%%%%%%
  % 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(Ts)

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


sys = simsizes(sizes);

x0 = zeros(sizes.NumDiscStates,1);
str = [];
ts  = [Ts 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_handles  = x(2:end);      % figure handles
  NN = 0;
  pn         = uu(1+NN);     % actual states
  pe         = uu(2+NN);
  pd         = uu(3+NN);
  u          = uu(4+NN);
  v          = uu(5+NN);
  w          = uu(6+NN);
  phi        = uu(7+NN);          
  theta      = uu(8+NN);          
  psi        = uu(9+NN);          
  p          = uu(10+NN);
  q          = uu(11+NN);
  r          = uu(12+NN);
  NN = NN + 12;
  pn_hat         = uu(1+NN);     % estimated states
  pe_hat         = uu(2+NN);
  pd_hat         = uu(3+NN);
  u_hat          = uu(4+NN);
  v_hat          = uu(5+NN);
  w_hat          = uu(6+NN);
  phi_hat        = uu(7+NN);          
  theta_hat      = uu(8+NN);          
  psi_hat        = uu(9+NN);          
  p_hat          = uu(10+NN);
  q_hat          = uu(11+NN);
  r_hat          = uu(12+NN);
  NN = NN + 12;
  pn_d         = uu(1+NN);     % desired states
  pe_d         = uu(2+NN);
  pd_d         = uu(3+NN);
  u_d          = uu(4+NN);
  v_d          = uu(5+NN);
  w_d          = uu(6+NN);
  phi_d        = uu(7+NN);          
  theta_d      = uu(8+NN);          
  psi_d        = uu(9+NN);          
  p_d          = uu(10+NN);
  q_d          = uu(11+NN);
  r_d          = uu(12+NN);
  NN = NN + 12;
  pwm_f = sat(uu(NN+1),0,1);  % pwm commend for front motor
  pwm_r = sat(uu(NN+2),0,1);  % pwm commend for right motor
  pwm_b = sat(uu(NN+3),0,1);  % pwm commend for back motor
  pwm_l = sat(uu(NN+4),0,1);  % pwm commend for left motor
  % computer thrust produced by each motor
  thrust_front = C.kmotor_lift*pwm_f;
  thrust_right = C.kmotor_lift*pwm_r;
  thrust_back  = C.kmotor_lift*pwm_b;
  thrust_left  = C.kmotor_lift*pwm_l;
  % computer torque produced by each motor
  torque_front = C.kmotor_drag*pwm_f;
  torque_right = C.kmotor_drag*pwm_r;
  torque_back  = C.kmotor_drag*pwm_b;
  torque_left  = C.kmotor_drag*pwm_l;
  % compute body frame torques
  F = thrust_front + thrust_right + thrust_left + thrust_back;
  T_p = C.l*(thrust_left - thrust_right); % torque about roll axis
  T_q = C.l*(thrust_front - thrust_back); % torque about pitch axis
  T_r = torque_front + torque_right - torque_back - torque_left;  % torque about yaw axis

  if initialize==0, 
    % initialize the plot
    initialize = 1;
    figure(2), clf
    set(gcf,'DoubleBuffer','on')

    % state plots
    % pn
    NN = 0;
    subplot(8,2,1)
    hold on
    fig_handles(1+NN) = graph_y(t,pn,[],'b');
    fig_handles(2+NN) = graph_y(t,pn_hat,[],'g');
    fig_handles(3+NN) = graph_y(t,pn_d,[],'r');   
    ylabel('p_n')
    set(get(gca,'YLabel'),'Rotation',0.0);

    NN = NN+3;
    subplot(8,2,3)
    hold on
    fig_handles(1+NN) = graph_y(t,pe,[],'b');
    fig_handles(2+NN) = graph_y(t,pe_hat,[],'g');
    fig_handles(3+NN) = graph_y(t,pe_d,[],'r');   
    ylabel('p_e')
    set(get(gca,'YLabel'),'Rotation',0.0);

    NN = NN+3;
    subplot(8,2,5)
    hold on
    fig_handles(1+NN) = graph_y(t,-pd,[],'b');
    fig_handles(2+NN) = graph_y(t,-pd_hat,[],'g');
    fig_handles(3+NN) = graph_y(t,-pd_d,[],'r');   
    ylabel('h')
    set(get(gca,'YLabel'),'Rotation',0.0);

    NN = NN+3;
    subplot(8,2,7)
    hold on
    fig_handles(1+NN) = graph_y(t,u,[],'b');
    fig_handles(2+NN) = graph_y(t,u_hat,[],'g');
    fig_handles(3+NN) = graph_y(t,u_d,[],'r');   
    ylabel('u')
    set(get(gca,'YLabel'),'Rotation',0.0);

    NN = NN+3;
    subplot(8,2,9)
    hold on
    fig_handles(1+NN) = graph_y(t,v,[],'b');
    fig_handles(2+NN) = graph_y(t,v_hat,[],'g');
    fig_handles(3+NN) = graph_y(t,v_d,[],'r');   
    ylabel('v')
    set(get(gca,'YLabel'),'Rotation',0.0);

    NN = NN+3;
    subplot(8,2,11)
    hold on
    fig_handles(1+NN) = graph_y(t,w,[],'b');
    fig_handles(2+NN) = graph_y(t,w_hat,[],'g');
    fig_handles(3+NN) = graph_y(t,w_d,[],'r');   
    ylabel('w')
    set(get(gca,'YLabel'),'Rotation',0.0);

    % psi
    NN = NN+3;
    subplot(8,2,2)
    hold on
    fig_handles(1+NN) = graph_y(t,(180/pi)*phi,[],'b');
    fig_handles(2+NN) = graph_y(t,(180/pi)*phi_hat,[],'g');
    fig_handles(3+NN) = graph_y(t,(180/pi)*phi_d,[],'r');   
    ylabel('\phi')
    set(get(gca,'YLabel'),'Rotation',0.0);

    % psi
    NN = NN+3;
    subplot(8,2,4)
    hold on
    fig_handles(1+NN) = graph_y(t,(180/pi)*theta,[],'b');
    fig_handles(2+NN) = graph_y(t,(180/pi)*theta_hat,[],'g');
    fig_handles(3+NN) = graph_y(t,(180/pi)*theta_d,[],'r');   
    ylabel('\theta')
    set(get(gca,'YLabel'),'Rotation',0.0);

    % psi
    NN = NN+3;
    subplot(8,2,6)
    hold on
    fig_handles(1+NN) = graph_y(t,(180/pi)*psi,[],'b');
    fig_handles(2+NN) = graph_y(t,(180/pi)*psi_hat,[],'g');
    fig_handles(3+NN) = graph_y(t,(180/pi)*psi_d,[],'r');   
    ylabel('\psi')
    set(get(gca,'YLabel'),'Rotation',0.0);

    % p
    NN = NN+3;
    subplot(8,2,8)
    hold on
    fig_handles(1+NN) = graph_y(t,(180/pi)*p,[],'b');
    fig_handles(2+NN) = graph_y(t,(180/pi)*p_hat,[],'g');
    fig_handles(3+NN) = graph_y(t,(180/pi)*p_d,[],'r');
    ylabel('p')
    set(get(gca,'YLabel'),'Rotation',0.0);    

    % q
    NN = NN+3;
    subplot(8,2,10)
    hold on
    fig_handles(1+NN) = graph_y(t,(180/pi)*q,[],'b');
    fig_handles(2+NN) = graph_y(t,(180/pi)*q_hat,[],'g');
    fig_handles(3+NN) = graph_y(t,(180/pi)*q_d,[],'r');
    ylabel('q')
    set(get(gca,'YLabel'),'Rotation',0.0);    

    % r
    NN = NN+3;
    subplot(8,2,12)
    hold on
    fig_handles(1+NN) = graph_y(t,(180/pi)*r,[],'b');
    fig_handles(2+NN) = graph_y(t,(180/pi)*r_hat,[],'g');
    fig_handles(3+NN) = graph_y(t,(180/pi)*r_d,[],'r');
    ylabel('r')
    set(get(gca,'YLabel'),'Rotation',0.0);    

    % F
    NN = NN+3;
    subplot(8,8,13)
    hold on
    fig_handles(1+NN) = graph_y(t,F,[],'b');
    ylabel('F')
    set(get(gca,'YLabel'),'Rotation',0.0);    

    % T_p
    NN = NN+1;
    subplot(8,8,15)
    hold on
    fig_handles(1+NN) = graph_y(t,T_p,[],'b');
    ylabel('Tp')
    set(get(gca,'YLabel'),'Rotation',0.0);    

      % T_q
    NN = NN+1;
    subplot(8,8,14)
    hold on
    fig_handles(1+NN) = graph_y(t,T_q,[],'b');
    ylabel('Tq')
    set(get(gca,'YLabel'),'Rotation',0.0);    

    % T_r
    NN = NN+1;
    subplot(8,8,16)
    hold on
    fig_handles(1+NN) = graph_y(t,T_r,[],'b');
    ylabel('Tr')
    set(get(gca,'YLabel'),'Rotation',0.0);    

  else  % do this at every time step
      
    % psi
    NN = 0;    
    graph_y(t, (180/pi)*psi,     fig_handles(1+NN));
    graph_y(t, (180/pi)*psi_hat, fig_handles(2+NN));
    graph_y(t, (180/pi)*psi_d,   fig_handles(3+NN));
    % drawnow

    % r
    NN = NN+3;
    graph_y(t, (180/pi)*r,     fig_handles(1+NN));
    graph_y(t, (180/pi)*r_hat, fig_handles(2+NN));
    graph_y(t, (180/pi)*r_d,   fig_handles(3+NN));
    % drawnow

    % r
    NN = NN+3;
    graph_y(t, (180/pi)*r,     fig_handles(1+NN));
    graph_y(t, (180/pi)*r_hat, fig_handles(2+NN));
    graph_y(t, (180/pi)*r_d,   fig_handles(3+NN));
    % drawnow
    % r
    NN = NN+3;
    graph_y(t, (180/pi)*r,     fig_handles(1+NN));
    graph_y(t, (180/pi)*r_hat, fig_handles(2+NN));
    graph_y(t, (180/pi)*r_d,   fig_handles(3+NN));
    % drawnow
    % Tr
    NN = NN+3;
    graph_y(t, T_r,             fig_handles(1+NN));
    % drawnow
    
end
  
  xup = [initialize; fig_handles];


%end mdlUpdate

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% graph y with lable mylabel
function handle = graph_y(t, y, handle, color)
  
  if isempty(handle),
    handle    = plot(t,y,color);
  else
    set(handle,'Xdata',[get(handle,'Xdata'),t]);
    set(handle,'Ydata',[get(handle,'Ydata'),y]);
    %drawnow
  end




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% plot the variable y in blue, its estimated value yhat in green, and its 
% desired value yd in red, lab is the label on the graph
function [handle_y, handle_yhat, handle_yd] = graph_y_yhat_yd(t, y, yhat, yd, lab, handle)
  
  if isempty(handle),
    handle_y    = plot(t,y,'b');
    handle_yhat = plot(t,yhat,'g');
    handle_yd   = plot(t,yd,'r');
    ylabel(lab)
    set(get(gca,'YLabel'),'Rotation',0.0);
  else
    set(handle_y,'Xdata',[get(handle_y,'Xdata'),t]);
    set(handle_y,'Ydata',[get(handle_y,'Ydata'),y]);
    set(handle_yhat,'Xdata',[get(handle_yhat,'Xdata'),t]);
    set(handle_yhat,'Ydata',[get(handle_yhat,'Ydata'),yhat]);
    set(handle_yd,'Xdata',[get(handle_yd,'Xdata'),t]);
    set(handle_yd,'Ydata',[get(handle_yd,'Ydata'),yd]);     
    %drawnow
  end

%
%=============================================================================
% sat
% saturates the input between high and low
%=============================================================================
%
function out=sat(in, low, high)

  if in < low,
      out = low;
  elseif in > high,
      out = high;
  else
      out = in;
  end

% end sat