%_______________________________________________________________________________
% This program calculates the aircraft's dimensional longitudinal stability and % control derivatives and outputs the A and B matrices for the state vector
% [u/uo; w/uo; q; theta], where w/uo = alpha.  Notation is from Nelson, "Flight Stability and Automatic Control"
%_______________________________________________________________________________      
        disp('  '); disp('Longitudinal Dynamics'),
% Import flight condition, mass and nondimensionalaerodynamic data
     load newname
%_______________________________________________________________________________
% COMPUTE DIMENSIONAL LONGITUDINAL DERIVATIVES
% Taken from Table 4.2 of Nelson
%_______________________________________________________________________________
% X-FORCE DERIVATIVES
					Xu = -(CDm*M +2*(CDo+CLo*tan(theta0)))*QS/(m*u0);
					Xw = -(CDalpha-CLo)*QS/(m*u0);
					Xdeltae = -CDdeltae*QS/m;
% Z-FORCE DERIVATIVES
					Zu = -(2*CLo+CLm*M)*QS/(m*u0);
					Zw = -(CLalpha+CDo)*QS/(m*u0); Zalpha = u0*Zw;
					Zwdot = -CLalphadot*(cbar/(2*u0))*QS/(m*u0); Zalphadot = u0*Zwdot;    
     				Zq = -CLq*(cbar/(2*u0))*QS/m;
					Zdeltae = -CLdeltae*QS/m;
% PITCHING MOMENT DERIVATIVES
					Mu = (CMm*M)*(QS*cbar/(u0*Iy)); 
					Mw = CMalpha*(QS*cbar/(u0*Iy)); Malpha = u0*Mw;
					Mwdot = CMalphadot*(cbar/(2*u0))*(QS*cbar/(u0*Iy)); Malphadot = u0*Mwdot;
     				Mq = CMq*cbar/(2*u0)*(QS*cbar/Iy);
					Mdeltae = CMdeltae*(QS*cbar/Iy);
%_______________________________________________________________________________
% CONSTRUCT LONGITUDINAL A AND B MATRICES
% Extended to include non-level flight, Zwdot and Zq
% Order of state variables are Xlong = [u/u0;w/u0;q;theta] = [u/u0;alpha;q;theta]
%__________________________________________________________________________    
%
Zwdp = 1/(1+Zwdot);
Along=	[Xu Xw 0 -g*cos(theta0)/u0
		Zu*Zwdp Zw*Zwdp (1+Zq/u0)*Zwdp -(g*sin(theta0)/u0)*Zwdp
		(Mu+Mwdot*Zu*Zwdp)*u0 (Mw+Mwdot*Zw*Zwdp)*u0 Mq+Mwdot*(u0+Zq)*Zwdp 0
		0 0 1 0];
Blong = [Xdeltae/u0; Zdeltae*Zwdp/u0; Mdeltae+Mwdot*Zdeltae*Zwdp; 0];
[eigenvectors, eigenvalues] = eig(Along);
damp(eigenvalues),
%
%Calculate approximate longitudinal modes
%
disp('\\\Approximate longitudinal modes\\\')
Mqstar=Mq+Mwdot*u0;
wph=sqrt(-32.2*Zu/u0), zetaph=-Xu/2/wph;
wsp=sqrt(Zw*Mq-Mw*u0), zetasp=-(Zw+Mqstar)/2/wsp;