function [sys,x0,str,ts] = E_aircraft(t,x,u,flag,constant,xIC)
%   function [sys,x0,str,ts] = E_aircraft(t,x,u,flag,constant,xIC)
%   Version 1.0, 2/21/01
%   S-file for six degree of freedom aircraft simulation.
%   constant is a vector of constants that describe the aircraft.
%   xIC is the initial condition on the state vector.
%   x=[U,V,W,P,Q,R,E1(Phi),E2(Theta),E3(Psi),x(eci),y(eci),z(eci)]
%   Units are [ft/sec, ft/sec, ft/sec, rad/sec, rad/sec, rad/sec, rad, rad, rad, ft, ft, ft]
%   u=[deltaE,deltaA,deltaR,bhp]
%   Units are [rad,rad,rad,bhp]

switch flag,      
  case 0,         % Initialization
    [sys,x0,str,ts]=ACInitializeSizes(xIC);
  case 1,         % Compute derivatives of continuous states
    sys=ACDerivatives(t,x,u,constant) ;
  case 2,
    sys=ACUpdate(t,x,u);
  case 3,
    sys=ACOutputs(t,x,u);  % Compute output vector
  case 4,                   % Compute time of next sample
    sys=ACGetTimeOfNextVarHit(t,x,u);
  case 9,                   % Finished. Do any needed 
    sys=ACTerminate(t,x,u);
  otherwise                 % Invalid input
    error(['Unhandled flag = ',num2str(flag)]);
end
 
%***********************************************************
%*                    ACInitializeSizes                   *
%***********************************************************
function [sys,x0,str,ts]=ACInitializeSizes(xIC)
% Return the sizes of the system vectors, initial 
% conditions, and the sample times and offets.
sizes = simsizes;   % Create the sizes structure
sizes.NumContStates  = 12;
sizes.NumDiscStates  = 0;
sizes.NumOutputs     = 20;
sizes.NumInputs      = 4;
sizes.DirFeedthrough = 0;
sizes.NumSampleTimes = 1;   % at least one sample time is needed
sys = simsizes(sizes);      % load sys with the sizes structure
x0  = xIC;    % Specify initial conditions for all states 
str = [];    % str is always an empty matrix
ts  = [0 0]; %initialize the array of sample times


%***********************************************************
%*                    ACDerivatives                       *
%***********************************************************
function sys=ACDerivatives(t,x,u,constant)
% Compute derivatives of continuous states
% Get elements of the state vector
U=x(1);   V=x(2);     W=x(3);   P=x(4);  Q=x(5);  R=x(6);
E1=x(7); E2=x(8); E3=x(9); X=x(10); Y=x(11); Z=x(12);

% Get geodetic latitude (phi), celestial longitude (lamdac), altitude (h).
[phi,lamdac,h]=e2g(X,Y,Z); % Elliptical Earth modification
[TransMatrixB2NED]=TransformB2NED(E1,E2,E3); % Elliptical Earth modification
VB=TransMatrixB2NED*[U;V;W]; % Elliptical Earth modification
VN=VB(1); % Elliptical Earth modification
VE=VB(2); % Elliptical Earth modification
a=20925646.3254593; % (ft), ellipsoid equatorial radius (semi-major axis)
e2=0.00669437999;   % e squared, e=first eccentricity of the ellipsoid
omegae=7.292115e-5; % angular velocity of the Earth
RN=a*(1-e2)/(1-e2*sin(phi)*sin(phi))^(3/2); % Elliptical Earth modification
RE=a/(1-e2*sin(phi)*sin(phi))^(1/2);        % Elliptical Earth modification

% Get rate of change of geodetic latitude
phidot=VN/(RN+h); % Elliptical Earth modification
% Get rate of change of geodetic (lamda) and celestial (lamdac) longitude
lamdadot=VE/((RE+h)*cos(phi)); % Elliptical Earth modification
lamdacdot=lamdadot+omegae;     % Elliptical Earth modification

% Compute air data
Vt=sqrt(U*U+V*V+W*W);  % ft/sec, velocity relative to the airmass
alpha=atan2(W,U);      % rad
beta=asin(V/Vt);       % rad
rho=0.00237691267925741*(1-6.87558563248308e-06*h)^(4.25591641274834); %slug/ft3, troposphere
qbar=.5*rho*Vt*Vt;     %lbf/ft^2


% Get necessary constants
mass=constant(3);  S=constant(20); cbar=constant(21);
CLalphadot=constant(31);
c1=constant(11); c2=constant(12); c3=constant(13); c4=constant(14); 
c5=constant(15); c6=constant(16); c7=constant(17); c8=constant(18); 
c9=constant(19);

sE1=sin(E1);
cE1=cos(E1);
sE2=sin(E2);
cE2=cos(E2);
tE2=sE2/cE2;
sE3=sin(E3);    % Elliptical Earth modification
cE3=cos(E3);    % Elliptical Earth modification
sphi=sin(phi);  % Elliptical Earth modification
cphi=cos(phi);  % Elliptical Earth modification

% Force equations of motion
% Get aerodynamic forces Fa
[Fax,Fay,Fazprime,CX,CY,CZprime]=aeroforce(x,u,constant,alpha,beta,Vt,qbar);
% Get thrust forces and Moments
[Ftx,Fty,Ftz,Lt,Mt,Nt]=thrust(u,constant,Vt);

% Note gprime is normal to the ellipsoid
% where gprime=g-omegae X (omegae X p))
% The formula for gprime is given in WGS-84 (NIMA TR8350.2, page 4-1, 4-2).
gprime=gravity(phi,h); % Elliptical Earth modification
% Compute modification terms for force equations of motion for elliptical Earth.
% This added term is -(omegae X VB).
[TransMatrixNED2ECI]=TransformNED2ECI(phi,lamdac);
OmegaE=TransMatrixB2NED'*TransMatrixNED2ECI'*[0;0;omegae];
Umod=OmegaE(2)*W-OmegaE(3)*V; % Elliptical Earth modification
Vmod=OmegaE(3)*U-OmegaE(1)*W; % Elliptical Earth modification
Wmod=OmegaE(1)*V-OmegaE(2)*U; % Elliptical Earth modification
% Stevens and lewis, p.81
Udot= R*V-Q*W-gprime*sE2     +(Fax+Ftx)/mass - Umod; % Elliptical Earth modification
Vdot=-R*U+P*W+gprime*sE1*cE2+(Fay+Fty)/mass  - Vmod; % Elliptical Earth modification
aa=qbar*S*cos(alpha)*CLalphadot*cbar/(2*Vt*U);
alphadotfactor=(1/(1+aa/mass));
Wdot= (Q*U-P*V+gprime*cE1*cE2+(Fazprime+Ftz)/mass - Wmod)*alphadotfactor; % Elliptical mod

% Moment equations of motion
% Get aerodynamic moments
alphadot=(U*Wdot-W*Udot)/(U*U+W*W);
[La,Ma,Na,Croll,Cpitch,Cyaw]=aeromoment(x,u,constant,alpha,beta,Vt,qbar,alphadot,CX,CY,CZprime);

% Stevens and lewis, pp. 80-81. These are unmodified when going to an elliptical Earth
Pdot=(c1*R+c2*P)*Q      +c3*(La+Lt)+c4*(Na+Nt);
Qdot=c5*P*R-c6*(P*P-R*R)+c7*(Ma+Mt);
Rdot=(c8*P-c2*R)*Q      +c4*(La+Lt)+c9*(Na+Nt);

% Kinematic equations
% Compute modification terms for kinematic equations of motion for elliptical Earth.
e1mod=-lamdacdot*cE3*cphi/cE2 + sE3*phidot/cE2;  % Elliptical Earth modification
e2mod=cphi*sE3*lamdacdot + cE3*phidot;  % Elliptical Earth modification
e3mod=(-sE2*cphi*cE3/cE2+sphi)*lamdacdot + phidot*sE2*sE3/cE2;  % Elliptical Earth mod
E1dot=P+tE2*(Q*sE1+R*cE1)+e1mod;  % Elliptical Earth modification
E2dot=Q*cE1-R*sE1        +e2mod;  % Elliptical Earth modification
E3dot=(Q*sE1+R*cE1)/cE2  +e3mod;  % Elliptical Earth modification

% Navigation equations. The modification here is in the addition of one more transformation.
% [TransMatrixB2NED]=TransformB2NED(E1,E2,E3); %This was done above
% [TransMatrixNED2ECI]=TransformNED2ECI(phi,lamdac); %This was done above
xyzdot=TransMatrixNED2ECI*TransMatrixB2NED*[U,V,W]'; % Elliptical Earth modification
% We also need to add in the (omegae X p) term.
Xdot= xyzdot(1)-omegae*Y; % Elliptical Earth modification
Ydot= xyzdot(2)+omegae*X; % Elliptical Earth modification
Zdot= xyzdot(3); 

% Assemble the entire derivative vector.
sys = [Udot,Vdot,Wdot,Pdot,Qdot,Rdot,E1dot,E2dot,E3dot,Xdot,Ydot,Zdot];

%***********************************************************
%*                    ACUpdate                            *
%***********************************************************
function sys=ACUpdate(t,x,u)
% Compute update for discrete states. If necessary, check for
% sample time hits.
sys = [];    % Empty since this model has no discrete states.

%***********************************************************
%*                    ACOutputs                           *
%***********************************************************
function sys=ACOutputs(t,x,u)
% Compute output vector given current state, time, and input
% y=[U,V,W,P,Q,R,E1(roll),E2(pitch),E3(yaw),X,Y,Z,Vt,alpha,beta,gamma
% geodetic latitude (phi), celestial longitude (lamdac), 
% geodetic londitude (lamda), altitude (h)]
% Units are [meters/sec, meters/sec, meters/sec, rad/sec, rad/sec, rad/sec, 
% rad, rad, rad, meters, meters, meters, meters/sec, rad, rad, rad
% rad,rad,rad,meters]
f2m=.3048;
Umeterpsec=f2m*x(1);  % m/sec
Vmeterpsec=f2m*x(2);  % m/sec
Wmeterpsec=f2m*x(3);  % m/sec
E1=x(7);
E2=x(8);
Vtmeterpsec=sqrt(Umeterpsec*Umeterpsec+Vmeterpsec*Vmeterpsec+Wmeterpsec*Wmeterpsec);  % m/sec
alpha=atan2(Wmeterpsec,Umeterpsec);      % rad
beta=asin(Vmeterpsec/Vtmeterpsec);       % rad
a=cos(alpha)*cos(beta);
b=sin(E1)*sin(beta)+cos(E1)*sin(alpha)*cos(beta);
gamma=asin(a*sin(E2)-b*cos(E2)); %Stevens and Lewis p131, eqn 3.4-2
[phi,lamdac,h]=e2g(x(10),x(11),x(12)); % h is in feet
omegae=7.292115e-5; % angular velocity of the Earth
lamda=lamdac-omegae*t+86.917*pi/180; % geodetic londitude of West Lafayette, IN
sys = [Umeterpsec,Vmeterpsec,Wmeterpsec,x(4),x(5),x(6),E1,E2,x(9),f2m*x(10),...
       f2m*x(11),f2m*x(12),Vtmeterpsec,alpha,beta,gamma,phi,lamdac,lamda,f2m*h];

%***********************************************************
%*                    ACGetTimeOfNextVarHit               *
%***********************************************************
function sys=ACGetTimeOfNextVarHit(t,x,u)
% 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 sampleTime = 1;    
sys = [] ;

%************************************************************
%*                    ACTerminate                          *
%************************************************************
function sys=ACTerminate(t,x,u)
% Perform any necessary tasks at the end of the simulation
sys = [];

