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:
11:fad579538b4c
Parent:
9:15058b4fa090
Child:
13:1a871ebd35bb

File content as of revision 11:fad579538b4c:

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

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

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

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

// Update vertical position and velocity by merging range sensor readings
void VerticalEstimator::update(float phi, float theta)
{
    range.read();
    float z_m = range.z*cos(phi)*cos(theta);
    float w_m = (z_m-z_m_last)/dt_pos;
    z = (1-rho_ver)*z+rho_ver*z_m;
    w = (1-rho_ver)*w+rho_ver*w_m;
    z_m_last = z_m;
}