controlador de atitude
Diff: VerticalEstimator/VerticalEstimator.cpp
- Revision:
- 5:b1f5ea192d12
- Child:
- 6:7a447d4ae677
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/VerticalEstimator/VerticalEstimator.cpp Wed Oct 17 12:13:33 2018 +0000 @@ -0,0 +1,31 @@ +#include "mbed.h" +#include "VerticalEstimator.h" +// Class constructor +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; +}