AttitudeController.cpp

00001 #include "mbed.h"
00002 #include "Parameters.h"
00003 #include "AttitudeController.h"
00004 
00005 // Class constructor
00006 AttitudeController::AttitudeController()
00007 {
00008 }
00009 
00010 // Control torques given reference angles and current angles and angular velocities
00011 void AttitudeController::control( float phi_r , float theta_r , float psi_r , float phi, float theta , float psi , float p, float q, float r)
00012 {
00013     tau_phi = single_axis_control(phi_r, phi, p, phi_kp, phi_kd, I_xx);
00014     tau_theta = single_axis_control(theta_r, theta, q, theta_kp, theta_kd, I_yy);
00015     tau_psi = single_axis_control(psi_r, psi, r, psi_kp, psi_kd, I_zz);
00016 }
00017 // 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 )
00018 float AttitudeController::single_axis_control( float angle_r , float angle , float rate , float K_angle , float K_rate , float I)
00019 {
00020     return I*(K_angle*(angle_r - angle) - K_rate*rate);
00021 }