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-27
Revision:
7:220ce3839be8
Child:
9:15058b4fa090

File content as of revision 7:220ce3839be8:

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

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

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

//
void VerticalEstimator::estimate(float phi, float theta)
{
    // 
    range.read();
    float z_new = range.z*cos(phi)*cos(theta);
    w = (z_new-z)/dt_pos;
    z = z_new;
}