VerticalEstimator.cpp

00001 #include "mbed.h"
00002 #include "VerticalEstimator.h"
00003 //#include <math.h>
00004 #include "Library.h"
00005 
00006 // Class constructor
00007 VerticalEstimator::VerticalEstimator():range (PB_7 , PB_6 )
00008 {
00009 }
00010 
00011 // Initialize class
00012 void VerticalEstimator::init ()
00013 {
00014     VL53L0X range (PB_7 , PB_6 );
00015     range.init() ;
00016     
00017     z = 0.0f;//leitura da distancia vertical para função predict
00018     zm = 0.0f;//definido uma leitura para distancia vertical
00019     zml = 0.0f;//definindo uma segunda leitura para calcular a velocidade vertical
00020     z_est = 0.0f;
00021     
00022     w = 0.0f;//definido a velocidade vertical para função predict
00023     wm = 0.0f;
00024     w_est = 0.0f;  
00025     
00026 }
00027 // Predict vertical position and velocity from model
00028 void VerticalEstimator::predict ()
00029 {
00030     range.read();
00031     z = range.d;
00032     z = z + w*dt_ver_pre;//velocidade prevista, como a leitura é feita com 500Hz 0.002f sao os 2ms equivalentes
00033 }
00034 
00035 // Correct vertical position and velocity with measurement
00036 void VerticalEstimator::correct ( float phi , float theta )//phi e theta são os angulos de euler estimados
00037 {
00038 
00039     zm = range.d;
00040     zm = zm*cos(phi)*cos(theta);//zm é a distancia medida e phi e theta sao os angulos de Euler para correcao da medida
00041     wait(dt_ver_cor);
00042     zml = range.d;
00043     zm = zml*cos(phi)*cos(theta);//d é a distancia medida e phi e theta sao os angulos de Euler para correcao da medida
00044     wm = (zm-zml)/dt_ver_cor;
00045 
00046     z_est = (1-p_verti)*z + p_verti*zm;
00047     w_est = (1-p_verti)*w + p_verti*wm;
00048 
00049 }