controlador de atitude
Diff: VerticalEstimator/VerticalEstimator.cpp
- Branch:
- yuri
- Revision:
- 8:c96125e9ac70
- Parent:
- 6:7a447d4ae677
diff -r 7a447d4ae677 -r c96125e9ac70 VerticalEstimator/VerticalEstimator.cpp --- a/VerticalEstimator/VerticalEstimator.cpp Wed Oct 17 12:18:29 2018 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,31 +0,0 @@ -#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; -}