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

% Removing warning. To enable if needed.
warning('off', 'all')

% Data loading

simu_data;

% simulink_model = 'Discrete_Closed_Loop_Model_74';
% simulink_model = 'Continuous_Closed_Loop_Model';
simulink_model = 'Hybrid_Closed_Loop_Model';

% Example of single failure injection.
%
% Single Injection failure in FCC1 on delta elevator command.
% fcc1_err_delta_e_c = [0 0    % No failure at t=0s
%                       10 1   % 1 rad/s added to delta_e_c in FCC1 at 
%                              % t=10s
%                       20 0]; % No more failure at t=20s
% Results: From t=10s, FCC2 takes control on delta_e_c.
%
% Injection failure in FCC1 on delta elevator command, and in FCC2 
% on delta throttle command.
% fcc1_err_delta_e_c = [0 0    % No failure at t=0s
%                       30 1]; % A permanant failure of 1 rad/s to 
%                              % delta_e_c in FCC1 from t=30s
% fcc2_err_delta_x_c = [0 0    % No failure at t=0s
%                       10 1]; % A permanant failure of 1 rad/s to
%                              % delta_e_c in FCC1 from t=30s
% Results: From t=30s, FCC2 takes control on delta_e_c.
%
% Double failure injection (don't work, out of our hypothesis of single 
% failure)
% fcc1_err_delta_e_c = [0 0
%                       10 1];
% fcc2_err_delta_e_c = [0 0
%                       20 1];
%
% Injection of single failure, but the error is smaller than the error
% tolerance : epsilon_delta_i_c, where i is e or x.
% fcc1_err_delta_e_c = [0 0
%                       10 epsilon_delta_e_c/10];
% Results: As expected, the redundancy is not activated.

%% 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)

fcc1_err_delta_e_c = [0 0
                      10 0.001
                      20 0];
fcc2_err_delta_e_c = [0 0
                      30 0.001
                      40 0];
                  
sim(simulink_model)

% Aggregating data
data_Va.Name        = 'Va';
data_az.Name        = 'az';
data_q.Name         = 'q';
data_Vz.Name        = 'Vz';
data_h.Name         = 'h';
data_delta_x_c.Name = 'delta_x_c';
data_delta_e_c.Name = 'delta_e_c';

timeline = data_Va.Time;
timeline = union(timeline, data_az.Time);
timeline = union(timeline, data_q.Time);
timeline = union(timeline, data_Vz.Time);
timeline = union(timeline, data_h.Time);
timeline = union(timeline, data_delta_x_c.Time);
timeline = union(timeline, data_delta_e_c.Time);

data_Va        = resample(data_Va, timeline);
data_az        = resample(data_az, timeline);
data_q         = resample(data_q, timeline);
data_Vz        = resample(data_Vz, timeline);
data_h         = resample(data_h, timeline);
data_delta_x_c = resample(data_delta_x_c, timeline);
data_delta_e_c = resample(data_delta_e_c, timeline);

% Graphic
figure(1)
subplot(223)
plot(data_Va,'Linewidth',1)
hold on
plot(timeline, Va_eq * ones(size(timeline)), ':')
hold on
plot(timeline, min(data_Va) * ones(size(timeline)), ':r') % Min
hold on
plot(timeline, max(data_Va) * ones(size(timeline)), ':r') % Max
hold off
title('True airspeed evolution over time, case 1')
grid on
axis normal
xlabel('Time (s)')
ylabel('V_a (m/s)')

subplot(224)
plot(data_Vz,'Linewidth',1)
hold on
plot(timeline, Vz_c * ones(size(timeline)), '-m')
hold on
plot(timeline, 0.98 * Vz_c * ones(size(timeline)), ':m') % Settling time diff min
hold on
plot(timeline, 1.02 * Vz_c * ones(size(timeline)), ':m') % Settling time diff max
hold on
plot(timeline, 0.1 * Vz_c * ones(size(timeline)), ':k') % Rise time diff min
hold on
plot(timeline, 0.9 * Vz_c * ones(size(timeline)), ':k') % Rise time diff max
hold on
plot(timeline, max(data_Vz) * ones(size(timeline)), ':r') % Overshoot
hold off
title('Vertical speed evolution over time, case 1');
grid on
xlabel('Time (s)')
ylabel('V_z (m/s)')

% CSV file 
fid = fopen([simulink_model '-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);

y = [timeline];
y = [y data_Va.Data];
y = [y data_az.Data];
y = [y data_q.Data];
y = [y data_Vz.Data];
y = [y data_h.Data];
y = [y data_delta_x_c.Data];
y = [y data_delta_e_c.Data];

dlmwrite([simulink_model '-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)

% Aggregating data
data_Va.Name        = 'Va';
data_az.Name        = 'az';
data_q.Name         = 'q';
data_Vz.Name        = 'Vz';
data_h.Name         = 'h';
data_delta_x_c.Name = 'delta_x_c';
data_delta_e_c.Name = 'delta_e_c';

timeline = data_Va.Time;
timeline = union(timeline, data_az.Time);
timeline = union(timeline, data_q.Time);
timeline = union(timeline, data_Vz.Time);
timeline = union(timeline, data_h.Time);
timeline = union(timeline, data_delta_x_c.Time);
timeline = union(timeline, data_delta_e_c.Time);

data_Va        = resample(data_Va, timeline);
data_az        = resample(data_az, timeline);
data_q         = resample(data_q, timeline);
data_Vz        = resample(data_Vz, timeline);
data_h         = resample(data_h, timeline);
data_delta_x_c = resample(data_delta_x_c, timeline);
data_delta_e_c = resample(data_delta_e_c, timeline);

% Graphic
figure(1)
subplot(221)
plot(data_Va,'Linewidth',1)
hold on
plot(timeline, Va_c * ones(size(timeline)), '-m')
hold on
Va_diff = abs(Va_c - Va_eq);
plot(timeline, (Va_eq + 0.98 * Va_diff) * ones(size(timeline)), ':m') % Settling time diff min
hold on
plot(timeline, (Va_eq + 1.02 * Va_diff) * ones(size(timeline)), ':m') % Settling time diff max
hold on
plot(timeline, (Va_eq + 0.1 * Va_diff) * ones(size(timeline)), ':k') % Rise time diff min
hold on
plot(timeline, (Va_eq + 0.9 * Va_diff) * ones(size(timeline)), ':k') % Rise time diff max
hold on
plot(timeline, max(data_Va) * ones(size(timeline)), ':r') % Overshoot
hold off
title('True airspeed evolution over time, case 2')
grid on
axis normal
xlabel('Time (s)')
ylabel('V_a (m/s)')

subplot(222)
plot(data_Vz,'Linewidth',1)
hold on
plot(timeline, 0 * ones(size(timeline)), ':')
hold on
plot(timeline, min(data_Vz) * ones(size(timeline)), ':r') % Min
hold on
plot(timeline, max(data_Vz) * ones(size(timeline)), ':r') % Max
hold off
title('Vertical speed evolution over time, case 2')
grid on
xlabel('Time (s)')
ylabel('V_z (m/s)')

% CSV file

fid = fopen([simulink_model '-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);

y = [timeline];
y = [y data_Va.Data];
y = [y data_az.Data];
y = [y data_q.Data];
y = [y data_Vz.Data];
y = [y data_h.Data];
y = [y data_delta_x_c.Data];
y = [y data_delta_e_c.Data];

dlmwrite([simulink_model '-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)

% Aggregating data
data_Va.Name        = 'Va';
data_az.Name        = 'az';
data_q.Name         = 'q';
data_Vz.Name        = 'Vz';
data_h.Name         = 'h';
data_delta_x_c.Name = 'delta_x_c';
data_delta_e_c.Name = 'delta_e_c';

timeline = data_Va.Time;
timeline = union(timeline, data_az.Time);
timeline = union(timeline, data_q.Time);
timeline = union(timeline, data_Vz.Time);
timeline = union(timeline, data_h.Time);
timeline = union(timeline, data_delta_x_c.Time);
timeline = union(timeline, data_delta_e_c.Time);

data_Va        = resample(data_Va, timeline);
data_az        = resample(data_az, timeline);
data_q         = resample(data_q, timeline);
data_Vz        = resample(data_Vz, timeline);
data_h         = resample(data_h, timeline);
data_delta_x_c = resample(data_delta_x_c, timeline);
data_delta_e_c = resample(data_delta_e_c, timeline);

% Graphic
figure(3)

subplot(221)
plot(data_h/1000,'Linewidth',1)
title('Altitude evolution over time, case 3')
grid on
axis normal
xlabel('Time (s)')
ylabel('h (km)')

subplot(222)
plot(data_Vzc,'Linewidth',1)
title('Vertical speed command evolution over time, case 3')
grid on
axis normal
xlabel('Time (s)')
ylabel('V_{z_c} (m/s)')

subplot(223)
plot(data_Va,'Linewidth',1)
title('True airspeed evolution over time, case 3')
grid on
xlabel('Time (s)')
ylabel('V_a (m/s)')

subplot(224)
plot(data_Vz,'Linewidth',1)
title('Vertical speed evolution over time, case 3')
grid on
xlabel('Time (s)')
ylabel('V_z (m/s)')

% CSV file

fid = fopen([simulink_model '-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);

y = [timeline];
y = [y data_Va.Data];
y = [y data_az.Data];
y = [y data_q.Data];
y = [y data_Vz.Data];
y = [y data_h.Data];
y = [y data_delta_x_c.Data];
y = [y data_delta_e_c.Data];

dlmwrite([simulink_model '-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)

% Aggregating data
data_Va.Name        = 'Va';
data_az.Name        = 'az';
data_q.Name         = 'q';
data_Vz.Name        = 'Vz';
data_h.Name         = 'h';
data_delta_x_c.Name = 'delta_x_c';
data_delta_e_c.Name = 'delta_e_c';

timeline = data_Va.Time;
timeline = union(timeline, data_az.Time);
timeline = union(timeline, data_q.Time);
timeline = union(timeline, data_Vz.Time);
timeline = union(timeline, data_h.Time);
timeline = union(timeline, data_delta_x_c.Time);
timeline = union(timeline, data_delta_e_c.Time);

data_Va        = resample(data_Va, timeline);
data_az        = resample(data_az, timeline);
data_q         = resample(data_q, timeline);
data_Vz        = resample(data_Vz, timeline);
data_h         = resample(data_h, timeline);
data_delta_x_c = resample(data_delta_x_c, timeline);
data_delta_e_c = resample(data_delta_e_c, timeline);

% Graphic
figure(2)

subplot(221)
plot(data_h/1000,'Linewidth',1)
title('Altitude evolution over time, case 4')
grid on
axis normal
xlabel('Time (s)')
ylabel('h (km)')

subplot(222)
plot(data_Vzc,'Linewidth',1)
title('Vertical speed command evolution over time, case 4')
grid on
axis normal
xlabel('Time (s)')
ylabel('V_{z_c} (m/s)')

subplot(223)
plot(data_Va,'Linewidth',1)
title('True airspeed evolution over time, case 4')
grid on
xlabel('Time (s)')
ylabel('V_a (m/s)')

subplot(224)
plot(data_Vz,'Linewidth',1)
title('Vertical speed evolution over time, case 4')
grid on
xlabel('Time (s)')
ylabel('V_z (m/s)')

% CSV file

% Warning: Writing results_4 is really long. By default, this script exits
% before doing so.
% To obtain csv file, comment the next 'return'.
return

fid = fopen([simulink_model '-results_4.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);

y = [timeline];
y = [y data_Va.Data];
y = [y data_az.Data];
y = [y data_q.Data];
y = [y data_Vz.Data];
y = [y data_h.Data];
y = [y data_delta_x_c.Data];
y = [y data_delta_e_c.Data];

dlmwrite([simulink_model '-results_4.csv'],y,'delimiter',',','precision','%5.15f','-append');
