Fabio Bobrow / CrazyflieController

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

HorizontalController/HorizontalController.cpp

Committer:
fbob
Date:
2018-10-05
Revision:
16:54d2f299e404
Parent:
15:155ca63b7884
Child:
17:f682b4a5686d

File content as of revision 16:54d2f299e404:

#include "mbed.h"
#include "HorizontalController.h"

// Class constructor
HorizontalController::HorizontalController()
{
    phi_r = 0.0f;
    theta_r = 0.0f;
}

// 
void HorizontalController::control(float u_r, float v_r, float u, float v)
{
    phi_r = -control_single(v_r,v,T_v)/g;
    theta_r = control_single(u_r,u,T_u)/g;
}

// 
void HorizontalController::control_take_off(float u_r, float v_r, float u, float v)
{
    phi_r = -control_single(v_r,v,T_v_take_off)/g;
    theta_r = control_single(u_r,u,T_u_take_off)/g;
}

// 
float HorizontalController::control_single(float velocity_r, float velocity, float T_velocity)
{
    return (1.0f/T_velocity)*(velocity_r-velocity);    
}