Diff: AttitudeController/AttitudeController.cpp

Revision:

21:169cc2b1d2ff

Parent:

19:83b357d6806e

--- a/AttitudeController/AttitudeController.cpp	Thu Oct 18 13:38:10 2018 +0000
+++ b/AttitudeController/AttitudeController.cpp	Wed Oct 24 14:24:56 2018 +0000
@@ -12,25 +12,14 @@
 // Control torques (N.m) given reference angles (rad) and current angles (rad) and angular velocities (rad/s)
 void AttitudeController::control(float phi_r, float theta_r, float psi_r, float phi, float theta, float psi, float p, float q, float r)
 {
-    //tau_phi = control_single(phi_r, phi, p, T_phi, T_p, I_xx);
-    //tau_theta = control_single(theta_r, theta, q, T_theta, T_q, I_yy);
-    //tau_psi = control_single(psi_r, psi, r, T_psi, T_r, I_zz);
-    tau_phi = control_single_regulator(phi_r, phi, p, K_phi, K_p, I_xx);
-    tau_theta = control_single_regulator(theta_r, theta, q, K_theta, K_q, I_yy);
-    tau_psi = control_single_regulator(psi_r, psi, r, K_psi, K_r, I_zz);
+    tau_phi = control_state_regulator(phi_r, phi, p, kp_phi, kd_phi, I_xx);
+    tau_theta = control_state_regulator(theta_r, theta, q, kp_theta, kd_theta, I_yy);
+    tau_psi = control_state_regulator(psi_r, psi, r, kp_psi, kd_psi, I_zz);
 }
 
 // Control torque (N.m) given reference angle (rad) and current angle (rad) and angular velocity (rad/s) with given time constants (s) and moment of inertia (kg.m^2)
-float AttitudeController::control_single(float angle_r, float angle, float angular_velocity, float T_angle, float T_angular_velocity, float I)
+float AttitudeController::control_state_regulator(float angle_r, float angle, float rate, float kp, float kd, float I)
 {
-    float angular_velocity_r = (1.0f/T_angle)*(angle_r-angle);
-    float angular_acceleration_r = (1.0f/T_angular_velocity)*(angular_velocity_r-angular_velocity);
-    float tau = angular_acceleration_r*I;
-    return tau;
-}
-
-// Control torque (N.m) given reference angle (rad) and current angle (rad) and angular velocity (rad/s) with given time constants (s) and moment of inertia (kg.m^2)
-float AttitudeController::control_single_regulator(float angle_r, float angle, float rate, float K_angle, float K_rate, float I)
-{
-    return I*(K_angle*(angle_r-angle)+K_rate*(0.0f-rate));
+    float acc_r =  I*(kp*(angle_r-angle)+kd*(0.0f-rate));
+    return acc_r;
 }
\ No newline at end of file