controlador de atitude
VerticalEstimator/VerticalEstimator.cpp
- Committer:
- yurindes
- Date:
- 2018-10-17
- Revision:
- 6:7a447d4ae677
- Parent:
- 5:b1f5ea192d12
File content as of revision 6:7a447d4ae677:
#include "mbed.h" #include "VerticalEstimator.h" // Class constructor hhh VerticalEstimator :: VerticalEstimator() : range (PB_7 , PB_6 ) { z=0; w=0; z_m_last=0; } // Initialize class void VerticalEstimator :: init() { range.init(); } // Predict vertical position and velocity from model void VerticalEstimator :: predict() { z=z+0.05f*w; w=w; } // Correct vertical position and velocity with measurement void VerticalEstimator :: correct ( float phi , float theta ) { p=0.5f; range.read; float z_m = range.z; float w_m = (z-z_m)/0.05f; z=(1-p)*z+p*z_m; w=(1-p)*w+p*w_m; z_m_last = z_m; }