Library containing Crazyflie 2.0 controller classes: - Attitude estimator - Horizontal estimator - Vertical estimator - Attitude controller - Horizontal controller - Vertical controller - Mixer
HorizontaEstimator/HorizontalEstimator.cpp
- Committer:
- fbob
- Date:
- 2018-12-06
- Revision:
- 24:7b9e3beb61d5
- Parent:
- 22:5f2323e30cdc
File content as of revision 24:7b9e3beb61d5:
#include "mbed.h" #include "HorizontalEstimator.h" // Class constructor HorizontalEstimator::HorizontalEstimator() : flow(PA_7,PA_6,PA_5,PB_4) { x = 0.0f; y = 0.0f; u = 0.0f; v = 0.0f; x_m_last = 0.0f; y_m_last = 0.0f; } // Initialize class void HorizontalEstimator::init() { flow.init(); } // Predict horizontal velocity from model void HorizontalEstimator::predict() { x = x+u*dt; y = y+v*dt; u = u; v = v; } // Correct horizontal velocity with measurements void HorizontalEstimator::correct(float phi, float theta, float p, float q, float z) { float div = (cos(phi)*cos(theta)); if (div>0.5f) { flow.read(); float d = z/div; float u_m = (flow.px*sigma+q)*d; float v_m = (flow.py*sigma-p)*d; float x_m = x_m_last + u_m*dt_flow; float y_m = y_m_last + v_m*dt_flow; x = (1-rho_hor)*x+rho_hor*x_m; y = (1-rho_hor)*y+rho_hor*y_m; u = (1-rho_hor)*u+rho_hor*u_m; v = (1-rho_hor)*v+rho_hor*v_m; x_m_last = x_m; y_m_last = y_m; } }