clear
clc
xdel(winsid())
// Massa do quadrotor
mt=0.76105;

// Momentos de inercia
Ix=0.2;
Iy=0.2;
Iz=0.03;

// Gravidade
g=-9.81;

// Areas frontal (Ax) e lateral
Ax=0.008553265;
Ay=Ax;
Az=0.0131505375;

// Coeficientes de arrasto
Cdx=0.13
Cdy=Cdx;
Cdz=0.27;

// Densidade do ar
ro=1.2041;

// Forcas
Fsus=7.4659005 // Forca de sustentacao
Fmax=8.277 // Forca maxima

// Tempo
t0 = 0;
tfinal = 100;
tpasso = 0.1;

t = [t0:tpasso:tfinal];

//-------------------------------

// Matriz A

A= [0 0 0 1 0 0 0 -15 0 0 0 0;
    0 0 0 0 0 0 0 0 0 0 0 0;
    0 0 0 0 0 1 0 15 0 0 0 0;
    0 0 0 -30*ro*Az*Cdz/mt 0 0 0 -g 0 0 -15 0;
    0 0 0 0 0 0 0 0 0 0 0 0;
    0 0 0 0 0 -30*ro*Az*Cdz/mt 0 0 0 0 15 0;
    0 0 0 0 0 0 0 0 0 0 0 0;
    0 0 0 0 0 0 0 0 0 0 1 0;
    0 0 0 0 0 0 0 0 0 0 0 0;
    0 0 0 0 0 0 0 0 0 0 0 0;
    0 0 0 0 0 0 0 0 0 0 0 0;
    0 0 0 0 0 0 0 0 0 0 0 0]

// Matriz B

B=[
0 0 0 0 0;
0 0 0 0 0;
0 0 0 0 0;
0 0 0 0 225*ro*Ax*Cdx/mt;
0 0 0 0 0;
0 0 0 1/mt g+225*ro*Ax*Cdx/mt;
0 0 0 0 0;
0 0 0 0 0;
0 0 0 0 0;
0 0 0 0 0;
0 1/Iy 0 0 0;
0 0 0 0 0;
]

// Matriz C

C = [1 0 0 0 0 0 0 0 0 0 0 0;
     0 1 0 0 0 0 0 0 0 0 0 0;
     0 0 1 0 0 0 0 0 0 0 0 0;
     0 0 0 1 0 0 0 0 0 0 0 0;
     0 0 0 0 1 0 0 0 0 0 0 0;
     0 0 0 0 0 1 0 0 0 0 0 0;
     0 0 0 0 0 0 1 0 0 0 0 0;
     0 0 0 0 0 0 0 1 0 0 0 0;
     0 0 0 0 0 0 0 0 1 0 0 0;
     0 0 0 0 0 0 0 0 0 0 0 0;
     0 0 0 0 0 0 0 0 0 0 0 0;
     0 0 0 0 0 0 0 0 0 0 0 0]

// Matriz D

D=[0 0 0 0 0;
   0 0 0 0 0;
   0 0 0 0 0;
   0 0 0 0 0;
   0 0 0 0 0;
   0 0 0 0 0;
   0 0 0 0 0;
   0 0 0 0 0;
   0 0 0 0 0;
   0 0 0 0 0;
   0 0 0 0 0;
   0 0 0 0 0;]

// Sistema linear
sistema = syslin('c',A,B,C,D);

//-------------------------------

// Funcoes de transferencia
G1=ss2tf(sistema);

// Mostra as funcoes de transferencia no terminal
disp(G1)

// Definicao de s
s = poly(0, 's');

// Remover o comentario da funcao desejada para obter os diagramas

// Entrada Ttheta
h = syslin('c', (36.410231-150*s +(2.960e-14)*(s^2)-(7.827e-15)*(s^3))/(((0.1685303*s^3)+s^4))); // Saida x
//h = syslin('c', (12.639769+150*s +(4.607e-14)*(s^2)+(3.636e-15)*(s^3))/(((0.1685303*s^3)+s^4))); // Saida z
//h = syslin('c', (49.05-75*s -(1.998e-15)*(s^2))/(((0.1685303*s^2)+s^3))); // Saida u
//h = syslin('c', (75 +(1.804e-15)*(s))/(((0.1685303*s)+s^2))); // Saida w
//h = syslin('c', (5 +(3.925e-17)*(s))/(s^2)); Saida theta

// Entrada F
//h = syslin('c', (1.3139741 -(4.163e-16)*(s))/(((0.1685303*s)+s^2))); // Saida z
//h = syslin('c', (1.3139741 )/(((0.1685303)+s))); // Saida w

// Diagrama de polos
f10=scf(1);
plzr(h)

// Diagrama de bode
f10=scf(2);
clf(); bode(h, 0.01, 1e20);

//-------------------------------

// Entradas
// Torque em phi
u1=zeros (t);
u1(:)=0;

// Torque em theta
u2=zeros (t);
u2(:)=0;

// Torque em psi
u3=zeros (t);
u3(:)=0;

// Forca
u4=zeros (t);
u4(:)=Fmax;

// Entrada unitaria
u5=zeros (t);
u5(:)=1;

// Matriz U
U = [u1; u2; u3; u4; u5];

// Condicoes iniciais
x0e = [0; 0; 0; 0; 0; 0; 0; 15*%pi/180; 0; 0; 0; 0];


// Simulacao
[y,x] = csim (U, t, sistema, x0e); 

//-------------------------------

// Variaveis para o plot
X = x(1,:);
Y = x(2,:);
Z = x(3,:);
u = x(4,:);
v = x(5,:);
w = x(6,:);
phi = x(7,:);
theta = x(8,:);
psi = x(9,:);
p = x(10,:);
q = x(11,:);
r = x(12,:);

// Plot dos gráficos de deslocamento e velocidade
f10=scf(3);
subplot(231)
plot(t,X);
title("Deslocamento eixo x","fontsize", 3);
xlabel("tempo[s]","fontsize",3);
ylabel("deslocamento[m]","fontsize",3);
rect=[0,0,100,1400];
plotframe(rect)

subplot(232)
plot(t,Y);
title("Deslocamento no eixo y","fontsize", 3);
xlabel("tempo[s]","fontsize", 3);
ylabel("deslocamento[m]","fontsize",3);

subplot(233)
plot(t,Z);
title("Deslocamento no eixo z","fontsize", 3);
xlabel("tempo[s]","fontsize", 3);
ylabel("deslocamento[m]","fontsize",3);

subplot(234)
plot(t,u);
title("Velocidade no eixo x","fontsize", 3);
xlabel("tempo[s]","fontsize", 3);
ylabel("velocidade[m/s]","fontsize",3);

subplot(235)
plot(t,v);
title("Velocidade no eixo y","fontsize", 3);
xlabel("tempo[s]","fontsize", 3);
ylabel("velocidade[m/s]","fontsize",3);

subplot(236)
plot(t,y(6,:));
title("Velocidade no eixo z","fontsize", 3);
xlabel("tempo[s]","fontsize", 3);
ylabel("velocidade[m/s]","fontsize",3);

// Plot dos gráficos de deslocamento e velocidade angular
f10=scf(4);
subplot(231)
plot(t,phi);
title("Deslocamento angular no eixo x","fontsize", 3);
xlabel("tempo[s]","fontsize",3);
ylabel("${\phi}[rad]$","fontsize",4);

subplot(232)
plot(t,theta);
title("Deslocamento angular no eixo y","fontsize", 3);
xlabel("tempo[s]","fontsize", 3);
ylabel("${\theta}[rad]$","fontsize",4);
rect=[0,-1,100,1];
plotframe(rect)

subplot(233)
plot(t,psi);
title("Deslocamento angular no eixo z","fontsize", 3);
xlabel("tempo[s]","fontsize", 3);
ylabel("${\psi}[rad]$","fontsize",4);

subplot(234)
plot(t,p);
title("Velocidade angular no eixo x","fontsize", 3);
xlabel("tempo[s]","fontsize", 3);
ylabel("$\dot{\varphi}[rad/s]$","fontsize",4);

subplot(235)
plot(t,q);
title("Velocidade angular no eixo y","fontsize", 3);
xlabel("tempo[s]","fontsize", 3);
ylabel("$\dot{\theta}[rad/s]$","fontsize",4);
rect=[0,-1,100,1];
plotframe(rect)

subplot(236)
plot(t,r);
title("Velocidade angular no eixo z","fontsize", 3);
xlabel("tempo[s]","fontsize", 3);
ylabel("$\dot{\psi}[rad/s]$","fontsize",4);