Fabio Bobrow / CrazyflieController

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

HorizontaEstimator/HorizontalEstimator.cpp

Committer:
fbob
Date:
2018-09-28
Revision:
10:7074bc7038d6
Parent:
8:d05fe9f8bfb6
Child:
12:9fed6f656f88

File content as of revision 10:7074bc7038d6:

#include "mbed.h"
#include "HorizontalEstimator.h"

// Class constructor
HorizontalEstimator::HorizontalEstimator() : flow(PA_7,PA_6,PA_5,PB_4)
{
}

// Initialize class 
void HorizontalEstimator::init()
{
    // Initialize flow sensor object
    flow.init();
}

//
void HorizontalEstimator::estimate(float p, float q, float z)
{
    // 
    flow.read();
    u = 0.5f*u+0.5f*(flow.x-p)*z;
    v = 0.5f*v+0.5f*(flow.y-q)*z;
}