File content as of revision 8:1ad52489f6f3:

#include "mbed.h"
#include "VerticalEstimator.h"
//#include <math.h>
#include "Library.h"

// Class constructor
VerticalEstimator::VerticalEstimator():range (PB_7 , PB_6 )
{
}

// Initialize class
void VerticalEstimator::init ()
{
    VL53L0X range (PB_7 , PB_6 );
    range.init() ;
    
    z = 0.0f;//leitura da distancia vertical para função predict
    zm = 0.0f;//definido uma leitura para distancia vertical
    zml = 0.0f;//definindo uma segunda leitura para calcular a velocidade vertical
    z_est = 0.0f;
    
    w = 0.0f;//definido a velocidade vertical para função predict
    wm = 0.0f;
    w_est = 0.0f;  
    
}
// Predict vertical position and velocity from model
void VerticalEstimator::predict ()
{
    range.read();
    z = range.d;
    z = z + w*dt_ver_pre;//velocidade prevista, como a leitura é feita com 500Hz 0.002f sao os 2ms equivalentes
}

// Correct vertical position and velocity with measurement
void VerticalEstimator::correct ( float phi , float theta )//phi e theta são os angulos de euler estimados
{

    zm = range.d;
    zm = zm*cos(phi)*cos(theta);//zm é a distancia medida e phi e theta sao os angulos de Euler para correcao da medida
    wait(dt_ver_cor);
    zml = range.d;
    zm = zml*cos(phi)*cos(theta);//d é a distancia medida e phi e theta sao os angulos de Euler para correcao da medida
    wm = (zm-zml)/dt_ver_cor;

    z_est = (1-p_verti)*z + p_verti*zm;
    w_est = (1-p_verti)*w + p_verti*wm;

}