File content as of revision 2:7e01bc32bf4c:

#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();
    // Calculate torques given reference angles and current angles and rates
    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:
    // Calculate torque given reference angle and current angle and rate (with given gains and moment of inertia)
    float cascate_control(float angle_r, float angle, float rate, float K_angle, float K_rate, float I);
};

#endif