Diff: AttitudeController/AttitudeController.h

Revision:

0:3871dc7bedf7

Child:

2:54fabc943f25

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AttitudeController/AttitudeController.h	Wed Oct 10 11:13:07 2018 +0000
@@ -0,0 +1,41 @@
+#ifndef AttitudeController_h
+#define AttitudeController_h
+
+#include "mbed.h"
+#include "Library.h"
+
+/*
+float const dt2 = 0.005f;
+// Controller gains and /or time constants
+float const Kp_phi = 3.63*1.96;
+float const Kp_theta = Kp_phi;
+float const Td_phi = 1.96;
+float const Td_theta = Td_phi;
+
+float const Kp_psi = 2.57*0.81;
+float const Td_psi = 0.81;
+
+
+// Quadcopter moments of inertia (kg.m ^2)
+float const I_xx = 16.0e-6f;
+float const I_yy = 16.0e-6f;
+float const I_zz = 29.0e-6f;
+*/
+
+class AttitudeController
+{
+    public :
+        // Class constructor
+        AttitudeController () ;
+        // Control torques given reference angles and current angles and angular velocities
+            void control(float phi_r, float theta_r, float psi_r, float phi, float theta, float psi);
+        // Torques (N.m)
+        float tau_phi, tau_theta, tau_psi;
+    private :
+    // 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 single_axis_control(float angle_r, float angle, float K_angle, float K_rate , float I, float last_erro_angle);
+        float last_erro_phi, last_erro_theta, last_erro_psi;
+        float last_erro;
+};
+# endif
+