AttitudeController.cpp

00001 #include "mbed.h"
00002 #include "AttitudeController.h"
00003 #include "Library.h"
00004 
00005 // Class constructor
00006 AttitudeController :: AttitudeController ()
00007 {
00008 }
00009 // Control torques given reference angles and current angles and angular velocities
00010 void AttitudeController::control ( float phi_r, float theta_r, float psi_r, float phi, float theta, float psi)
00011 {
00012     tau_phi = single_axis_control ( phi_r , phi , Kp_phi , Td_phi , I_xx, last_erro_phi);
00013 
00014     last_erro_phi = last_erro;
00015 
00016     tau_theta = single_axis_control (  theta_r , theta , Kp_theta , Td_theta , I_yy, last_erro_theta);
00017 
00018     last_erro_theta = last_erro;
00019 
00020     tau_psi = single_axis_control ( psi_r , psi , Kp_psi , Td_psi , I_zz, last_erro_psi);
00021 
00022     last_erro_psi = last_erro;
00023 }
00024 // 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 )
00025 float AttitudeController :: single_axis_control ( float angle_r , float angle , float K_angle , float K_rate , float I, float last_erro_angle)
00026 {
00027     float erro = angle_r - angle;
00028     float erro_p = (erro - last_erro_angle)/dt_alti;
00029     last_erro = erro;
00030     float angle_2p = K_angle*(erro+1/K_rate*erro_p);
00031     return I*angle_2p;
00032 }