Fabio Bobrow / CrazyflieController

Library containing Crazyflie 2.0 controller classes: - Attitude estimator - Horizontal estimator - Vertical estimator - Attitude controller - Horizontal controller - Vertical controller - Mixer

VerticalEstimator/VerticalEstimator.cpp

Committer:
fbob
Date:
2018-09-28
Revision:
9:15058b4fa090
Parent:
7:220ce3839be8
Child:
11:fad579538b4c

File content as of revision 9:15058b4fa090:

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

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

// Initialize class 
void VerticalEstimator::init()
{
    range.init();
}

// Estimate vertical position and velocity
void VerticalEstimator::estimate(float phi, float theta)
{
    range.read();
    measure(phi,theta);
    predict();
    z = rho_ver*z_m+(1-rho_ver)*z_p;
    w = rho_ver*w_m+(1-rho_ver)*w_p;
}

// Measure vertical position and velocity
void VerticalEstimator::measure(float phi, float theta)
{ 
    float z_m_new = range.z*cos(phi)*cos(theta);
    w_m = (z_m_new-z_m)/dt_pos;
    z_m = z_m_new;
}

// Predict vertical position and velocity
void VerticalEstimator::predict()
{
    z_p = z+w*dt_pos;
    w_p = w;
}