File content as of revision 7:fb6174d2ea78:

#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() ;
    
    float z = 0.0f;//leitura da distancia vertical para função predict
    float zm = 0.0f;//definido uma leitura para distancia vertical
    float zml = 0.0f;//definindo uma segunda leitura para calcular a velocidade vertical
    float z_est = 0.0f;
    
    float w = 0.0f;//definido a velocidade vertical para função predict
    float wm = 0.0f;
    float w_est = 0.0f;  
    
    float pon = 0.3f;//constante de ponderação
    
}
// Predict vertical position and velocity from model
void VerticalEstimator::predict ()
{
    range.read();
    z = range.d;
    z = z + w*0.002f;//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(0.05);
    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)/0.05f;

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

}