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:
12:9fed6f656f88
Parent:
10:7074bc7038d6
Child:
15:155ca63b7884

File content as of revision 12:9fed6f656f88:

#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();
}

// Predict vertical position and velocity
void HorizontalEstimator::predict()
{
    u = u;
    v = v;
}

//
void HorizontalEstimator::update(float phi, float theta, float p, float q, float z)
{
    // 
    flow.read();
    float d = z/(cos(phi)*cos(theta));
    float u_m = (flow.x+q)*d;
    float v_m = (flow.y-p)*d;
    u = (1-rho_hor)*u+rho_hor*u_m;
    v = (1-rho_hor)*v+rho_hor*v_m;
}