Diff: Library/Library.h

Branch:

yuri

Revision:

8:c96125e9ac70

Parent:

1:579511e9f0b8

--- a/Library/Library.h	Wed Oct 17 12:18:29 2018 +0000
+++ b/Library/Library.h	Wed Nov 21 10:07:00 2018 +0000
@@ -1,26 +1,43 @@
 #ifndef Library_h
 #define Library_h
+#include <math.h>
+
+ float const pi = 3.14159265f;
 //Attitude Controller VARIABLES
-float const dt2 = 0.005f;
+float const dt_alti = 0.005f;
 // Controller gains and /or time constants
-float const Kp_phi = 3.63*1.96;
+float const Ts = 2.0f;//0.3f;
+float const OS = 0.5f;
+
+float const zeta = sqrt(log(OS/100.0f)*log(OS/100.0f))/(sqrt(log(OS/100.0f)*log(OS/100.0f)+pi*pi));
+float const omega_n = 4.0f/(Ts*zeta);
+
+float const Kp_phi = omega_n*omega_n;
 float const Kp_theta = Kp_phi;
-float const Td_phi = 1.96;
+float const Td_phi = 1/(zeta*2*Kp_phi*sqrt(Kp_phi));
 float const Td_theta = Td_phi;
-float const Kp_psi = 2.57*0.81;
-float const Td_psi = 0.81;
+
+float const Ts2 = 0.6f;
+float const OS2= 0.5f;
+
+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));
+float const omega_n2 = 4.0f/(Ts2*zeta2);
+
+float const Kp_psi = omega_n2*omega_n2;
+float const Td_psi = 4.0f/(Ts2*zeta2);
 // Quadcopter moments of inertia (kg.m ^2)
 float const I_xx = 16.0e-6f;
 float const I_yy = 16.0e-6f;
 float const I_zz = 29.0e-6f;
 // Attitude Estimator constants
- float const pi = 3.14159265f;
- float const dt = 0.005f;
- float const rho = 0.05f;
+ float const dt = 0.002f;
+ float const rho = 0.01f;
  // Mixer
 const float alpha = 1.081E-7;
 const float beta = 2.678E-11;
 const float kl = 2.69E-8;
 const float kd = 1.59E-10;
 const float l = 33E-3;
+const float m = 0.03f;
+const float g = 9.81f;
 # endif