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-10-05
Revision:
17:f682b4a5686d
Parent:
15:155ca63b7884
Child:
18:60516a4cba27

File content as of revision 17:f682b4a5686d:

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

// Class constructor
HorizontalEstimator::HorizontalEstimator() : flow(PA_7,PA_6,PA_5,PB_4)
{
    u = 0.0f;
    v = 0.0f;
}

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

// Predict horizontal velocity from model
void HorizontalEstimator::predict()
{
    u = u;
    v = v;
}

// Update horizontal velocity with measurement
void HorizontalEstimator::update(float phi, float theta, float p, float q, float z)
{
    flow.read();
    float div = (cos(phi)*cos(theta));
    if (div>0.5f)
    {
        float d = z/div;
        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;
    }
}