Library containing Crazyflie 2.0 controller classes: - Attitude estimator - Horizontal estimator - Vertical estimator - Attitude controller - Horizontal controller - Vertical controller - Mixer
HorizontaEstimator/HorizontalEstimator.cpp@19:83b357d6806e, 2018-10-18 (annotated)
- Committer:
- fbob
- Date:
- Thu Oct 18 12:53:22 2018 +0000
- Revision:
- 19:83b357d6806e
- Parent:
- 18:60516a4cba27
- Child:
- 22:5f2323e30cdc
Included zeta and omega_n
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
fbob | 8:d05fe9f8bfb6 | 1 | #include "mbed.h" |
fbob | 8:d05fe9f8bfb6 | 2 | #include "HorizontalEstimator.h" |
fbob | 8:d05fe9f8bfb6 | 3 | |
fbob | 8:d05fe9f8bfb6 | 4 | // Class constructor |
fbob | 8:d05fe9f8bfb6 | 5 | HorizontalEstimator::HorizontalEstimator() : flow(PA_7,PA_6,PA_5,PB_4) |
fbob | 8:d05fe9f8bfb6 | 6 | { |
fbob | 18:60516a4cba27 | 7 | x = 0.0f; |
fbob | 18:60516a4cba27 | 8 | y = 0.0f; |
fbob | 15:155ca63b7884 | 9 | u = 0.0f; |
fbob | 15:155ca63b7884 | 10 | v = 0.0f; |
fbob | 18:60516a4cba27 | 11 | x_m_last = 0.0f; |
fbob | 18:60516a4cba27 | 12 | y_m_last = 0.0f; |
fbob | 8:d05fe9f8bfb6 | 13 | } |
fbob | 8:d05fe9f8bfb6 | 14 | |
fbob | 8:d05fe9f8bfb6 | 15 | // Initialize class |
fbob | 8:d05fe9f8bfb6 | 16 | void HorizontalEstimator::init() |
fbob | 8:d05fe9f8bfb6 | 17 | { |
fbob | 8:d05fe9f8bfb6 | 18 | flow.init(); |
fbob | 8:d05fe9f8bfb6 | 19 | } |
fbob | 8:d05fe9f8bfb6 | 20 | |
fbob | 17:f682b4a5686d | 21 | // Predict horizontal velocity from model |
fbob | 12:9fed6f656f88 | 22 | void HorizontalEstimator::predict() |
fbob | 12:9fed6f656f88 | 23 | { |
fbob | 18:60516a4cba27 | 24 | x = x+u*dt; |
fbob | 18:60516a4cba27 | 25 | y = y+v*dt; |
fbob | 12:9fed6f656f88 | 26 | u = u; |
fbob | 12:9fed6f656f88 | 27 | v = v; |
fbob | 12:9fed6f656f88 | 28 | } |
fbob | 12:9fed6f656f88 | 29 | |
fbob | 19:83b357d6806e | 30 | // Correct horizontal velocity with measurements |
fbob | 18:60516a4cba27 | 31 | void HorizontalEstimator::correct(float phi, float theta, float p, float q, float z) |
fbob | 8:d05fe9f8bfb6 | 32 | { |
fbob | 15:155ca63b7884 | 33 | float div = (cos(phi)*cos(theta)); |
fbob | 15:155ca63b7884 | 34 | if (div>0.5f) |
fbob | 15:155ca63b7884 | 35 | { |
fbob | 18:60516a4cba27 | 36 | flow.read(); |
fbob | 15:155ca63b7884 | 37 | float d = z/div; |
fbob | 19:83b357d6806e | 38 | float u_m = (flow.px+q)*d; |
fbob | 19:83b357d6806e | 39 | float v_m = (flow.py-p)*d; |
fbob | 18:60516a4cba27 | 40 | float x_m = x_m_last + u_m*dt_flow; |
fbob | 18:60516a4cba27 | 41 | float y_m = y_m_last + v_m*dt_flow; |
fbob | 18:60516a4cba27 | 42 | x = (1-rho_hor)*x+rho_hor*x_m; |
fbob | 18:60516a4cba27 | 43 | y = (1-rho_hor)*y+rho_hor*y_m; |
fbob | 15:155ca63b7884 | 44 | u = (1-rho_hor)*u+rho_hor*u_m; |
fbob | 15:155ca63b7884 | 45 | v = (1-rho_hor)*v+rho_hor*v_m; |
fbob | 18:60516a4cba27 | 46 | x_m_last = x_m; |
fbob | 18:60516a4cba27 | 47 | y_m_last = y_m; |
fbob | 15:155ca63b7884 | 48 | } |
fbob | 8:d05fe9f8bfb6 | 49 | } |