Library containing Crazyflie 2.0 controller classes: - Attitude estimator - Horizontal estimator - Vertical estimator - Attitude controller - Horizontal controller - Vertical controller - Mixer
HorizontaEstimator/HorizontalEstimator.cpp@12:9fed6f656f88, 2018-09-28 (annotated)
- Committer:
- fbob
- Date:
- Fri Sep 28 21:45:50 2018 +0000
- Revision:
- 12:9fed6f656f88
- Parent:
- 10:7074bc7038d6
- Child:
- 15:155ca63b7884
Parameters improvements
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 | 8:d05fe9f8bfb6 | 7 | } |
fbob | 8:d05fe9f8bfb6 | 8 | |
fbob | 8:d05fe9f8bfb6 | 9 | // Initialize class |
fbob | 8:d05fe9f8bfb6 | 10 | void HorizontalEstimator::init() |
fbob | 8:d05fe9f8bfb6 | 11 | { |
fbob | 8:d05fe9f8bfb6 | 12 | // Initialize flow sensor object |
fbob | 8:d05fe9f8bfb6 | 13 | flow.init(); |
fbob | 8:d05fe9f8bfb6 | 14 | } |
fbob | 8:d05fe9f8bfb6 | 15 | |
fbob | 12:9fed6f656f88 | 16 | // Predict vertical position and velocity |
fbob | 12:9fed6f656f88 | 17 | void HorizontalEstimator::predict() |
fbob | 12:9fed6f656f88 | 18 | { |
fbob | 12:9fed6f656f88 | 19 | u = u; |
fbob | 12:9fed6f656f88 | 20 | v = v; |
fbob | 12:9fed6f656f88 | 21 | } |
fbob | 12:9fed6f656f88 | 22 | |
fbob | 8:d05fe9f8bfb6 | 23 | // |
fbob | 12:9fed6f656f88 | 24 | void HorizontalEstimator::update(float phi, float theta, float p, float q, float z) |
fbob | 8:d05fe9f8bfb6 | 25 | { |
fbob | 8:d05fe9f8bfb6 | 26 | // |
fbob | 8:d05fe9f8bfb6 | 27 | flow.read(); |
fbob | 12:9fed6f656f88 | 28 | float d = z/(cos(phi)*cos(theta)); |
fbob | 12:9fed6f656f88 | 29 | float u_m = (flow.x+q)*d; |
fbob | 12:9fed6f656f88 | 30 | float v_m = (flow.y-p)*d; |
fbob | 12:9fed6f656f88 | 31 | u = (1-rho_hor)*u+rho_hor*u_m; |
fbob | 12:9fed6f656f88 | 32 | v = (1-rho_hor)*v+rho_hor*v_m; |
fbob | 8:d05fe9f8bfb6 | 33 | } |