% runfile.m
%
%    Script to simulate the free flight and aircraft-manipulator system
%    and to plot the results for the paper:
%    "An Aircraft-Manipulator System for Virtual Flight 
%     Testing of Longitudinal Flight Dynamics" 
%                  by 
%    Ademayowa A. Ishola, James F. Whidborne, Gilbert Tang 
%
% 

%   (c) James Whidborne & Ade Ishola
%   Cranfield University, 2 December 2024
%   MATLAB Version: 23.2.0.2409890 (R2023b) Update 3

format compact
clear all
close all

%% printing parameters
printflag=0; % set to 1 to save plots
fs=14;       % fontsize
lw=1.2;      % linewidth

%% define data

[hawk,man] = SystemData; % data for aircraft and manipulator

% set controller gains 
K.p= [1;1]*100;  % (nm/rad)
K.i= [1;1]*4;
K.d= [1;1]*200;

%% trim aircraft
VT_trim = man.VWT;  % trim airspeed set to wind tunnel speed
h_trim = 10;        % altitude (m) 
deltat_trim =-0.062111170520782; % tail trim value (calculated previously) (rad)
[deltat_trim,alpha_trim,L_trim, D_trim, T_trim,theta_trim] = ...
         hawk_trim(VT_trim,h_trim,deltat_trim,hawk); %  trim aircraft model
q_trim = 0;         % trim pitch rate

% set initial conditions (trim) for Aircraft Manipulator System simulation
omega0 = [0; 0; 0]; % initial angular rates 
xa0=0.0; ha0=0.4;   % initial position in wind tunnel 

% calculate initial manipulator angles
% theta  2 from eq (14) (negative solution)
opp = sqrt((xa0^2 + ha0^2 + man.l1^2 + man.l2^2)^2-2*((xa0^2+ha0^2)^2+man.l1^4+man.l2^4));
adj = (xa0^2 + ha0^2 - man.l1^2 - man.l2^2);
theta20 = -atan2(opp,adj);
% theta 1 from eq (15)
theta10 =  atan2(ha0,xa0) + atan2(opp,(xa0^2 + ha0^2 + man.l1^2 - man.l2^2)); 
  
% sine / cosine abbreviations
s1 = sin(theta10);
s12 = sin(theta10+theta20); 
c1 = cos(theta10); 
c12 = cos(theta10+theta20); 
 
% gravitational torques at trim
G0  = [(man.m1  + man.m2 + hawk.ma)* man.l1*c1;
    (man.m2/2 + hawk.ma)*man.l2*c12]*hawk.g;

% aerodynamic torques at trim
tau_a0 = [L_trim*(man.l1*c1+man.l2*c12) + D_trim*(man.l1*s1+man.l2*s12);
         L_trim*man.l2*c12 + D_trim*man.l2*s12];

torque0 = -tau_a0 + G0 % total torque at trim - to initialize PID controller

%% simulation 1 - excite the short period mode
disp('----------------------------------------------------------------')
disp('------------------------  Simulation   -------------------------')
disp('----------------------------------------------------------------')
 
% add initial perturbation & simulate
disp('Initial perturbation:')
perturbation = 0.02                    % perturbation from trim (rad)
thetaa0 = theta_trim+perturbation       % perturb pitch
theta0 = [theta10; theta20; thetaa0];   % initial link angles
alpha0 = thetaa0                        % perturbed AoA 
t_end= 5                                % simulation time 

% simulate
warning off
sim("HawkFreeFlight.slx")               % free flight simulation
sim("InverseKinematics.slx")            % invert kinematics
sim("AMSFlight.slx")                  % simulate aircraft-manipulator
 
% plots
 
% Short period mode altitude response of the free-flying Hawk model 
% and the Aircraft-Manipulator System (AMS) model.
figure(9)
plot(IKxaha.time(:,1),IKxaha.data(:,2),'--','linewidth',lw), hold on
plot(AMSxaha.time(:,1),squeeze(AMSxaha.data(2,1,:)),'linewidth',lw), hold off
grid on
title('')
ylabel('$h(t)$ (m)','Interpreter','latex','fontsize',fs)
xlabel('$t$ (s)','Interpreter','latex','fontsize',fs)
legend({'free-flight';'AMS'},'Location','southeast')
if(printflag) save_pdf(gcf,'SPheight'); end

% Short period mode pitch response of the free-flying Hawk model 
% and the Aircraft-Manipulator System (AMS) model
figure(7)
plot(HFFtheta*180/pi,'--','linewidth',lw), hold on
plot(AMStheta_a*180/pi,'linewidth',lw), hold off
grid on
ylabel('$\theta(t), \theta_a(t)$ (deg)','Interpreter','latex','fontsize',fs)
xlabel('$t$ (s)','Interpreter','latex','fontsize',fs)  
legend({'$\theta$'; '$\theta_a$'},...
    'Location','northeast','Interpreter','latex','fontsize',fs)
title('')
if(printflag) save_pdf(gcf,'SPpitch'); end
 
%% simulation 2 - observe the phugoid mode  

% add initial perturbation & simulate
% perturbation = 0.002    
% thetaa0 = theta_trim+perturbation;      % pitch perurbation
% theta0 = [theta10; theta20; thetaa0];   % initial link angles
% alpha0 = theta_trim                     % unperturbed AoA 
t_end = 60                             % simulation time 

% simulate
sim("HawkFreeFlight.slx")               % free flight simulation
sim("InverseKinematics.slx")            % invert kinematics
sim("AMSFlight.slx")                  % simulate aircraft-manipulator

% plots 

% Phugoid mode pitch response of the free-flying Hawk model and 
% the Aircraft-Manipulator System (AMS) model.
figure(8)
plot(HFFtheta*180/pi,'--', 'linewidth',lw), hold on
plot(AMStheta_a*180/pi,'-','linewidth',lw), hold off
grid on
ylabel('$\theta(t), \theta_a(t)$ (deg)','Interpreter','latex','fontsize',fs)
xlabel('$t$ (s)','Interpreter','latex','fontsize',fs)
title('')
legend({'$\theta$'; '$\theta_a$'},...
    'Location','northeast','Interpreter','latex','fontsize',fs)
if(printflag) save_pdf(gcf,'PHpitch'); end

% Phugoid mode height and lateral surge position response of the 
% free-flying Hawk model  and the Aircraft-Manipulator System (AMS) model 
figure(10)
plot(IKxaha,'--','linewidth',lw), hold on 
plot(AMSxaha,'linewidth',lw), hold off
grid on
ylabel('$(x_a,h_a)(t)$ (m)','Interpreter','latex','fontsize',fs)
xlabel('$t$ (s)','Interpreter','latex','fontsize',fs)
legend({'free-flight $x_a$';'free-flight $h_a$';'AMS $x_a$';'AMS $h_a$'},...
    'Location','east','Interpreter','latex','fontsize',fs)
title('')
if(printflag) save_pdf(gcf,'PHxaha'); end

% Phugoid mode position response of the Aircraft-Manipulator System (AMS) 
% model in the wind tunnel
figure(11) 
plot(IKxaha.data(:,1),IKxaha.data(:,2),'--',...
      'linewidth',lw)
grid on, axis square
xlabel('$x_a(t)$ (m)','Interpreter','latex','fontsize',fs)
ylabel('$h_a(t)$ (m)','Interpreter','latex','fontsize',fs)
hold on
plot(squeeze(AMSxaha.data(1,1,:)),squeeze(AMSxaha.data(2,1,:)),...
    'linewidth',lw)
hold off
axis([-0.3 0.1 0.2 0.6])
axis square
legend({'free-flight';'AMS'},'Location','southwest')
% Create arrows
annotation(gcf,'arrow',[0.655357142857143 0.655357142857143],...
    [0.69 0.71], ...
    'Color',[0.847058832645416 0.329411774873734 0.0901960805058479]);
annotation(gcf,'arrow',[0.480357142857143 0.473214285714286],...
    [0.677571428571428 0.669047619047619], ...
    'Color',[0.847058832645416 0.329411774873734 0.0901960805058479]);
if(printflag) save_pdf(gcf,'PHposition'); end

%% Lanchester Approximation
% Analysis of longitudinal dynamics
disp('Lanchester approximation')
disp(['Phugoid period = ',num2str( pi*VT_trim*sqrt(2)/(hawk.g))])
disp(['Phugoid freq = ',num2str(hawk.g*sqrt(2)/VT_trim)])
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% functions %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% hawk_trim

% find the trim condition for the  hawk aircraft (straight and level)
  
function [deltat_trim,alphatrim,Ltrim, Dtrim, Ttrim,thetatrim] = ...
         hawk_trim(VT_trim,htrim,deltat0,hawk)

%  Inputs
%    hawk: parameters of the aircraft model (see systemData)
%    VT_trim : wind tunnel speed (maximum  39 m/s)
%    htrim : trim altitude (sea level)
%    deltat0 : intial guess of tail trim 
%
%  Outputs
%    deltat_trim : tail trim (rad)
%    alphatrim : AoA trim
%    Ltrim : list trim 
%    Dtrim : drag trim
%    Ttrim : thrust trim
%    thetatrim : pitch trim

[~, ~, ~, rho] = atmosisa(htrim);   % Standard Atmosphere density (Aerospace Toolbox)  
qbar = rho*VT_trim^2/2;             % dynamic pressure

% calculate some parameters
cbar = hawk.mac;                    % assume
%St = hawk.VHbar*hawk.Sw*hawk.mac/hawk.Lt;  % tail area
AR = hawk.Sw/cbar^2;                % aspect ratio
sigma = 1/(pi*hawk.oef*AR);         % induced drag factor
Weight = hawk.ma*hawk.g;            % weight = mg

%delta_t = (hawk.Cm0+CL*(hawk.h_cg-hawk.h_0))/(hawk.VHbar*hawk.CLtalpha) -CL/hawk.CLwalpha*(1-hawk.dwg)

% trim by minimizing trim residual
trimoptions = optimset('Display','on','MaxFunEvals',1000,'TolFun',1e-8,'TolX',1e-8);% search minimization options
[deltat_trim ,fmin] = fminsearch(@(xt) hawk_trim_residual(xt,qbar,hawk),deltat0,trimoptions);
if(fmin>1e-10),warning(['fmin = ',num2str(fmin)]),end

% from Cook (3.12)
alphatrim = ((hawk.Cm0-hawk.CLtalpha*deltat_trim*hawk.VHbar)... 
          /(hawk.CLtalpha*hawk.VHbar*(1-hawk.dwg)/hawk.CLwalpha-(hawk.h_cg-hawk.h_0) )-hawk.CL0)/hawk.CLwalpha;
% Lift at trim
CLw = hawk.CLwalpha*alphatrim+hawk.CL0;             % wing lift coefficient  
alpha_t = alphatrim*(1-hawk.dwg) + deltat_trim;     % tailplane incidence [dwg = downwash gradient]
CLt = hawk.CLtalpha*alpha_t;                        % tail lift coefficient  
CL =  CLw + (hawk.St/hawk.Sw)*CLt;                  % total lift coefficient (eq 2)
Ltrim = qbar*hawk.Sw*CL;                            % trimmed lift (N)
Dtrim = qbar*hawk.Sw*(hawk.CD0 + sigma*CL^2);       % Drag at trim (N) 
Ttrim = sqrt( (Weight - Ltrim)^2 + Dtrim^2);        % Trim thrust (N)
%zres = Weight -Ltrim - Ttrim*sin(alphatrim);        % use to check residuals
%xres = Dtrim - Ttrim*cos(alphatrim);

%thetatrim = atan2(Weight - Ltrim, Dtrim) ; % in case of not level flight
thetatrim = alphatrim;                      % level flight
disp('----------------------------------------------------------------')
disp('------------------------  Trim results -------------------------')
disp('----------------------------------------------------------------')
disp(['trim delta_t  = ',num2str(deltat_trim), ' rad (= ',num2str(deltat_trim*180/pi), ' deg)']) 
disp(['trim thrust T = ',num2str(Ttrim), ' N'])
disp(['trim alpha    = ',num2str(alphatrim), ' rad  (= ',num2str(alphatrim*180/pi), ' deg)']) 

end

%% hawk_trim_residual

function [e] = hawk_trim_residual(delta_t,qbar,hawk)

%  Calculate trim residual for hawk model
%
%  Inputs
%    delta_t: trim variable  tail angle  
%    qbar = 1/2 rho V_T^2 : dynamic pressure
%    hawk: parameters of the aircraft model (see systemData.m)
%    
%  Outputs
%    e: residual
%


% calculate some parameters
cbar = hawk.mac;                    % assume
%St = hawk.VHbar*hawk.Sw*hawk.mac/hawk.Lt;  % tail area
AR = hawk.Sw/cbar^2;                % aspect ratio
sigma = 1/(pi*hawk.oef*AR);         % induced drag factor
Weight = hawk.ma*hawk.g;            % weight = mg

%delta_t = (hawk.Cm0+CL*(hawk.h_cg-hawk.h_0))/(hawk.VHbar*hawk.CLtalpha) -CL/hawk.CLwalpha*(1-hawk.dwg)

% from Cook (3.12)
alphatrim = ((hawk.Cm0-hawk.CLtalpha*delta_t*hawk.VHbar)/(hawk.CLtalpha*hawk.VHbar*(1-hawk.dwg)/hawk.CLwalpha-(hawk.h_cg-hawk.h_0) )-hawk.CL0)/hawk.CLwalpha;
% Lift at trim
CLw = hawk.CLwalpha*alphatrim + hawk.CL0;       % wing lift coefficient eq (65)!!
alpha_t = alphatrim*(1-hawk.dwg) + delta_t;     % tailplane incidence eq (67) & (68) [dwg = downwash gradient]
CLt = hawk.CLtalpha*alpha_t;                    % tail lift coefficient eq (65)
CL = CLw + (hawk.St/hawk.Sw)*CLt;               % total lift coefficienteq (64)
Ltrim = qbar*hawk.Sw*CL;                        % lift at trim
Dtrim = qbar*hawk.Sw*(hawk.CD0 + sigma*CL^2);   % Drag at trim

%Gamma=a; % straight and level
%disp([num2str(Gamma*180/pi), ' degrees'])
Ttrim = sqrt( (Weight - Ltrim)^2 + Dtrim^2);   % trim thrust
zres = Weight - Ltrim - Ttrim*sin(alphatrim);  % vertical residual component
xres = Dtrim - Ttrim*cos(alphatrim);           % horizontal residual component

% thetatrim =  atan2(Weight-Ltrim, Dtrim) ;
% disp([num2str(thetatrim*180/pi), ' degrees'])

% in body axes
% xbres = Ttrim+Ltrim*sin(thetatrim)-Dtrim*cos(thetatrim)-Weight*sin(thetatrim) 
% zbres = Ltrim*cos(thetatrim)+Dtrim*sin(thetatrim)-Weight*cos(thetatrim)
e = xres^2+zres^2; % square and sum to provide residual 
end