Fabio Bobrow / CrazyflieController

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

VerticalController/VerticalController.cpp

Committer:
fbob
Date:
2018-12-06
Revision:
24:7b9e3beb61d5
Parent:
21:169cc2b1d2ff

File content as of revision 24:7b9e3beb61d5:

#include "mbed.h"
#include "VerticalController.h"

// Class constructor
VerticalController::VerticalController()
{
    f_t = 0.0f;
}

// Control thrust force (N) given vertical position (m) and velocity (m/s)
void VerticalController::control(float z_r, float z, float w)
{
    f_t = m*(g+control_state_regulator(z_r,z,w,kp_z,kd_z));
}

// Control aceleration given reference position (m) and current position (m) and velocity (m/s) with given controller gains
float VerticalController::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;
}