function Main_modelagem
% Versão: Matlab 2020a
% Disciplina: PME3380 - Modelagem de Sistemas Dinâmicos
% Autores:
% Ives C. Vieira - 10355551
% Mauricio C. Leiman - 10772571
% Vitor Facchini - 10772605
% Yago N. Yang - 10772626

% Essa rotina realizará a simulação linear da aeronave, além disso plotará
% os diagramas de Bode, polos e zeros e obtera a função de transferencia.
% Ela tambem calcula uma solucao linear, entretanto os diferentes casos
% foram simulados na rotina "compara.m"
% O equacionamento esta descritos no relatório entregue.
close all

%% Valores
% Constantes 
rho = 1.1062;
g = 9.81;
% Parametros Geométricos 
m = 20;
W = m*g;
Iyy = 1.36;
z_motor = 0.4438;
z_cg = 0.4384;
S = 3.2;
Sh = 0.3387;
mac = 0.234;
x1 = -0.0059;
x2 = -0.1242;
xh = -0.2561;
xCG = 0.1509;

% Coeficientes Aerodinâmicos 
dT = -1.7464;
eta = 1;
CL0 = 0.49;
CD0 = 0.06;
CM0 = 0.06;
CL_alpha = 3.94;
CD_alpha = 0.2180;
CM_alpha = -0.3846;
CL_def = -0.3538;
CD_def = -0.008;
CM_def = -0.3470;
CL_alpha_1 = 1.9108;
CL_alpha_2 = 1.7226;
CL_alpha_h = 0.3110;

%% Condicao de trimagem
% Velocidade Inicial 
V = 15.75;

% Chute Inicial 
x0 = [0;0;60];
rvs = 0.5*rho*V^2*S;

% Equações do Movimento 
fun = @(x) [ (CL0+CL_alpha*x(1)+CL_def*x(2)) + x(3)*sin(x(1))/rvs  - W/rvs;
    x(3)*cos(x(1)) - (CD0+CD_alpha*x(1)+CD_def*x(2));
    (CM0+CM_alpha*x(1)+CM_def*x(2)) - (z_motor-z_cg)*x(3)/(rvs*mac)];

% Variáveis Trimadas 
options = optimoptions('fsolve','Display','off');
y = fsolve(fun,x0,options);
alpha_trim = y(1); def_trim = y(2) ; T_trim = y(3);

% Estados no  sistema de eixos fixo no avião %
Vu = V*cos(alpha_trim);
Vv = 0;
Vw = V*sin(alpha_trim);
% vet_estados = 
%         Vu Vw q xf zf theta
y_trim = [Vu Vw 0 0  0  alpha_trim]';

%% Derivadas de estabilidade
% Coeficientes aerodinâmicos 
CL = CL0 + CL_alpha*alpha_trim + CL_def*def_trim;
CD = CD0 + CD_alpha*alpha_trim + CD_def*def_trim;
CM = CM0 + CM_alpha*alpha_trim + CM_def*def_trim;

% Forca peso 
Wf = [0 0 W]; 
Wb = body(y_trim,Wf);
Xg = Wb(1);
Zg = Wb(3);

% Derivadas 
Fxu = -rho*S*V*CD + dT;
Fzu = -rho*S*V*CL;
Myu = rho*S*mac*V*CM + dT*(-z_motor+z_cg);
Fxw = 0.5*rho*S*V*(CL - CD_alpha);
Fzw = 0.5*rho*S*V*(-CD - CL_alpha);
Myw = 0.5*rho*S*V*mac*CM_alpha;
Fxq = 0;
Fzq = 0.5*rho*V*(S*x1*CL_alpha_1 + S*x2*CL_alpha_2 + eta*Sh*xh*CL_alpha_h);
Myq = -0.5*rho*V*(S*x1^2*CL_alpha_1 + S*x2^2*CL_alpha_2 + eta*Sh*xh^2*CL_alpha_h);;
Fxd = -0.5*rho*S*V^2*CD_def;
Fzd = -0.5*rho*S*V^2*CL_def;
Myd =  0.5*rho*S*V^2*mac*CM_def;


%% Parametros
% Matrizes do Sistema 
Inv = inv([W/g 0 0 0 0 0
    0 W/g 0 0 0 0
    0 0 Iyy 0 0 0
    0 0 0 1 0 0
    0 0 0 0 1 0
    0 0 0 0 0 1]);

A = [Fxu Fxw Fzq 0 0 -W*cos(alpha_trim)
Fzu Fzw Fzq+W*V/g 0 0 -W*sin(alpha_trim)
Myu Myw Myq 0 0 0
cos(alpha_trim) sin(alpha_trim) 0 0 0 -V*sin(alpha_trim)
-sin(alpha_trim) cos(alpha_trim) 0 0 0 -V*cos(alpha_trim)
0 0 1 0 0 0 ];

B = [Fxd
    Fzd
    Myd
    0
    0
    0];

A = Inv*A;
B = Inv*B;

% C = zeros(2,4);
% C(1,:) = rad2deg([0 1/V0 0 0]); C(2,:) = A(2,:)/g;
C = eye(6);
D = zeros(6,1);

% Obtenção da Função de transferência e diagrama de Polos e zeros 
[autoval,autovet]=eig(A); 
[numerador,denominador]=ss2tf(A,B,C,D,1);
[zer,pol,~]=ss2zp(A,B,C,D,1);

figure
plot(real(pol),imag(pol),'kx','MarkerSize',20,'LineWidth',1.5) 
hold on
plot(real(zer),imag(zer),'ro','MarkerSize',10,'LineWidth',1.5)
xlabel('Eixo real');
ylabel('Eixo imaginário')
legend('Polos','Zeros')
grid on;

% Espaço de Estados 
sys = ss(A,B,C,D);

% Diagrama de Bode 
H1=tf(numerador(1,:),denominador);
H2=tf(numerador(2,:),denominador);
H3=tf(numerador(3,:),denominador);
H4=tf(numerador(4,:),denominador);
H5=tf(numerador(5,:),denominador);
H6=tf(numerador(6,:),denominador);

figure
bode(H1)
grid on
title('Diagrama de Bode para a variável u')

figure
bode(H2)
grid on
title('Diagrama de Bode para a variável w')

figure
bode(H3)
grid on
title('Diagrama de Bode para a variável q')

figure
bode(H6)
grid on
title('Diagrama de Bode para a variável \theta')

% Parâmetros da Simulação 
step = 0.005;
tempo_final = 10;
t = 0:step:tempo_final;
vet_temp = t;
n = length(t);
u = zeros(1,n); 
u(1,:) = deg2rad(0); 

y_0 = zeros(6,1);
def = rad2deg(u);

%% Solucao do sistema
% Solução do Sistema por ODE 
fun = @(t,x) odefun_linear(t,x,A,B,u,vet_temp);

[t_tot,y_tot] = ode45(fun, t, y_0);

% Plots 
u=y_tot(:,1);
w=y_tot(:,2);
q=y_tot(:,3);
x=y_tot(:,4);
z=y_tot(:,5);
alfa=rad2deg(y_tot(:,6));
t=t_tot;

figure
subplot(2,1,1)
plot(t,u,'LineWidth',2)
xlabel('Tempo [s]')
ylabel('u [m/s]')
grid on
subplot(2,1,2)
plot(t,w,'LineWidth',2)
xlabel('Tempo [s]')
ylabel('w [m/s]')
grid on

figure
subplot(2,1,1)
plot(t,x,'LineWidth',2)
xlabel('Tempo [s]')
ylabel('x [m]')
grid on
subplot(2,1,2)
plot(t,z,'LineWidth',2)
xlabel('Tempo [s]')
ylabel('z [s]')
grid on

figure
subplot(2,1,1)
plot(t,alfa,'LineWidth',2)
xlabel('Tempo [s]')
ylabel('\alpha [°]')
grid on
subplot(2,1,2)
plot(t,q,'LineWidth',2)
xlabel('Tempo [s]')
ylabel('q [°/s]')
grid on

figure
plot(t,def,'LineWidth',2)
xlabel('Tempo [s]')
ylabel('\delta_{prof} [°]')
grid on
end

function x_ponto = odefun_linear(t,x,A,B,u,vet_temp)
[~,instante] = min(abs(vet_temp-t)); 
u_t = u(:,instante);
x_ponto = A*x + B*u_t;
end

function Vb = body(y, V)
% Essa funcao transforma um vetor da base fixa no solo em uma na base fixa
% no aviao (body), pode ser linha ou coluna.
% y eh o vetor de estados no instante, pode ser linha ou coluna, apenas
% importam os angulos de Euler
% V eh o vetor a ser transformado

[l,~] = size(V);
vet_col=false;
if l==3
    vet_col = true;
    V = V';
end
M = [cos(y(6)) *cos(0), cos(y(6)) * sin(0), -sin(y(6));
    sin(0) *sin(y(6)) *cos(0) - cos(0) *sin(0), sin(0) *sin(y(6)) *sin(0) + cos(0) *cos(0), sin(0) *cos(y(6));
    cos(0) *sin(y(6)) *cos(0) + sin(0) *sin(0), cos(0) *sin(y(6)) *sin(0) - sin(0) *cos(0), cos(0) *cos(y(6))];
Vb = (M*V')';
if vet_col
    Vb = Vb';
end
end