quadrotor_dynamics.asv

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

function [sys,x0,str,ts] = uav_dynamics(t,x,u,flag,C)
%
switch flag,

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

  %%%%%%%%%%%%%%%
  % Derivatives %
  %%%%%%%%%%%%%%%
  case 1,
    sys=mdlDerivatives(t,x,u,C);

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

  %%%%%%%%%%%
  % Outputs %
  %%%%%%%%%%%
  case 3,
    sys=mdlOutputs(t,x,u,C);

  %%%%%%%%%%%%%%%%%%%%%%%
  % GetTimeOfNextVarHit %
  %%%%%%%%%%%%%%%%%%%%%%%
  case 4,
    sys=mdlGetTimeOfNextVarHit(t,x,u);

  %%%%%%%%%%%%%
  % Terminate %
  %%%%%%%%%%%%%
  case 9,
    sys=mdlTerminate(t,x,u);

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

end

% end sfuntmpl

%
%=============================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=============================================================================
%
function [sys,x0,str,ts]=mdlInitializeSizes(C)
%
% call simsizes for a sizes structure, fill it in and convert it to a
% sizes array.
%
% Note that in this example, the values are hard coded.  This is not a
% recommended practice as the characteristics of the block are typically
% defined by the S-function parameters.
%
sizes = simsizes;

sizes.NumContStates  = 12;
sizes.NumDiscStates  = 0;
sizes.NumOutputs     = 2;
sizes.NumInputs      = 6;
sizes.DirFeedthrough = 0;
sizes.NumSampleTimes = 1;   % at least one sample time is needed

sys = simsizes(sizes);

%
% initialize the initial conditions
%
x0  = C.x0;

%
% str is always an empty matrix
%
str = [];

%
% initialize the array of sample times
%
ts  = [0 0];

% end mdlInitializeSizes

%
%=============================================================================
% mdlDerivatives
% Return the derivatives for the continuous states.
%=============================================================================
%
function sys=mdlDerivatives(t,x,uu,C)
  % process states
  pn = x(1);    % north position (m)
  pe = x(2);    % east position (m)
  pd = x(3);    % position down (negative of altitude) (m)
  u  = x(4);    % velocity along body x-axis (m/s)
  v  = x(5);    % velocity along body y-axis (m/s)
  w  = x(6);    % velocity along body z-axis (m/s)
  phi = x(7);   % roll angle (rad)
  theta = x(8); % pitch angle  (rad)
  psi = x(9);   % yaw angle (rad)
  p = x(10);    % roll rate  (rad/s)
  q = x(11);    % pitch rate (rad/s)
  r = x(12);    % yaw rate (rad/s)
  

  % process inputs
   pwm_f = sat(uu(1),0,1);  % pwm commend for front motor
   pwm_r = sat(uu(2),0,1);  % pwm commend for right motor
   pwm_b = sat(uu(3),0,1);  % pwm commend for back motor
   pwm_l = sat(uu(4),0,1);  % pwm commend for left motor
   wn    = uu(5); % wind along the North coordinate
   we    = uu(6); % wind along the East coordinate


  % 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
  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
  
  % compute body frame force
  F = thrust_front + thrust_right + thrust_back + thrust_left;

  % compute system dynamics
  pndot    = u + wn;
  pedot    = v + we;
  pddot    = w;
  udot     = C.g*tan(theta);  % need to change
  vdot     = C.g*tan(phi);    % need to change
  wdot     = C.g - F*cos(phi)*cos(theta)/C.m;
  phidot   = p;
  thetadot = q;
  psidot   = r;
  pdot     = (1/C.Jx) * T_p;
  qdot     = (1/C.Jy) * T_q;
  rdot     = (1/C.Jz) * T_r;

  % compute system dynamics
  
  xdot = [...
      pndot; pedot; pddot;...
      udot; vdot; wdot;...
      phidot; thetadot; psidot;...
      pdot; qdot; rdot...
      ];
  
sys = xdot;

% end mdlDerivatives

%
%=============================================================================
% mdlUpdate
% Handle discrete state updates, sample time hits, and major time step
% requirements.
%=============================================================================
%
function sys=mdlUpdate(t,x,u)

sys = [];

% end mdlUpdate

%
%=============================================================================
% mdlOutputs
% Return the block outputs.
%=============================================================================
%
function sys=mdlOutputs(t,x,u,C)

sys = [x(9); x(12)];
%sys = x;

% end mdlOutputs

%
%=============================================================================
% mdlGetTimeOfNextVarHit
% Return the time of the next hit for this block.  Note that the result is
% absolute time.  Note that this function is only used when you specify a
% variable discrete-time sample time [-2 0] in the sample time array in
% mdlInitializeSizes.
%=============================================================================
%
function sys=mdlGetTimeOfNextVarHit(t,x,u)

sampleTime = 1;    %  Example, set the next hit to be one second later.
sys = t + sampleTime;

% end mdlGetTimeOfNextVarHit

%
%=============================================================================
% mdlTerminate
% Perform any end of simulation tasks.
%=============================================================================
%
function sys=mdlTerminate(t,x,u)

sys = [];

% end mdlTerminate

%
%=============================================================================
% 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