Crazyflie 2.0 Controller
Dependents: Drone_Controlador_Atitude
Diff: VerticalEstimator/VerticalEstimator.cpp
- Revision:
- 2:9e07bed8e8ed
diff -r 2abfa02e99d5 -r 9e07bed8e8ed VerticalEstimator/VerticalEstimator.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/VerticalEstimator/VerticalEstimator.cpp Wed Nov 21 10:46:29 2018 +0000 @@ -0,0 +1,32 @@ +#include "mbed.h" +#include "VerticalEstimator.h" + +// Class constructor +VerticalEstimator::VerticalEstimator() : range(PB_7,PB_6) +{ + z = 0.0f; + w = 0.0f; + z_m_last = 0.0f; +} +// Initialize class +void VerticalEstimator::init() +{ + range.init(); +} +// Predict vertical position and velocity from model +void VerticalEstimator::predict() +{ + z = z + w*dt; +} +// Correct vertical position and velocity with measurement +void VerticalEstimator::correct(float phi,float theta) +{ + range.read(); + float z_m = range.z*cos(phi)*cos(theta); + float w_m = (z_m - z_m_last)/dt_range; + + z = (1.0f - rho_vert_corr)*z + rho_vert_corr*z_m; + w = (1.0f - rho_vert_corr)*w + rho_vert_corr*w_m; + + z_m_last = z_m; +} \ No newline at end of file