File content as of revision 0:b1f2c9e88e32:

#include "mbed.h"
#include "AttitudeController.h"

// Class constructor
AttitudeController::AttitudeController()
{
}

// Calculate torques given reference angles and current angles and rates 
void AttitudeController::calculate(float phi_r, float theta_r, float psi_r, float phi, float theta, float psi, float p, float q, float r)
{
    // Calculate torque given reference angle and current angle and rate (with given gains and moment of inertia)
    tau_phi = calculate_single(phi_r, phi, p, K_phi, K_p, I_xx);
    tau_theta = calculate_single(theta_r, theta, q, K_theta, K_q, I_yy);
    tau_psi = calculate_single(psi_r, psi, r, K_psi, K_r, I_zz);
}

// Calculate torque given reference angle and current angle and rate (with given gains and moment of inertia)
float AttitudeController::calculate_single(float angle_r, float angle, float rate, float K_angle, float K_rate, float I)
{
    // Calculate torque
    float rate_r = K_angle*(angle_r-angle);
    float accel_r = K_rate*(rate_r-rate);
    float tau = accel_r*I;
    return tau;
}