Rodrigo Gikas / CrazyflieController

Crazyflie 2.0 Controller

Dependents:   Drone_Controlador_Atitude

VerticalEstimator/VerticalEstimator.cpp

Committer:
IgneousGuikas
Date:
2018-11-21
Revision:
2:9e07bed8e8ed

File content as of revision 2:9e07bed8e8ed:

#include "mbed.h"
#include "VerticalEstimator.h"

// Class constructor
VerticalEstimator::VerticalEstimator() : range(PB_7,PB_6)
{
    z = 0.0f;
    w = 0.0f;
    z_m_last = 0.0f;
}
// Initialize class
void VerticalEstimator::init()
{
    range.init();
}
// Predict vertical position and velocity from model
void VerticalEstimator::predict()
{
    z = z + w*dt;
}
// Correct vertical position and velocity with measurement
void VerticalEstimator::correct(float phi,float theta)
{
    range.read();
    float z_m = range.z*cos(phi)*cos(theta);
    float w_m = (z_m - z_m_last)/dt_range;
    
    z = (1.0f - rho_vert_corr)*z + rho_vert_corr*z_m;
    w = (1.0f - rho_vert_corr)*w + rho_vert_corr*w_m;
    
    z_m_last = z_m;
}