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-12-06
Revision:
24:7b9e3beb61d5
Parent:
21:169cc2b1d2ff

File content as of revision 24:7b9e3beb61d5:

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

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

// Control reference roll and pitch angles (rad) given reference positions (m) and current positions (m) and velocities (m/s)
void HorizontalController::control(float x_r, float y_r, float x, float y, float u, float v)
{
    phi_r = -control_state_regulator(y_r,y,v,kp_y,kd_y)/g;
    theta_r = control_state_regulator(x_r,x,u,kp_x,kd_x)/g;
}

// Control acceleration given reference position (m) and current position (m) and velocity (m/s) with given controller gains
float HorizontalController::control_state_regulator(float pos_r, float pos, float vel, float kp, float kd)
{
    float acc_r = kp*(pos_r-pos) + kd*(0.0f-vel); 
    return acc_r;
}