소형 무인 비행체 5 - 과제 2

5.2 mavsim_trim.slx복붙하고 동작하도록 파일 수정해라

 

 

% compute trim conditions using 'mavsim_chap5_trim.slx'
% nominal airspeed P.Va0 specified above with aircraft parameters
gamma = 0*pi/180;  % desired flight path angle (radians)
R     = 10000000000;        % desired radius (m) - use (+) for right handed orbit, 
                            %                          (-) for left handed orbit
Va = 25;


% set initial conditions 
x0 = [];
% specify which states to hold equal to the initial conditions
ix = [];

% specify initial inputs 
u0 = [...
    0;... % 1 - delta_e
    0;... % 2 - delta_a
    0;... % 3 - delta_r
    1;... % 4 - delta_t
    ];
% specify which inputs to hold constant
iu = [];

% define constant outputs
y0 = [...
    Va;...       % 1 - Va
    0;...        % 2 - alpha
    0;...        % 3 - beta
    ];
% specify which outputs to hold constant
iy = [1,3];

% define constant derivatives
dx0 = 

if R~=Inf, dx0(9) = Va*cos(gamma)/R; end  % 9 - psidot
% specify which derivaties to hold constant in trim algorithm
idx = [3; 4; 5; 6; 7; 8; 9; 10; 11; 12];

% compute trim conditions
[x_trim,u_trim,y_trim,dx_trim] = trim('mavsim_trim',x0,u0,y0,ix,iu,iy,dx0,idx);

% check to make sure that the linearization worked (should be small)
norm(dx_trim(3:end)-dx0(3:end))

% P.u_trim = u_trim;
% P.x_trim = x_trim;

% set initial conditions to trim conditions
% initial conditions
MAV.pn0    = 0;           % initial North position
MAV.pe0    = 0;           % initial East position
MAV.pd0    = -200;        % initial Down position (negative altitude)
MAV.u0     = x_trim(4);   % initial velocity along body x-axis
MAV.v0     = x_trim(5);   % initial velocity along body y-axis
MAV.w0     = x_trim(6);   % initial velocity along body z-axis
MAV.phi0   = x_trim(7);   % initial roll angle
MAV.theta0 = x_trim(8);   % initial pitch angle
MAV.psi0   = x_trim(9);   % initial yaw angle
MAV.p0     = x_trim(10);  % initial body frame roll rate
MAV.q0     = x_trim(11);  % initial body frame pitch rate
MAV.r0     = x_trim(12);  % initial body frame yaw rate  

 

trim 시뮬링크 모델이랑 트림 계산  파일이 저렇게 주어졌다.

 

xdot은 다음과 같으므로

 

값들은 아래와 같이 대입하면 될거같다

 

 

구현 하긴했는데

 

compute_trim에서 자꾸 dx0이랑 시뮬링크 블록상 연속 상태 크기랑 틀렸다고 에러뜬다.

 

한참 삽질하다보니 mav_dynamics에서 오일러각대신 쿼터니언을 쓰다보니 상태가 13개가 쓰여서 그렇더라

 

다시 챕터 3의 mav_dynamics를 오일러각으로 바꿧다.

 

function [sys,x0,str,ts,simStateCompliance] = mav_dynamics(t,x,u,flag,MAV)

switch flag

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

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

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

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

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

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

  %%%%%%%%%%%%%%%%%%%%
  % Unexpected flags %
  %%%%%%%%%%%%%%%%%%%%
  otherwise
    DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag));

end

% end sfuntmpl

%
%=============================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=============================================================================
%
function [sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes(MAV)

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

sys = simsizes(sizes);

%
% initialize the initial conditions
%
x0  = [...
    MAV.pn0;...
    MAV.pe0;...
    MAV.pd0;...
    MAV.u0;...
    MAV.v0;...
    MAV.w0;...
    MAV.phi0;...
    MAV.theta0;...
    MAV.psi0;...
    MAV.p0;...
    MAV.q0;...
    MAV.r0;...
    ];

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

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

% Specify the block simStateCompliance. The allowed values are:
%    'UnknownSimState', < The default setting; warn and assume DefaultSimState
%    'DefaultSimState', < Same sim state as a built-in block
%    'HasNoSimState',   < No sim state
%    'DisallowSimState' < Error out when saving or restoring the model sim state
simStateCompliance = 'UnknownSimState';

% end mdlInitializeSizes

%
%=============================================================================
% mdlDerivatives
% Return the derivatives for the continuous states.
%=============================================================================
%
function sys=mdlDerivatives(t,x,uu, MAV)
    pn    = x(1);
    pe    = x(2);
    pd    = x(3);
    u     = x(4);
    v     = x(5);
    w     = x(6);
    phi   = x(7);
    theta= x(8);
    psi   = x(9);
    p     = x(10);
    q     = x(11);
    r     = x(12);
    fx    = uu(1);
    fy    = uu(2);
    fz    = uu(3);
    ell   = uu(4);
    m     = uu(5);
    n     = uu(6);
    
    cphi = cos(phi);
    ctheta = cos(theta);
    cpsi = cos(psi);
    sphi = sin(phi);
    stheta = sin(theta);
    spsi = sin(psi);
    
    
    pndot = u*ctheta*cpsi -v*(sphi*stheta*cpsi - cphi*spsi) +w*(cphi*stheta*cpsi + sphi*spsi);
    
    pedot = u*ctheta*spsi + v*(sphi*stheta*spsi + cphi*cpsi) + w*(cphi*stheta*spsi - sphi*cpsi);
    
    pddot = -stheta*u + v*sphi*ctheta + w*cphi *ctheta;
    
    udot = r*v - q*w + 1/MAV.mass*fx;
    
    vdot = p*w - r*u + 1/MAV.mass*fy;
    
    wdot = q*u - p*v + 1/MAV.mass*fz;
       
    phidot = p + sphi*tan(theta)*q + r*cphi*tan(theta);
    
    thetadot = q*cphi -sphi*r;
    
    psidot = sphi/ctheta*q + cphi/ctheta*r;
    
    pdot = MAV.Gamma1*p*q-MAV.Gamma2*q*r+MAV.Gamma3*ell+MAV.Gamma4*n;
    qdot = MAV.Gamma5*p*r-MAV.Gamma6*(p^2-r^2) +1/MAV.Jy*m;
    rdot = MAV.Gamma7*p*q - MAV.Gamma1*q*r + MAV.Gamma4*ell + MAV.Gamma8*n;
        

sys = [pndot; pedot; pddot; udot; vdot; wdot; phidot; thetadot; psidot; pdot; qdot; rdot];

% 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)
    y = [...
        x(1);
        x(2);
        x(3);
        x(4);
        x(5);
        x(6);
        x(7);
        x(8);
        x(9);
        x(10);
        x(11);
        x(12);
        ];
sys = y;

% 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

 

다음 값이 주어질때

트림 값

 

 

 

아래의 트림 시뮬레이션 모델에서 

 

트림 계산 입력이 아래와 같이 주어질때 

gamma = 2*pi/180;  % desired flight path angle (radians)
R     = 5000;        % desired radius (m) - use (+) for right handed orbit, 
                            %                          (-) for left handed orbit
Va = 100;

 

 

시뮬레이션