File content as of revision 2:54fabc943f25:

#ifndef AttitudeController_h
#define AttitudeController_h

#include "mbed.h"
#include "Library.h"

/*
float const dt2 = 0.005f;
// Controller gains and /or time constants
float const Kp_phi = 3.63*1.96;
float const Kp_theta = Kp_phi;
float const Td_phi = 1.96;
float const Td_theta = Td_phi;

float const Kp_psi = 2.57*0.81;
float const Td_psi = 0.81;


// Quadcopter moments of inertia (kg.m ^2)
float const I_xx = 16.0e-6f;
float const I_yy = 16.0e-6f;
float const I_zz = 29.0e-6f;
*/

class AttitudeController
{
    public :
        // Class constructor
        AttitudeController () ;
        // Control torques given reference angles and current angles and angular velocities
            void control(float phi_r, float theta_r, float psi_r, float phi, float theta, float psi);
        // Torques (N.m)
        float tau_phi, tau_theta, tau_psi;
        
    private :
    // Control torque of a single axis given reference angles and current angles and angular velocities ( with given gains and /or time constants and moments of inertia )
        float single_axis_control(float angle_r, float angle, float K_angle, float K_rate , float I, float last_erro_angle);
        
        float last_erro_phi, last_erro_theta, last_erro_psi;
        
        float last_erro;
};
# endif