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-10-03
- Revision:
- 13:1a871ebd35bb
- Parent:
- 11:fad579538b4c
- Child:
- 15:155ca63b7884
File content as of revision 13:1a871ebd35bb:
#include "mbed.h" #include "VerticalEstimator.h" // Class constructor VerticalEstimator::VerticalEstimator() : range(PB_7,PB_6) { } // Initialize class void VerticalEstimator::init() { range.init(); } // Predict vertical position and velocity void VerticalEstimator::predict() { z = z+w*dt; w = w; } // Update vertical position and velocity by merging range sensor readings void VerticalEstimator::update(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-rho_ver)*z+rho_ver*z_m; w = (1-rho_ver)*w+rho_ver*w_m; z_m_last = z_m; }