File content as of revision 0:b1f2c9e88e32:

#ifndef AttitudeController_h
#define AttitudeController_h

#include "mbed.h"

// Controller gains
float const K_phi = 6.25f;
float const K_theta = 6.25f;
float const K_psi = 5.0f;
float const K_p = 25.0f;
float const K_q = 25.0f;
float const K_r = 10.0f;

// Quadcopter moments of inertia
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 calculate(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 calculate_single(float angle_r, float angle, float rate, float K_angle, float K_rate, float I);
    
};

#endif