Crazyflie 2.0 Controller
Dependents: Drone_Controlador_Atitude
VerticalEstimator/VerticalEstimator.cpp@2:9e07bed8e8ed, 2018-11-21 (annotated)
- Committer:
- IgneousGuikas
- Date:
- Wed Nov 21 10:46:29 2018 +0000
- Revision:
- 2:9e07bed8e8ed
Teste;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
IgneousGuikas | 2:9e07bed8e8ed | 1 | #include "mbed.h" |
IgneousGuikas | 2:9e07bed8e8ed | 2 | #include "VerticalEstimator.h" |
IgneousGuikas | 2:9e07bed8e8ed | 3 | |
IgneousGuikas | 2:9e07bed8e8ed | 4 | // Class constructor |
IgneousGuikas | 2:9e07bed8e8ed | 5 | VerticalEstimator::VerticalEstimator() : range(PB_7,PB_6) |
IgneousGuikas | 2:9e07bed8e8ed | 6 | { |
IgneousGuikas | 2:9e07bed8e8ed | 7 | z = 0.0f; |
IgneousGuikas | 2:9e07bed8e8ed | 8 | w = 0.0f; |
IgneousGuikas | 2:9e07bed8e8ed | 9 | z_m_last = 0.0f; |
IgneousGuikas | 2:9e07bed8e8ed | 10 | } |
IgneousGuikas | 2:9e07bed8e8ed | 11 | // Initialize class |
IgneousGuikas | 2:9e07bed8e8ed | 12 | void VerticalEstimator::init() |
IgneousGuikas | 2:9e07bed8e8ed | 13 | { |
IgneousGuikas | 2:9e07bed8e8ed | 14 | range.init(); |
IgneousGuikas | 2:9e07bed8e8ed | 15 | } |
IgneousGuikas | 2:9e07bed8e8ed | 16 | // Predict vertical position and velocity from model |
IgneousGuikas | 2:9e07bed8e8ed | 17 | void VerticalEstimator::predict() |
IgneousGuikas | 2:9e07bed8e8ed | 18 | { |
IgneousGuikas | 2:9e07bed8e8ed | 19 | z = z + w*dt; |
IgneousGuikas | 2:9e07bed8e8ed | 20 | } |
IgneousGuikas | 2:9e07bed8e8ed | 21 | // Correct vertical position and velocity with measurement |
IgneousGuikas | 2:9e07bed8e8ed | 22 | void VerticalEstimator::correct(float phi,float theta) |
IgneousGuikas | 2:9e07bed8e8ed | 23 | { |
IgneousGuikas | 2:9e07bed8e8ed | 24 | range.read(); |
IgneousGuikas | 2:9e07bed8e8ed | 25 | float z_m = range.z*cos(phi)*cos(theta); |
IgneousGuikas | 2:9e07bed8e8ed | 26 | float w_m = (z_m - z_m_last)/dt_range; |
IgneousGuikas | 2:9e07bed8e8ed | 27 | |
IgneousGuikas | 2:9e07bed8e8ed | 28 | z = (1.0f - rho_vert_corr)*z + rho_vert_corr*z_m; |
IgneousGuikas | 2:9e07bed8e8ed | 29 | w = (1.0f - rho_vert_corr)*w + rho_vert_corr*w_m; |
IgneousGuikas | 2:9e07bed8e8ed | 30 | |
IgneousGuikas | 2:9e07bed8e8ed | 31 | z_m_last = z_m; |
IgneousGuikas | 2:9e07bed8e8ed | 32 | } |