Yuri De Stefani / CrazyflieController

controlador de atitude

VerticalEstimator/VerticalEstimator.cpp

Committer:
yurindes
Date:
2018-10-17
Revision:
5:b1f5ea192d12
Child:
6:7a447d4ae677

File content as of revision 5:b1f5ea192d12:

#include "mbed.h"
#include "VerticalEstimator.h"
// Class constructor
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;
}