Diff: AttitudeController/AttitudeController.cpp

Branch:

yuri

Revision:

8:1ad52489f6f3

Parent:

2:54fabc943f25

diff -r 7a447d4ae677 -r 1ad52489f6f3 AttitudeController/AttitudeController.cpp
--- a/AttitudeController/AttitudeController.cpp	Wed Oct 17 12:18:29 2018 +0000
+++ b/AttitudeController/AttitudeController.cpp	Fri Nov 30 19:23:29 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;