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