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


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

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


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

for j=0:1:1,

 for i =1:20,


% 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 = i*1.94e-03;

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

       rm = 53.0;


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

% Total Mass 

    m = fw+rw+nm;
 
M(i) =32.2*m; 

% Total cg

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

 CG(i) =cg;


% Total Iyy

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

Iyy =Iyy +nIyy;

IYY(i)=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(i)=CMa;

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

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


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

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

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

  CMq = CMad/deda;
  CMQ(i) = 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;

  KK(i,j+2) = j*k;

   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(1:2,i)=eig(Acl);
 
     [w,z]= damp(Acl);

    WN(i,j+2)=w(1);
    Z(i,j+2) = z(1);

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





 % end

hold on
 
%************************************************
%               ROOT LOCUS
%************************************************
    figure(1)
    plot(real(evalues),imag(evalues),'x')
    axis([-15 15 -15 15])
    grid on
    line([-15 15],[0 0])
    line([0 0],[-15 15])

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

  end
  end
