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