Library containing Crazyflie 2.0 controller classes: - Attitude estimator - Horizontal estimator - Vertical estimator - Attitude controller - Horizontal controller - Vertical controller - Mixer
Parameters/Parameters.h@22:5f2323e30cdc, 2018-10-25 (annotated)
- Committer:
- fbob
- Date:
- Thu Oct 25 12:41:50 2018 +0000
- Revision:
- 22:5f2323e30cdc
- Parent:
- 21:169cc2b1d2ff
- Child:
- 23:4905fbc2b31a
Adjust flow resolution
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
fbob | 5:b9947e3d20cf | 1 | #ifndef Parameters_h |
fbob | 5:b9947e3d20cf | 2 | #define Parameters_h |
fbob | 5:b9947e3d20cf | 3 | |
fbob | 13:1a871ebd35bb | 4 | // Interrupt frequencies |
fbob | 15:155ca63b7884 | 5 | const float f = 500.0f; // Hz |
fbob | 13:1a871ebd35bb | 6 | const float f_range = 20.0f; // Hz |
fbob | 21:169cc2b1d2ff | 7 | const float f_flow = 500.0f; // Hz |
fbob | 13:1a871ebd35bb | 8 | const float dt = 1.0f/f; // s |
fbob | 13:1a871ebd35bb | 9 | const float dt_range = 1.0f/f_range;// s |
fbob | 13:1a871ebd35bb | 10 | const float dt_flow = 1.0f/f_flow; // s |
fbob | 9:15058b4fa090 | 11 | |
fbob | 21:169cc2b1d2ff | 12 | // Quadcopter dimensions |
fbob | 21:169cc2b1d2ff | 13 | const float m = 30e-3f; // kg |
fbob | 21:169cc2b1d2ff | 14 | const float I_xx = 16.0e-6f; // kg.m^2 |
fbob | 21:169cc2b1d2ff | 15 | const float I_yy = 16.0e-6f; // kg.m^2 |
fbob | 21:169cc2b1d2ff | 16 | const float I_zz = 29.0e-6f; // kg.m^2 |
fbob | 21:169cc2b1d2ff | 17 | const float l = 33e-3f; // m |
fbob | 21:169cc2b1d2ff | 18 | |
fbob | 21:169cc2b1d2ff | 19 | // Physical constants |
fbob | 21:169cc2b1d2ff | 20 | const float pi = 3.1416f; |
fbob | 21:169cc2b1d2ff | 21 | const float g = 9.81f; // m/s^2 |
fbob | 21:169cc2b1d2ff | 22 | |
fbob | 5:b9947e3d20cf | 23 | // Motor constants |
fbob | 5:b9947e3d20cf | 24 | const float alpha = 1.16e-07f; |
fbob | 5:b9947e3d20cf | 25 | const float beta = 7.149e-10f; |
fbob | 5:b9947e3d20cf | 26 | |
fbob | 5:b9947e3d20cf | 27 | // Propeller constants |
fbob | 9:15058b4fa090 | 28 | const float kl = 1.726e-08f; // N.s^2/rad^2 |
fbob | 9:15058b4fa090 | 29 | const float kd = 1.426e-10f; // N.m.s^2/rad^2 |
fbob | 5:b9947e3d20cf | 30 | |
fbob | 9:15058b4fa090 | 31 | // Attitude estimator weighthing (accelerometer X gyroscope) |
fbob | 15:155ca63b7884 | 32 | const float rho_att = 0.01f; |
fbob | 5:b9947e3d20cf | 33 | |
fbob | 19:83b357d6806e | 34 | // Attitude controller gains (roll/pitch) |
fbob | 21:169cc2b1d2ff | 35 | const float Ts_phi = 0.4f; // s |
fbob | 21:169cc2b1d2ff | 36 | const float OS_phi = 0.005f; // % |
fbob | 19:83b357d6806e | 37 | const float zeta_phi = abs(log(OS_phi))/sqrt(pow(log(OS_phi),2)+pow(pi,2)); |
fbob | 19:83b357d6806e | 38 | const float omega_n_phi = 4.0f/(Ts_phi*zeta_phi); |
fbob | 21:169cc2b1d2ff | 39 | const float kp_phi = pow(omega_n_phi,2.0f); |
fbob | 21:169cc2b1d2ff | 40 | const float kd_phi = 2.0f*zeta_phi*omega_n_phi; |
fbob | 21:169cc2b1d2ff | 41 | const float kp_theta = kp_phi; |
fbob | 21:169cc2b1d2ff | 42 | const float kd_theta = kd_phi; |
fbob | 19:83b357d6806e | 43 | |
fbob | 19:83b357d6806e | 44 | // Attitude controller gains (yaw) |
fbob | 20:4d0614886c80 | 45 | const float Ts_psi = 0.8f; // s |
fbob | 22:5f2323e30cdc | 46 | const float OS_psi = 0.005f; // % |
fbob | 19:83b357d6806e | 47 | const float zeta_psi = abs(log(OS_psi))/sqrt(pow(log(OS_psi),2)+pow(pi,2)); |
fbob | 19:83b357d6806e | 48 | const float omega_n_psi = 4.0f/(Ts_psi*zeta_psi); |
fbob | 21:169cc2b1d2ff | 49 | const float kd_psi = pow(omega_n_psi,2.0f); |
fbob | 21:169cc2b1d2ff | 50 | const float kp_psi = 2.0f*zeta_psi*omega_n_psi; |
fbob | 5:b9947e3d20cf | 51 | |
fbob | 9:15058b4fa090 | 52 | // Vertical estimator weighthing (measurement X prediction) |
fbob | 9:15058b4fa090 | 53 | const float rho_ver = 0.3f; |
fbob | 7:220ce3839be8 | 54 | |
fbob | 21:169cc2b1d2ff | 55 | // Vertical controller gains (z) |
fbob | 22:5f2323e30cdc | 56 | const float Ts_z = 2.0f; // s |
fbob | 22:5f2323e30cdc | 57 | const float OS_z = 0.005f; // % |
fbob | 19:83b357d6806e | 58 | const float zeta_z = abs(log(OS_z))/sqrt(pow(log(OS_z),2)+pow(pi,2)); |
fbob | 19:83b357d6806e | 59 | const float omega_n_z = 4.0f/(Ts_z*zeta_z); |
fbob | 21:169cc2b1d2ff | 60 | const float kp_z = pow(omega_n_z,2.0f); |
fbob | 21:169cc2b1d2ff | 61 | const float kd_z = 2.0f*zeta_z*omega_n_z; |
fbob | 9:15058b4fa090 | 62 | |
fbob | 22:5f2323e30cdc | 63 | // Optical flow sensor constant |
fbob | 22:5f2323e30cdc | 64 | const float gamma = 42.0f; |
fbob | 22:5f2323e30cdc | 65 | const float resolution = 420.0f; |
fbob | 22:5f2323e30cdc | 66 | const float sigma = (1.0f/dt_flow)*(2.0f*tan((gamma*pi/180.0f)/2.0f))/resolution; |
fbob | 22:5f2323e30cdc | 67 | |
fbob | 9:15058b4fa090 | 68 | // Horizontal estimator weighthing (measurement X prediction) |
fbob | 22:5f2323e30cdc | 69 | const float rho_hor = 0.3f; |
fbob | 8:d05fe9f8bfb6 | 70 | |
fbob | 21:169cc2b1d2ff | 71 | // Horizontal controller gains (x/y) |
fbob | 22:5f2323e30cdc | 72 | const float Ts_x = 4.0f; // s |
fbob | 22:5f2323e30cdc | 73 | const float OS_x = 0.005f; // % |
fbob | 21:169cc2b1d2ff | 74 | const float zeta_x = abs(log(OS_x))/sqrt(pow(log(OS_x),2)+pow(pi,2)); |
fbob | 19:83b357d6806e | 75 | const float omega_n_x = 4.0f/(Ts_x*zeta_x); |
fbob | 21:169cc2b1d2ff | 76 | const float kp_x = pow(omega_n_x,2.0f); |
fbob | 21:169cc2b1d2ff | 77 | const float kd_x = 2.0f*zeta_x*omega_n_x; |
fbob | 21:169cc2b1d2ff | 78 | const float kp_y = kp_x; |
fbob | 21:169cc2b1d2ff | 79 | const float kd_y = kd_x; |
fbob | 7:220ce3839be8 | 80 | |
fbob | 6:3188b00263e8 | 81 | #endif |