Diff: Parameters/Parameters.h

Revision:

21:169cc2b1d2ff

Parent:

20:4d0614886c80

Child:

22:5f2323e30cdc

--- a/Parameters/Parameters.h	Thu Oct 18 13:38:10 2018 +0000
+++ b/Parameters/Parameters.h	Wed Oct 24 14:24:56 2018 +0000
@@ -4,11 +4,22 @@
 // Interrupt frequencies
 const float f = 500.0f;             // Hz
 const float f_range = 20.0f;        // Hz 
-const float f_flow = 200.0f;        // Hz 
+const float f_flow = 500.0f;        // Hz 
 const float dt = 1.0f/f;            // s
 const float dt_range = 1.0f/f_range;// s
 const float dt_flow = 1.0f/f_flow;  // s 
 
+// Quadcopter dimensions
+const float m = 30e-3f;             // kg
+const float I_xx = 16.0e-6f;        // kg.m^2
+const float I_yy = 16.0e-6f;        // kg.m^2
+const float I_zz = 29.0e-6f;        // kg.m^2
+const float l = 33e-3f;             // m
+
+// Physical constants
+const float pi = 3.1416f;
+const float g = 9.81f;              // m/s^2
+
 // Motor constants 
 const float alpha = 1.16e-07f;
 const float beta = 7.149e-10f;
@@ -17,79 +28,50 @@
 const float kl = 1.726e-08f;        // N.s^2/rad^2
 const float kd = 1.426e-10f;        // N.m.s^2/rad^2
 
-// Quadcopter dimensions
-const float l = 0.033f;             // m
-const float m = 0.030f;             // kg
-const float I_xx = 16.0e-6f;        // kg.m^2
-const float I_yy = 16.0e-6f;        // kg.m^2
-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.32f;         // s
-const float OS_phi = 0.0043f;       // %
+const float Ts_phi = 0.4f;         // s
+const float OS_phi = 0.005f;       // %
 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; 
+const float kp_phi = pow(omega_n_phi,2.0f);       
+const float kd_phi = 2.0f*zeta_phi*omega_n_phi; 
+const float kp_theta = kp_phi;       
+const float kd_theta = kd_phi; 
 
 // Attitude controller gains (yaw)
 const float Ts_psi = 0.8f;          // s
-const float OS_psi = 0.0432f;       // %
+const float OS_psi = 0.005f;       // %
 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
-const float T_psi = 0.2f;           // s
-const float T_p = 0.04f;            // s
-const float T_q = 0.04f;            // s
-const float T_r = 0.1f;             // s
+const float kd_psi = pow(omega_n_psi,2.0f);       
+const float kp_psi = 2.0f*zeta_psi*omega_n_psi; 
 
 // Vertical estimator weighthing (measurement X prediction)
 const float rho_ver = 0.3f;        
 
-// Vertical controller gains
-const float Ts_z = 2.8571f;         // s
-const float OS_z = 0.0460f;          // %
+// Vertical controller gains (z)
+const float Ts_z = 2.0f;         // s
+const float OS_z = 0.005f;         // %
 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);       
-const float K_w_take_off = 2.0f*zeta_take_off*omega_n_take_off;  
+const float kp_z = pow(omega_n_z,2.0f);       
+const float kd_z = 2.0f*zeta_z*omega_n_z; 
 
 // Horizontal estimator weighthing (measurement X prediction)
-const float rho_hor = 0.5f;   
+const float rho_hor = 0.3f;   
 
-// Horizontal controller gains
-const float Ts_x = 8.0f;         // s
-const float zeta_x = 1.118;
+// Horizontal controller gains (x/y)
+const float Ts_x = 4.0f;         // s
+const float OS_x = 0.005f;         // %
+const float zeta_x = abs(log(OS_x))/sqrt(pow(log(OS_x),2)+pow(pi,2));
+//const float zeta_x = 1.5;
 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
-const float T_u_take_off = 4.0f;    // s  
-const float T_v_take_off = 4.0f;    // s
+const float kp_x = pow(omega_n_x,2.0f);       
+const float kd_x = 2.0f*zeta_x*omega_n_x; 
+const float kp_y = kp_x;       
+const float kd_y = kd_x; 
 
 #endif
\ No newline at end of file