Library containing Crazyflie 2.0 controller classes: - Attitude estimator - Horizontal estimator - Vertical estimator - Attitude controller - Horizontal controller - Vertical controller - Mixer
VerticalEstimator/VerticalEstimator.cpp@11:fad579538b4c, 2018-09-28 (annotated)
- Committer:
- fbob
- Date:
- Fri Sep 28 18:52:33 2018 +0000
- Revision:
- 11:fad579538b4c
- Parent:
- 9:15058b4fa090
- Child:
- 13:1a871ebd35bb
Vertical estimator improvements
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
fbob | 7:220ce3839be8 | 1 | #include "mbed.h" |
fbob | 7:220ce3839be8 | 2 | #include "VerticalEstimator.h" |
fbob | 7:220ce3839be8 | 3 | |
fbob | 7:220ce3839be8 | 4 | // Class constructor |
fbob | 7:220ce3839be8 | 5 | VerticalEstimator::VerticalEstimator() : range(PB_7,PB_6) |
fbob | 7:220ce3839be8 | 6 | { |
fbob | 7:220ce3839be8 | 7 | } |
fbob | 7:220ce3839be8 | 8 | |
fbob | 7:220ce3839be8 | 9 | // Initialize class |
fbob | 7:220ce3839be8 | 10 | void VerticalEstimator::init() |
fbob | 7:220ce3839be8 | 11 | { |
fbob | 7:220ce3839be8 | 12 | range.init(); |
fbob | 7:220ce3839be8 | 13 | } |
fbob | 7:220ce3839be8 | 14 | |
fbob | 9:15058b4fa090 | 15 | // Predict vertical position and velocity |
fbob | 9:15058b4fa090 | 16 | void VerticalEstimator::predict() |
fbob | 9:15058b4fa090 | 17 | { |
fbob | 11:fad579538b4c | 18 | z = z+w*dt_att; |
fbob | 11:fad579538b4c | 19 | w = w; |
fbob | 11:fad579538b4c | 20 | } |
fbob | 11:fad579538b4c | 21 | |
fbob | 11:fad579538b4c | 22 | // Update vertical position and velocity by merging range sensor readings |
fbob | 11:fad579538b4c | 23 | void VerticalEstimator::update(float phi, float theta) |
fbob | 11:fad579538b4c | 24 | { |
fbob | 11:fad579538b4c | 25 | range.read(); |
fbob | 11:fad579538b4c | 26 | float z_m = range.z*cos(phi)*cos(theta); |
fbob | 11:fad579538b4c | 27 | float w_m = (z_m-z_m_last)/dt_pos; |
fbob | 11:fad579538b4c | 28 | z = (1-rho_ver)*z+rho_ver*z_m; |
fbob | 11:fad579538b4c | 29 | w = (1-rho_ver)*w+rho_ver*w_m; |
fbob | 11:fad579538b4c | 30 | z_m_last = z_m; |
fbob | 11:fad579538b4c | 31 | } |
fbob | 11:fad579538b4c | 32 | |
fbob | 11:fad579538b4c | 33 |