File content as of revision 8:1ad52489f6f3:

#include "mbed.h"
#include "HorizontalController.h"
// Class constructor


// na biblioteca
HorizontalController :: HorizontalController ()
{
    
    phi_r=0;
    theta_r=0;
    kp_hori = 1.35f;
    kd_hori = 2;
    x_e_last=0;
    y_e_last=0;
}
// Control reference roll and pitch angles given reference horizontal positions and current horizontal positions and velocities
void HorizontalController :: control ( float x_r , float y_r , float x, float y, float u, float v)
{
    theta_r = (1 / g)* control_state_regulator (x_r , x , x_e_last , kp_hori , kd_hori);
    x_e_last = pos_e_last;
    phi_r = - (1 / g)* control_state_regulator(y_r , y , y_e_last , kp_hori , kd_hori);
    y_e_last = pos_e_last;
}
// Control acceleration given reference position and current position and velocity with given controller gains
float HorizontalController :: control_state_regulator ( float pos_r , float pos , float pos_e_last, float kp,  float kd)
{
    float pos_e_hori = pos_r - pos;
    float vel = (pos_e_hori - pos_e_last) / delta_hori;
    pos_e_last = pos_e_hori;
    float pos_e_hori_2ponto = kp * pos_e_hori + kd * vel;  
    return pos_e_hori_2ponto;
}