%******************************
%       FLYING QUALITIES
%******************************


%        MARK PETERS
%******************************

% Take whatever system is in the workspace
% and determine the stability and control
% coefficients. 


%******************************
%       MASS PROPERTIES
%******************************



% Nominal Mass 

   nm = 0.5286;

% Nominal moment of intertia

   nIyy=1.03395;

% nominal cg location

    ncg = 15.46;

% front mass 

      fw = 0.0;      

%  front moment arm in inches

      fm = 1.0/12;

%  Rear mass 

      rw = oz/16/32.2;

       RW = rw*32.2;
  % Rear moment arm in inches

       rm = 53.0;


% Calculations 
%**********************************

% Total Mass 

    m = fw+rw+nm;
 
M =32.2*m; 

% Total cg

 cg = (fw*fm+rm*rw+ncg*nm)/m;  

 CG =cg;


% Total Iyy

Iyy = nm*((ncg-cg)/12)^2 + fw*((ncg-fm)/12)^2 + rw*((ncg-rm)/12)^2;

Iyy =Iyy +nIyy;

IYY=Iyy;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%**********************************
%       CMa    derivative
%********************************** 

   lhp = 41.93;

   lh = (lhp-(cg-15))/12;

   Vh = lh*St/cw/Sw;

 CMa = 4.39*(cg-15.5)/15 - Vh*Clat*(1-deda);   
 CMA=CMa;

 % CLa ............ CL alpha derivative

   CLa = Claw + Clat*St/Sw*(1-deda);
   CLA=CLa;


% CMad............ CM alpha dot derivative

   CMad = -2*nh*Clat*Vh*lh/cw*deda;
   CMAD=CMad;

% CMq ............ CMq derivative

  CMq = CMad/deda;
  CMQ = CMq; 




  Q = 0.5*rho*Vt*Vt;

  K1 = Q*Sw*cw/Iy;

  Ma = CMa*K1;

  Mq = CMq*cw*K1/2/Vt;

  Mad = CMad*K1*cw/2/Vt;

  Za  = -(CLa+CD)*Q*Sw/m;

  Zde = -CLde*Q*Sw/m;

  Mde = CMde*Q*Sw*cw/Iy;



%                 STATE SPACE FORM
%**************************************************

    A = [ Za  Vt; Mad*Za+Vt*Ma Mad*Vt+Mq*Vt]/Vt;

    B = [ Zde ; Mad*Zde+Mde*Vt]/Vt;

%    C = [1 0; 0 1];

%    D =[0 ; 0];


      g =32.2;

      sf =Vt/g;


      B = [B(1,1) 0.0; B(2,1) 0.0];

      C = [57.3 -57.3*1.50/Vt; 0 57.3; -sf*A(1,1)-.122/g*A(2,1) ...
        -sf*(A(1,2)-1)-.122/g*A(2,2)];

      D = [0 3.0 ; 0 0; -B(1,1)*sf-.122*B(2,1)/g 0.8];

% *************************************************
 

%                   FEED BACK 
%**************************************************
 

  %  k =+0.0;
     k = 0.161;
   % k = -0.2800;


   K = [0.0 j*k/57.3 0.0; 0.0 0.0 0.0];

   Acl = (A-B*K*C);

    
   Zacl = Acl(1,1)*Vt;
   
   [num,den]= ss2tf(Acl,B,C,D,1);

    evalues=eig(Acl);
 
     [w,z]= damp(Acl);

    WN=w(1);
    Z = z(1);

    Tt2 = -Vt/Zacl;
    TT2 =Tt2;
    NA=Vt/Tt2/32.2;    
    
    rt=Acl(2,1);


%   figure(1)

%   hold on 

%   plot(NA,WN,'o')

%   hold off

%   figure(2)

%   hold on

%   plot(Z,WN*TT2,'o')

%   hold off 


    figure(3)

     hold on

     plot(Z,WN, 'o')

     hold off 

    
     
 
