function Main_modelagem_nl
% 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 não linear da aeronave e calculará 
% 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;

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

% 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 
% alfa def T
x0 = [0;0;60];
rvs = 0.5*rho*V^2*S;

% Equações do Movimento 
fun2 = @(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(fun2,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]';

% Parâmetros da Simulação 
step = 0.001;
tempo_final = 10;
t = 0:step:tempo_final;
vet_temp = t;
n = length(t);
u = zeros(1,n); 
u(1,:) = def_trim  + deg2rad(0); % = def do prof durante a simulacao, mudar! 

y_0 = y_trim;
def =  rad2deg(u);

%% Solucao do sistema
% Solução do Sistema por ODE 
fun2 = @(t,y) odefun_nlinear(t, y, u, vet_temp, T_trim);

[t_tot,y_tot] = ode45(fun2, 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-y_0(1),'LineWidth',2)
xlabel('Tempo [s]')
ylabel('u [m/s]')
grid on
subplot(2,1,2)
plot(t,w-y_0(2),'LineWidth',2)
xlabel('Tempo [s]')
ylabel('w [m/s]')
grid on

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

figure
subplot(2,1,1)
plot(t,alfa-alpha_trim,'LineWidth',2)
xlabel('Tempo [s]')
ylabel('\alpha [°]')
grid on
subplot(2,1,2)
plot(t,q-y_0(3),'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 yp = odefun_nlinear(t,y,u,vet_temp,T_trim)
%% Valores
% Constantes 
rho = 1.1062;
g = 9.81;

% Geométricos 
m = 20;
W = m*g;
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;
Iner = [0.36 0 0.0071
    0 1.36 0
    0.0071 0 0.46];
Ixxb = Iner(1,1);
Iyyb = Iner(2,2);
Izzb = Iner(3,3);
Ixzb = Iner(1,3);

% 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;
CD_q = 0.0049;
CL_q = .0380;
CM_q = -0.0187;

% Acha def no instante 
[~,instante] = min(abs(vet_temp-t)); 
def = u(:,instante);

%% Estados
% vet_estados = 
% Vu Vw q xf zf theta
u   = y(1);
v   = 0;
w   = y(2);
p   = 0;
q   = y(3);
r   = 0;
phi = 0;
the = y(6);
psi = 0;

V = [u;v;w];
if u == 0
    alpha = 0;
else
    alpha = atan(w/u);
end
T = T_trim;

%% Forcas e momentos
V0=(u*u + v*v + w*w)^0.5;

CL = CL0 + CL_alpha*the + CL_def*def + CL_q*q;
CD = CD0 + CD_alpha*the + CD_def*def + CD_q*q;
CM = CM0 + CM_alpha*the + CM_def*def + CM_q*q;
rsv = 0.5*rho*S*V0^2;

F = [rsv*(CL*sin(alpha) - CD*cos(alpha)) * T*cos(alpha) - W*sin(the) 0,...
    rsv*(-CL*cos(alpha) - CD*sin(alpha)) + W*cos(phi)*cos(the)]; 
M = [0 rsv*mac*CM + T*(z_cg-z_motor) 0]; 

%% Derivadas do vetor de estados
% Aceleracoes lineares
up = F(1)/m + r*v - q*w;
vp = 0;
wp = F(3)/m + q*u - p*v;

% Aceleracoes angulares
aang = (Iner^-1)*[
     M(1) + (Iyyb -Izzb)*q*r + Ixzb*p*q;
     M(2) + (Izzb -Ixxb)*p*r + Ixzb*(r^2-p^2);
     M(3) + (Ixxb -Iyyb)*p*q - Ixzb*q*r];
pp = 0;
qp = aang(2);
rp = 0;

% Velocidades em relacao ao solo
vf = [
    cos(the)*cos(psi), sin(phi)*sin(the)*cos(psi) - cos(phi)*sin(psi), cos(phi)*sin(the)*cos(psi) + sin(phi)*sin(psi);
    cos(the)*sin(psi), sin(phi)*sin(the)*sin(psi) + cos(phi)*cos(psi), cos(phi)*sin(the)*sin(psi) - sin(phi)*cos(psi);
    -sin(the), sin(phi)*cos(the), cos(phi)*cos(the)] * V;
xp = vf(1);
yp = 0;
zp = vf(3);

% Variacao dos angulos de Euler
dang = [1, sin(phi)*sin(the)/cos(the), cos(phi)*sin(the)/cos(the);
    0, cos(phi), -sin(phi);
    0, sin(phi)/cos(the), cos(phi)/cos(the)]*[p;q;r];
phip = 0;
thep = dang(2);
psip = 0;

% Resultado
yp = [up;wp;qp;xp;zp;thep];

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