File content as of revision 3:e782fe31ace2:

#ifndef AttitudeController_h
#define AttitudeController_h

#include "mbed.h"

// Controller time constants (s)
float const T_phi = 0.16f;
float const T_theta = 0.16f;
float const T_psi = 0.2f;
float const T_p = 0.04f;
float const T_q = 0.04f;
float const T_r = 0.1f;

// 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, float p, float q, float r);
    // Torques (N.m)
    float tau_phi, tau_theta, tau_psi;
  private:
    // Control torques given reference angles and current angles and angular velocities (with given time constants and moments of inertia)
    float control_single(float angle_r, float angle, float rate, float T_angle, float T_rate, float I);
};

#endif