set_param(0,'CharacterEncoding','ISO-8859-1')

% 1st experiment
% Look at Vz (Settling time, Overshoot, rise time, final value for Steady-State error, 
% Look that Va remains around 230, and max and min value
% 

% 2nd experiment
% Look at Va (Settling time, Overshoot, rise time, final value for Steady-State error, 
% Look that Vz remains around 0, and max and min value
% 

% 3rd experiment
% Look at h (time at 1950, finale value, max OS) 
% Look that Va remains around 230, and max and min value

% 4th experiment
% Multiple flight level changes


clear all

% Data loading

simu_data;

% simulink_model = 'Discrete_Closed_Loop_Model';

 simulink_model = 'Discrete_Closed_Loop_Model_74';


%% 1st experiment: Vertical speed control
h_c          = h_eq;
Flight_Level = [0 h_eq];
Va_c         = 230.0;
Vz_c         = 2.5;
T_final      = 50.0;
Flight_Mode  = 2; % Airspeed / Vertical Speed Control (2)
sim(simulink_model)
y = [tout data_Va data_az data_q data_Vz data_h data_delta_x_c data_delta_e_c];

% Graphic

figure(1)
subplot(223)
plot(tout,data_Va,'Linewidth',1)
grid on
axis normal
xlabel('Time (s)')
ylabel('V_a (m/s)')

subplot(224)
plot(tout,data_Vz,'Linewidth',1)
grid on
xlabel('Time (s)')
ylabel('V_z (m/s)')

% CSV file 

fid = fopen('results_1.csv','w');
fprintf(fid,'# Vertical Speed control: 0 m/s -> 2.5 m/s and Airspeed: 230 m/s \n');
fprintf(fid,'# Column headers are: \n');
fprintf(fid,'# - T: simulation time [s] \n');
fprintf(fid,'# - Va: airspeed [m/s] \n');
fprintf(fid,'# - az: normal acceleration [m/s^2] \n');
fprintf(fid,'# - q: pitch rate [rad/s] \n');
fprintf(fid,'# - Vz: vertical speed [m/s] \n');
fprintf(fid,'# - h: altitude [m] \n');
fprintf(fid,'# - delta_th_c: commanded throttle [-] \n');
fprintf(fid,'# - delta_e_c: commanded elevator deflection [rad] \n');
fprintf(fid,'# T, Va, Az, q, Vz, h, delta_th_c, delta_e_c   \n');
fclose(fid);
dlmwrite('results_1.csv',y,'delimiter',',','precision','%5.15f','-append');

%% 2nd experiment: Airspeed Control
h_c          = h_eq;
Flight_Level = [0 h_eq];
Va_c         = 235.0;
Vz_c         = 0.0;
T_final      = 50.0;
Flight_Mode  = 2; % Airspeed / Vertical Speed Control (2)
sim(simulink_model)
y = [tout data_Va data_az data_q data_Vz data_h data_delta_x_c data_delta_e_c];

% Graphic

figure(1)
subplot(221)
plot(tout,data_Va,'Linewidth',1)
grid on
axis normal
xlabel('Time (s)')
ylabel('V_a (m/s)')

subplot(222)
plot(tout,data_Vz,'Linewidth',1)
grid on
xlabel('Time (s)')
ylabel('V_z (m/s)')

% CSV file

fid = fopen('results_2.csv','w');
fprintf(fid,'# Airspeed control: 230 m/s -> 235 m/s and Vertical speed: 0 m/s \n');
fprintf(fid,'# Column headers are: \n');
fprintf(fid,'# - T: simulation time [s] \n');
fprintf(fid,'# - Va: airspeed [m/s] \n');
fprintf(fid,'# - az: normal acceleration [m/s^2] \n');
fprintf(fid,'# - q: pitch rate [rad/s] \n');
fprintf(fid,'# - Vz: vertical speed [m/s] \n');
fprintf(fid,'# - h: altitude [m] \n');
fprintf(fid,'# - delta_th_c: commanded throttle [-] \n');
fprintf(fid,'# - delta_e_c: commanded elevator deflection [rad] \n');
fprintf(fid,'# T, Va, Az, q, Vz, h, delta_th_c, delta_e_c   \n');
fclose(fid);
dlmwrite('results_2.csv',y,'delimiter',',','precision','%5.15f','-append');


%% 3rd experiment: Flight Level Change

h_c         = 11000.0;
Flight_Level = [0    h_eq
                50   h_c]; 
Va_c        = 230.0;
Vz_c        = -2.5;
T_final     = 600.0;
Flight_Mode = 1; % Airspeed / Climb / Altitude Hold Control (2)
sim(simulink_model)
y = [tout data_Va data_az data_q data_Vz data_h data_delta_x_c data_delta_e_c];

% Graphic

figure(3)

subplot(221)
plot(tout,data_h/1000,'Linewidth',1)
grid on
axis normal
xlabel('Time (s)')
ylabel('h (km)')

subplot(222)
plot(data_Vzc(:,1),-data_Vzc(:,2),'Linewidth',1)
grid on
axis normal
xlabel('Time (s)')
ylabel('V_{z_c} (m/s)')

subplot(223)
plot(tout,data_Va,'Linewidth',1)
grid on
xlabel('Time (s)')
ylabel('V_a (m/s)')

subplot(224)
plot(tout,-data_Vz,'Linewidth',1)
grid on
xlabel('Time (s)')
ylabel('V_z (m/s)')

% CSV file

fid = fopen('results_3.csv','w');
fprintf(fid,'# Flight Level change: 10000 m -> 11000 m and Airspeed: 230 m/s \n');
fprintf(fid,'# Column headers are: \n');
fprintf(fid,'# - T: simulation time [s] \n');
fprintf(fid,'# - Va: airspeed [m/s] \n');
fprintf(fid,'# - az: normal acceleration [m/s^2] \n');
fprintf(fid,'# - q: pitch rate [rad/s] \n');
fprintf(fid,'# - Vz: vertical speed [m/s] \n');
fprintf(fid,'# - h: altitude [m] \n');
fprintf(fid,'# - delta_th_c: commanded throttle [-] \n');
fprintf(fid,'# - delta_e_c: commanded elevator deflection [rad] \n');
fprintf(fid,'# T, Va, Az, q, Vz, h, delta_th_c, delta_e_c   \n');
fclose(fid);
dlmwrite('results_3.csv',y(1:10:end,:),'delimiter',',','precision','%5.15f','-append');

%% 4th experiment: Multiple Flight Level Changes

Flight_Level = [0    10000
                50   10500
                400  11000
                750  11500
                1100  8000]; 
Va_c        = 230.0;
Vz_c        = -2.5;
T_final     = 3000.0;
Flight_Mode = 1; % Airspeed / Climb / Altitude Hold Control (2)
sim(simulink_model)
y = [tout data_Va data_az data_q data_Vz data_h data_delta_x_c data_delta_e_c];

% Graphic

figure(2)

subplot(221)
plot(tout,data_h/1000,'Linewidth',1)
grid on
axis normal
xlabel('Time (s)')
ylabel('h (km)')

subplot(222)
plot(data_Vzc(:,1),-data_Vzc(:,2),'Linewidth',1)
grid on
axis normal
xlabel('Time (s)')
ylabel('V_{z_c} (m/s)')

subplot(223)
plot(tout,data_Va,'Linewidth',1)
grid on
xlabel('Time (s)')
ylabel('V_a (m/s)')

subplot(224)
plot(tout,-data_Vz,'Linewidth',1)
grid on
xlabel('Time (s)')
ylabel('V_z (m/s)')





