Yuri De Stefani / CrazyflieController

controlador de atitude

Library/Library.h@8:c96125e9ac70, 2018-11-21 (annotated)

Committer:
yurindes
Date:
Wed Nov 21 10:07:00 2018 +0000
Branch:
yuri
Revision:
8:c96125e9ac70
Parent:
1:579511e9f0b8 
teste controlador atitude;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yvesyuzo 0:3871dc7bedf7 1 #ifndef Library_h
yvesyuzo 0:3871dc7bedf7 2 #define Library_h
yurindes 8:c96125e9ac70 3 #include <math.h>
yurindes 8:c96125e9ac70 4
yurindes 8:c96125e9ac70 5 float const pi = 3.14159265f;
yvesyuzo 1:579511e9f0b8 6 //Attitude Controller VARIABLES
yurindes 8:c96125e9ac70 7 float const dt_alti = 0.005f;
yvesyuzo 0:3871dc7bedf7 8 // Controller gains and /or time constants
yurindes 8:c96125e9ac70 9 float const Ts = 2.0f;//0.3f;
yurindes 8:c96125e9ac70 10 float const OS = 0.5f;
yurindes 8:c96125e9ac70 11
yurindes 8:c96125e9ac70 12 float const zeta = sqrt(log(OS/100.0f)*log(OS/100.0f))/(sqrt(log(OS/100.0f)*log(OS/100.0f)+pi*pi));
yurindes 8:c96125e9ac70 13 float const omega_n = 4.0f/(Ts*zeta);
yurindes 8:c96125e9ac70 14
yurindes 8:c96125e9ac70 15 float const Kp_phi = omega_n*omega_n;
yvesyuzo 0:3871dc7bedf7 16 float const Kp_theta = Kp_phi;
yurindes 8:c96125e9ac70 17 float const Td_phi = 1/(zeta*2*Kp_phi*sqrt(Kp_phi));
yvesyuzo 0:3871dc7bedf7 18 float const Td_theta = Td_phi;
yurindes 8:c96125e9ac70 19
yurindes 8:c96125e9ac70 20 float const Ts2 = 0.6f;
yurindes 8:c96125e9ac70 21 float const OS2= 0.5f;
yurindes 8:c96125e9ac70 22
yurindes 8:c96125e9ac70 23 float const zeta2 = sqrt(log(OS2/100.0f)*log(OS2/100.0f))/(sqrt(log(OS2/100.0f)*log(OS2/100.0f)+3.1415f*3.1415f));
yurindes 8:c96125e9ac70 24 float const omega_n2 = 4.0f/(Ts2*zeta2);
yurindes 8:c96125e9ac70 25
yurindes 8:c96125e9ac70 26 float const Kp_psi = omega_n2*omega_n2;
yurindes 8:c96125e9ac70 27 float const Td_psi = 4.0f/(Ts2*zeta2);
yvesyuzo 0:3871dc7bedf7 28 // Quadcopter moments of inertia (kg.m ^2)
yvesyuzo 0:3871dc7bedf7 29 float const I_xx = 16.0e-6f;
yvesyuzo 0:3871dc7bedf7 30 float const I_yy = 16.0e-6f;
yvesyuzo 0:3871dc7bedf7 31 float const I_zz = 29.0e-6f;
yvesyuzo 1:579511e9f0b8 32 // Attitude Estimator constants
yurindes 8:c96125e9ac70 33 float const dt = 0.002f;
yurindes 8:c96125e9ac70 34 float const rho = 0.01f;
yvesyuzo 0:3871dc7bedf7 35 // Mixer
yvesyuzo 0:3871dc7bedf7 36 const float alpha = 1.081E-7;
yvesyuzo 0:3871dc7bedf7 37 const float beta = 2.678E-11;
yvesyuzo 0:3871dc7bedf7 38 const float kl = 2.69E-8;
yvesyuzo 0:3871dc7bedf7 39 const float kd = 1.59E-10;
yvesyuzo 0:3871dc7bedf7 40 const float l = 33E-3;
yurindes 8:c96125e9ac70 41 const float m = 0.03f;
yurindes 8:c96125e9ac70 42 const float g = 9.81f;
yvesyuzo 0:3871dc7bedf7 43 # endif