controlador de atitude
Diff: AttitudeController/AttitudeController.cpp
- Branch:
- yuri
- Revision:
- 8:c96125e9ac70
- Parent:
- 2:54fabc943f25
--- a/AttitudeController/AttitudeController.cpp Wed Oct 17 12:18:29 2018 +0000 +++ b/AttitudeController/AttitudeController.cpp Wed Nov 21 10:07:00 2018 +0000 @@ -2,7 +2,6 @@ #include "AttitudeController.h" #include "Library.h" - // Class constructor AttitudeController :: AttitudeController () { @@ -11,22 +10,22 @@ void AttitudeController::control ( float phi_r, float theta_r, float psi_r, float phi, float theta, float psi) { tau_phi = single_axis_control ( phi_r , phi , Kp_phi , Td_phi , I_xx, last_erro_phi); - + last_erro_phi = last_erro; - - tau_theta = single_axis_control ( psi_r , psi , Kp_psi , Td_psi , I_zz, last_erro_psi); - + + tau_theta = single_axis_control ( theta_r , theta , Kp_theta , Td_theta , I_yy, last_erro_theta); + last_erro_theta = last_erro; - - tau_psi = single_axis_control ( theta_r , theta , Kp_theta , Td_theta , I_yy, last_erro_theta); - + + tau_psi = single_axis_control ( psi_r , psi , Kp_psi , Td_psi , I_zz, last_erro_psi); + last_erro_psi = last_erro; } // 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 ) float AttitudeController :: single_axis_control ( float angle_r , float angle , float K_angle , float K_rate , float I, float last_erro_angle) { float erro = angle_r - angle; - float erro_p = (erro - last_erro_angle)/dt2; + float erro_p = (erro - last_erro_angle)/dt_alti; last_erro = erro; float angle_2p = K_angle*(erro+1/K_rate*erro_p); return I*angle_2p;