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


%% Node Rates
 
Ts_h     = 1/50.0;  % Altitude hold controller  
Ts_K1    = 1/50.0;  % Airspeed controller
Ts_K2    = 1/50.0;  % Vertical speed controller
Ts_f_Va  = 1/100.0; % Airspeed filter
Ts_f_az  = 1/100.0; % Vertical acceleration filter
Ts_f_q   = 1/100.0; % Pitch rate filter
Ts_f_Vz  = 1/100.0; % Vertical speed filter;
Ts_f_h   = 1/50.0;  % Altitude filter
dt       = 1/200.0; % Flight dynamics
dt_de    = 1/200.0; % Elevator
dt_dx    = 1/200.0; % Engine

%% Aircraft and Environment parameters

% Atmosphere parameters

rho0 =  1.225;   % air density at SL (Sea Level) (in kg/m^3)
g0   =  9.80665; % standard gravity (in m/s^2)
T0_0 =  288.15;  % Temperature at SL (in �K)
T0_h = -0.0065;  % Temperature gradient between 0 and 11 km (in �K/m)
Rs   =  287.05;  % Air specific constant

% Aircraft parameters

mass      =  57837.5;   % Mass (in kg)
I_y       =  3781272.0; % Moment of inertia (in kg.m^2)
S         =  122.6;     % Wing aera (in m^2)
cbar      =  4.29;      % Aerodynamic chord (in m)
CD_0      =  0.016;     % Drag coefficient
CD_alpha  =  2.5;       % Drag coefficient w.r.t. alpha
CD_deltae =  0.05;      % Drag coefficient w.r.t. elevator deflection
CL_alpha  =  5.5;       % Lift coefficient w.r.t. alpha
CL_deltae =  0.193;     % Lift coefficient w.r.t. elevator deflection
alpha_0   = -0.05;      % Zero-lift angle of incidence
Cm_0      =  0.04;      % Pitching moment coefficient at aerodynamic center
Cm_alpha  = -0.83;      % Pitching moment coefficient w.r.t. alpha
Cm_deltae = -1.5;       % Pitching moment coefficient w.r.t. elevator deflection
Cm_q      = -30;        % Pithcing moment coefficient w.r.t. pitch rate

% Init redundancy
init_fcc1_master_in_law = true;
init_fcc2_master_in_law = false;

% Error tolerance

epsilon_delta_e_c = 1e-5; % Tolerance on delta_e_c monitoring
epsilon_delta_x_c = 1e-5; % Tolerance on delta_x_c monitoring

% Initializing error (failure) injection lists.
fcc1_err_delta_e_c = [0 0]; % at t=0, nothing added to delta e c
fcc1_err_delta_x_c = [0 0];
fcc2_err_delta_e_c = [0 0];
fcc2_err_delta_x_c = [0 0];
% To inject error, add in simu results file:
% fccx_err_delta_y_c = [t0 error0epsilon_delta_e_c
%                       t1 error1
%                       ... ];
% See simu results file for better examples.

% Initial flight condition (Equilibrium)

h_eq       = 10000.0;           % Initial altitude
Va_eq      = 230.0;             % Initial airspeed
delta_x_eq = 1.5868660794926;   % Initial throttle
delta_e_eq = 0.012009615652468; % Initial elevator deflection               
alpha_eq   = 0.026485847681737; % Initial alpha (angle of attack)
theta_eq   = 0.026485847681737; % Initial theta (pitch angle)

% Initial conditions for flight dynamic state vector

u0     = Va_eq * cos(theta_eq);
w0     = Va_eq * sin(theta_eq);
q0     = 0;
theta0 = theta_eq;
h0     = h_eq;
CI_FD = [u0 w0 q0 theta0 h0];

% Elevator parameters

xi    = 0.85;
omega = 25;
A_de  = [0 1;-omega^2 -2*xi*omega];
B_de  = [0 omega^2]';
C_de  = [1 0];
D_de  = [0];
CI_de = [delta_e_eq 0];

% Engine parameters

A_dx  = [-0.75];
B_dx  = [0.75]';
C_dx  = [26350];
D_dx  = [0];
CI_dx = [delta_x_eq];

% h Sensor (Butterworth 3 Hz)
% Continuous

[num_h,den_h] = butter(2,2*pi*3,'low','s');
A_h  = [0 1;-den_h(end:-1:2)];
B_h  = [0 num_h(end)]';
C_h  = [1 0];
D_h  = [0];
CI_h = [h_eq 0];

% Discrete

G_h   = c2d(tf(num_h,den_h),Ts_f_h);
Az_h  = [0 1;-G_h.den{1}(end:-1:2)]';
Bz_h  = [G_h.num{1}(end:-1:2)]';
Cz_h  = [0 1];
Dz_h  = [0];
CIz_h = [1+G_h.den{1}(2)-G_h.num{1}(2) 1]*h_eq;

% Va Sensor (Butterworth 0.5 Hz)
% Continuous

[num_Va,den_Va] = butter(2,2*pi*0.5,'low','s');
A_Va  = [0 1;-den_Va(end:-1:2)];
B_Va  = [0 num_Va(end)]';
C_Va  = [1 0];
D_Va  = [0];
CI_Va = [Va_eq 0];

% Discrete

G_Va   = c2d(tf(num_Va,den_Va),Ts_f_Va);
Az_Va  = [0 1;-G_Va.den{1}(end:-1:2)]';
Bz_Va  = [G_Va.num{1}(end:-1:2)]';
Cz_Va  = [0 1];
Dz_Va  = [0];
CIz_Va = [1+G_Va.den{1}(2)-G_Va.num{1}(2) 1]*Va_eq;

% Vz Sensor (Butterworth 0.5 Hz)
% Continuous

[num_Vz,den_Vz] = butter(2,2*pi*0.5,'low','s');
A_Vz  = [0 1;-den_Vz(end:-1:2)];
B_Vz  = [0 num_Vz(end)]';
C_Vz  = [1 0];
D_Vz  = [0];
CI_Vz = [0 0];

% Discrete

G_Vz   = c2d(tf(num_Vz,den_Vz),Ts_f_Vz);
Az_Vz  = [0 1;-G_Vz.den{1}(end:-1:2)]';
Bz_Vz  = [G_Vz.num{1}(end:-1:2)]';
Cz_Vz  = [0 1];
Dz_Vz  = [0];
CIz_Vz = [0 0];

% q Sensor (Butterworth 3 Hz)
% Continuous

[num_q,den_q] = butter(2,2*pi*3,'low','s');
A_q  = [0 1;-den_q(end:-1:2)];
B_q  = [0 num_q(end)]';
C_q  = [1 0];
D_q  = [0];
CI_q = [0 0];

% Discrete

G_q    = c2d(tf(num_q,den_q),Ts_f_q);
Az_q  = [0 1;-G_q.den{1}(end:-1:2)]';
Bz_q  = [G_q.num{1}(end:-1:2)]';
Cz_q  = [0 1];
Dz_q  = [0];
CIz_q = [0 0];

% az Sensor (Butterworth 10 Hz)
% Continuous

[num_az,den_az] = butter(2,2*pi*10,'low','s');
A_az  = [0 1;-den_az(end:-1:2)];
B_az  = [0 num_az(end)]';
C_az  = [1 0];
D_az  = [0];
CI_az = [0 0];

% Discrete

G_az   = c2d(tf(num_az,den_az),Ts_f_az);
Az_az  = [0 1;-G_az.den{1}(end:-1:2)]';
Bz_az  = [G_az.num{1}(end:-1:2)]';
Cz_az  = [0 1];
Dz_az  = [0];
CIz_az = [0 0];

%% Controller parameters

% Altitude hold

Kp_h       = 0.1014048;
Ki_h       = 0.0048288;
Vz_c       = -2.5;
h_switch   = 50.0;
CI_AltHold = Vz_c/Ki_h + h_switch*Kp_h/Ki_h;

% Va Speed controller

K1_intVa =  0.049802610664357;
K1_Va    = -0.486813084356079;
K1_Vz    = -0.077603095495388;
K1_q     = 21.692383376322041;

% Vz Speed controller

K2_intVz =  0.000627342822264;
K2_Vz    = -0.003252836726554;
K2_q     =  0.376071446897134;
K2_az    = -0.001566907423747;

% Simulation time

T_final = 600;

% Autopilot/Authrottle inputs

h_c  = 10000.0;
Va_c = 230.0;












