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

    function [sys,x0,str,ts] = mothership_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,P);

  %%%%%%%%%%%%%%%%%%%%%%%
  % 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  = 6;
sizes.NumDiscStates  = 0;
sizes.NumOutputs     = 6 ... % states 
                       +3;  % mothership accelerations
sizes.NumInputs      = 2;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;   % at least one sample time is needed

sys = simsizes(sizes);

%
% initialize the initial conditions
%
  x0  = [... 
      P.mothership.n;...
      P.mothership.e;...
      P.mothership.d;...
      P.mothership.chi;...
      P.mothership.phi;...
      P.mothership.gam;...
      ];

%
% 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,P)
    NN = 0;
    pn  = x(1+NN);
    pe  = x(2+NN);
    pd  = x(3+NN); 
    chi = x(4+NN);
    phi = x(5+NN);
    gam = x(6+NN);
    NN = 0;
    u_phi = uu(1+NN);
    u_gam = uu(2+NN);
    V = P.mothership.V;

    % uav kinematics
    pndot  = V*cos(chi)*cos(gam);  % northing
    pedot  = V*sin(chi)*cos(gam);  % easting
    pddot  = -V*sin(gam);           % altitude
    chidot = (P.g/V)*tan(phi);   % coordinated turn assumption
    phidot = lim_sat(phi, u_phi, P.mothership.phibar, P.mothership.phidotbar); % command roll rate
    gamdot = lim_sat(gam, u_gam/V, P.mothership.gambar, P.mothership.gamdotbar); % command path angle rate
 
    sys = [pndot; pedot; pddot; chidot; phidot; gamdot];

% 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,uu,P)
    NN = 0;
    n  = x(1+NN);
    e  = x(2+NN);
    d  = x(3+NN); 
    chi = x(4+NN);
    phi = x(5+NN);
    gam = x(6+NN);
    NN = 0;
    u_phi = uu(1+NN);
    u_gam = uu(2+NN);
    V = P.mothership.V;

    % compute accelerations on the mothership
    nddot = -P.g*sin(chi)*cos(gam)*tan(phi) - V*u_gam*cos(chi)*sin(gam);
    eddot =  P.g*cos(chi)*cos(gam)*tan(phi) - V*u_gam*sin(chi)*sin(gam);
    dddot = -V*u_gam*cos(gam);

sys = [x; nddot; eddot; dddot];  % output UAV states + mothership accelerations


% 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