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-12-06
Revision:
24:7b9e3beb61d5
Parent:
19:83b357d6806e

File content as of revision 24:7b9e3beb61d5:

#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;
    w = w;
}

// Correct vertical position and velocity with measurement
void VerticalEstimator::correct(float phi, float theta)
{
    range.read();
    if (range.d < 2.0f)
    {
        float z_m = range.d*cos(phi)*cos(theta);
        float w_m = (z_m-z_m_last)/dt_range;
        z = (1-rho_ver)*z+rho_ver*z_m;
        w = (1-rho_ver)*w+rho_ver*w_m;
        z_m_last = z_m;
    }
}