Library containing Crazyflie 2.0 controller classes: - Attitude estimator - Horizontal estimator - Vertical estimator - Attitude controller - Horizontal controller - Vertical controller - Mixer
VerticalEstimator/VerticalEstimator.cpp
- Committer:
- fbob
- Date:
- 2018-12-06
- Revision:
- 24:7b9e3beb61d5
- Parent:
- 19:83b357d6806e
File content as of revision 24:7b9e3beb61d5:
#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; w = w; } // Correct vertical position and velocity with measurement void VerticalEstimator::correct(float phi, float theta) { range.read(); if (range.d < 2.0f) { float z_m = range.d*cos(phi)*cos(theta); float w_m = (z_m-z_m_last)/dt_range; z = (1-rho_ver)*z+rho_ver*z_m; w = (1-rho_ver)*w+rho_ver*w_m; z_m_last = z_m; } }