Diff: AttitudeController/AttitudeController.cpp

Revision:

0:3871dc7bedf7

Child:

2:54fabc943f25

diff -r 000000000000 -r 3871dc7bedf7 AttitudeController/AttitudeController.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AttitudeController/AttitudeController.cpp	Wed Oct 10 11:13:07 2018 +0000
@@ -0,0 +1,28 @@
+#include "mbed.h"
+#include "AttitudeController.h"
+#include "Library.h"
+
+
+// Class constructor
+AttitudeController :: AttitudeController ()
+{
+}
+// Control torques given reference angles and current angles and angular velocities
+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;
+    psi_r=single_axis_control (  psi_r , psi , Kp_psi , Td_psi , I_zz, last_erro_psi);
+    last_erro_psi=last_erro;
+    theta_r=single_axis_control ( theta_r , theta , Kp_theta , Td_theta , I_yy, last_erro_theta);
+    last_erro_theta=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;
+    last_erro=erro;
+    float angle_2p = K_angle*(erro+1/K_rate*erro_p);
+    return I*angle_2p;
+}