Diff: Parameters/Parameters.h

Revision:

19:83b357d6806e

Parent:

17:f682b4a5686d

Child:

20:4d0614886c80

--- a/Parameters/Parameters.h	Wed Oct 10 10:48:52 2018 +0000
+++ b/Parameters/Parameters.h	Thu Oct 18 12:53:22 2018 +0000
@@ -25,11 +25,30 @@
 const float I_zz = 29.0e-6f;        // kg.m^2
 
 // Gravity constant
+const float pi = 3.1416f;
 const float g = 9.81f;              // m/s^2
 
 // Attitude estimator weighthing (accelerometer X gyroscope)
 const float rho_att = 0.01f;        
 
+// Attitude controller gains (roll/pitch)
+const float Ts_phi = 0.9599f;       // s
+const float OS_phi = 0.3878f;       // %
+const float zeta_phi = abs(log(OS_phi))/sqrt(pow(log(OS_phi),2)+pow(pi,2));
+const float omega_n_phi = 4.0f/(Ts_phi*zeta_phi);
+const float K_phi = pow(omega_n_phi,2.0f);       
+const float K_p = 2.0f*zeta_phi*omega_n_phi; 
+const float K_theta = K_phi;       
+const float K_q = K_p; 
+
+// Attitude controller gains (yaw)
+const float Ts_psi = 0.9599f;       // s
+const float OS_psi = 0.3878f;       // %
+const float zeta_psi = abs(log(OS_psi))/sqrt(pow(log(OS_psi),2)+pow(pi,2));
+const float omega_n_psi = 4.0f/(Ts_psi*zeta_psi);
+const float K_psi = pow(omega_n_psi,2.0f);       
+const float K_r = 2.0f*zeta_psi*omega_n_psi; 
+
 // Attitude controller time constants
 const float T_phi = 0.12f;          // s
 const float T_theta = 0.12f;        // s
@@ -41,11 +60,15 @@
 // Vertical estimator weighthing (measurement X prediction)
 const float rho_ver = 0.3f;        
 
-// Vertical controller time constants
-const float zeta = 0.7f;     
-const float omega_n = 2.0f;         // rad/s
-const float K_z = pow(omega_n,2.0f);       
-const float K_w = 2.0f*zeta*omega_n;  
+// Vertical controller gains
+const float Ts_z = 2.8571f;         // s
+const float OS_z = 0.0460f;          // %
+const float zeta_z = abs(log(OS_z))/sqrt(pow(log(OS_z),2)+pow(pi,2));
+const float omega_n_z = 4.0f/(Ts_z*zeta_z);
+const float K_z = pow(omega_n_z,2.0f);       
+const float K_w = 2.0f*zeta_z*omega_n_z; 
+
+// Vertical controller gains
 const float zeta_take_off = 1.4f;     
 const float omega_n_take_off = 2.0f;// rad/s
 const float K_z_take_off = pow(omega_n_take_off,2.0f);       
@@ -54,6 +77,15 @@
 // Horizontal estimator weighthing (measurement X prediction)
 const float rho_hor = 0.5f;   
 
+// Horizontal controller gains
+const float Ts_x = 8.0f;         // s
+const float zeta_x = 1.118;
+const float omega_n_x = 4.0f/(Ts_x*zeta_x);
+const float K_x = pow(omega_n_x,2.0f);       
+const float K_u = 2.0f*zeta_x*omega_n_x; 
+const float K_y = K_x;       
+const float K_v = K_v; 
+
 // Horizontal controller time constants
 const float T_u = 2.0f;             // s  
 const float T_v = 2.0f;             // s