gusucode.com > Simulink——飞行器轨迹恢复源码程序 > Simulink——飞行器轨迹恢复源码程序/Aerial_recovery_demo/mav_dynamics.m

    function [sys,x0,str,ts] = mav_dynamics(t,x,u,flag,P)
% Simple UAV Dynamics

switch flag,

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

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

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

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

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

%
% 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  = 9;
sizes.NumDiscStates  = 0;
sizes.NumOutputs     = 9;
sizes.NumInputs      = 5;
sizes.DirFeedthrough = 0;
sizes.NumSampleTimes = 1;   % at least one sample time is needed

sys = simsizes(sizes);

%
% initialize the initial conditions
%
x0  = [...
    P.mav.n;...
    P.mav.e;...
    P.mav.d;...
    P.mav.chi;...
    P.mav.V;...
    P.mav.phi;...
    P.mav.gam;...
    P.mav.az;...
    P.mav.el;...
    ];

%
% 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,u,P)
 % relabel states
 n  = x(1);
 e  = x(2);
 d  = x(3); 
 chi = x(4);
 V   = x(5);
 phi = x(6);
 gam = x(7);
 az  = x(8);
 el  = x(9);

 % uav kinematics
 ndot  = V*cos(chi)*cos(gam)+P.wind.n;  % northing
 edot  = V*sin(chi)*cos(gam)+P.wind.e;  % easting
 ddot  = -V*sin(gam);           % altitude
 if ((V>=P.mav.Vbar_up)&(u(1)>0))|((V<=-P.mav.Vbar_low)&( u(1)<0)) % keep velocity within limits
      Vdot = 0;                 
 else
     Vdot = sat(u(1), P.mav.Vdotbar);  % velocity 
 end
 chidot = (P.g/V)*tan(phi);   % coordinated turn assumption
 phidot = lim_sat(phi, u(2), P.mav.phibar, P.mav.phidotbar); % command roll rate
 gamdot = lim_sat(gam, u(3)/V, P.mav.gambar, P.mav.gamdotbar); % command path angle rate
 
 % azimuth kinematics
 azdot  = lim_sat(az,  u(4), P.mav.gim.azbar,  P.mav.gim.azdotbar);  % command azimuth rate
 eldot  = lim_sat(el,  u(5), P.mav.gim.elbar,  P.mav.gim.eldotbar);  % command elevation rate
 
 
sys = [ndot; edot; ddot; chidot; Vdot; phidot; gamdot; azdot; eldot];

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

sys = [x];  % output all UAV states

% 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

%
%=============================================================================
% lim_sat
% the integration variable is limited to be within a certain interval and
% the rate of increase is saturated
%=============================================================================
%
function state_dot = lim_sat(state, input, state_limit, rate_limit)

  if ((state>=state_limit)&(input>0))|((state<=-state_limit)&( input<0)) 
     state_dot = 0;
 else
     state_dot = sat(input, rate_limit);
 end


%
%=============================================================================
% sat
% saturate the input u at the constraint c
%=============================================================================
%
function out = sat(u,c)

 if u>c,
     out=c;
 elseif u<-c,
     out=-c;
 else
     out=u;
 end

% end sat