integral action-LQR control for quadcopter UAV

libraries/AC_CustomControl/AC_CustomControl.cpp

@@ -8,6 +8,7 @@
 #include "AC_CustomControl_Backend.h"
 // #include "AC_CustomControl_Empty.h"
 #include "AC_CustomControl_PID.h"
+#include "AC_CustomControl_LQR.h"
 #include <GCS_MAVLink/GCS.h>
 
 // table of user settable parameters
@@ -33,6 +34,9 @@ const AP_Param::GroupInfo AC_CustomControl::var_info[] = {
     // parameters for PID controller
     AP_SUBGROUPVARPTR(_backend, "2_", 7, AC_CustomControl, _backend_var_info[1]),
 
+    // parameters for LQR controller
+    AP_SUBGROUPVARPTR(_backend, "3_", 8, AC_CustomControl, _backend_var_info[2]),
+
     AP_GROUPEND
 };
 
@@ -62,6 +66,10 @@ void AC_CustomControl::init(void)
             _backend = new AC_CustomControl_PID(*this, _ahrs, _att_control, _motors, _dt);
             _backend_var_info[get_type()] = AC_CustomControl_PID::var_info;
             break;
+        case CustomControlType::CONT_LQR:
+            _backend = new AC_CustomControl_LQR(*this, _ahrs, _att_control, _motors, _dt);
+            _backend_var_info[get_type()] = AC_CustomControl_LQR::var_info;
+            break;
         default:
             return;
     }

libraries/AC_CustomControl/AC_CustomControl.h

@@ -13,7 +13,7 @@
 #if AP_CUSTOMCONTROL_ENABLED
 
 #ifndef CUSTOMCONTROL_MAX_TYPES
-#define CUSTOMCONTROL_MAX_TYPES 2
+#define CUSTOMCONTROL_MAX_TYPES 3
 #endif
 
 class AC_CustomControl_Backend;
@@ -48,6 +48,7 @@ class AC_CustomControl {
         CONT_NONE            = 0,
         CONT_EMPTY           = 1,
         CONT_PID             = 2,
+        CONT_LQR             = 3,
     };            // controller that should be used     
 
     enum class  CustomControlOption {

libraries/AC_CustomControl/AC_CustomControl_LQR.cpp

@@ -0,0 +1,114 @@
+#include "AC_CustomControl_LQR.h"
+
+#if CUSTOMCONTROL_LQR_ENABLED
+
+#include <GCS_MAVLink/GCS.h>
+
+// table of user settable parameters
+const AP_Param::GroupInfo AC_CustomControl_LQR::var_info[] = {
+    // @Param: PARAM1
+    // @DisplayName: LQR param1
+    // @Description: Dumy parameter for LQR custom controller backend
+    // @User: Advanced
+    AP_GROUPINFO("PARAM1", 1, AC_CustomControl_LQR, param1, 0.0f),
+
+    // @Param: PARAM2
+    // @DisplayName: LQR param2
+    // @Description: Dumy parameter for LQR custom controller backend
+    // @User: Advanced
+    AP_GROUPINFO("PARAM2", 2, AC_CustomControl_LQR, param2, 0.0f),
+
+    // @Param: PARAM3
+    // @DisplayName: LQR param3
+    // @Description: Dumy parameter for LQR custom controller backend
+    // @User: Advanced
+    AP_GROUPINFO("PARAM3", 3, AC_CustomControl_LQR, param3, 0.0f),
+
+    AP_GROUPEND
+};
+
+// initialize in the constructor
+AC_CustomControl_LQR::AC_CustomControl_LQR(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl*& att_control, AP_MotorsMulticopter*& motors, float dt) :
+    AC_CustomControl_Backend(frontend, ahrs, att_control, motors, dt)
+{
+    _dt = dt;
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+// update controller
+// return roll, pitch, yaw controller output
+Vector3f AC_CustomControl_LQR::update(void)
+{
+    // reset controller based on spool state
+    switch (_motors->get_spool_state()) {
+        case AP_Motors::SpoolState::SHUT_DOWN:
+        case AP_Motors::SpoolState::GROUND_IDLE:
+            // We are still at the ground. Reset custom controller to avoid
+            // build up, ex: integrator
+            reset();
+            break;
+
+        case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
+        case AP_Motors::SpoolState::SPOOLING_UP:
+        case AP_Motors::SpoolState::SPOOLING_DOWN:
+            // we are off the ground
+            break;
+    }
+
+    // arducopter main attitude controller already runned
+    // we don't need to do anything else
+
+    //gcs().send_text(MAV_SEVERITY_INFO, "LQR custom controller working");
+
+    Quaternion attitude_body, attitude_target;
+   _ahrs->get_quat_body_to_ned(attitude_body);
+
+
+    attitude_target = _att_control->get_attitude_target_quat();
+
+
+    Quaternion attitude_error_quat = attitude_target.inverse() * attitude_body;
+
+
+// axis-angle representation of quaternion is used. The resulting //state-space for attitude control is [v1 v2 v3 p q r]
+    Vector3f attitude_error_angle;
+    attitude_error_quat.to_axis_angle(attitude_error_angle);
+
+
+    Vector3f gyro_latest = _ahrs->get_gyro_latest();
+
+
+// k_gain LQR matrix is defined as [kij], where i:[T1 T2 T3](torques in  // three axis) and j:[v1 v2 v3 p q r](roll pitch yaw roll-rate //pitch-rate yaw-rate)
+// k_gain = [k11 k12 k13 k14 k15 k16;
+//       k21 k22 k23 k24 k25 k26;
+//       k31 k32 k33 k34 k35 k36;]
+// [T1 T2 T3] = -k_gain*[v1 v2 v3 p q r]
+    _integralX += 0.01*attitude_error_angle.x*_dt;
+    _integralY += 0.01*attitude_error_angle.y*_dt;
+    _integralZ += 0.01*attitude_error_angle.z*_dt;
+    constrain_float(_integralX, -_kimax_LQR, _kimax_LQR);
+    constrain_float(_integralY, -_kimax_LQR, _kimax_LQR);
+    constrain_float(_integralZ, -_kimax_LQR, _kimax_LQR);
+
+    Vector3f motor_out;
+
+    motor_out.x = -_integralX - 0.45*attitude_error_angle.x - 0.094*gyro_latest.x;
+
+    motor_out.y = -_integralY - 0.45*attitude_error_angle.y - 0.094*gyro_latest.y;
+
+    motor_out.z = -_integralZ - 0.45*attitude_error_angle.z - 0.094*gyro_latest.z;
+
+   return motor_out;
+
+}
+
+// reset controller to avoid build up on the ground
+// or to provide bumpless transfer from arducopter main controller
+void AC_CustomControl_LQR::reset(void)
+{
+    _integralX = 0;
+    _integralY = 0;
+    _integralZ = 0;
+}
+
+#endif

libraries/AC_CustomControl/AC_CustomControl_LQR.h

@@ -0,0 +1,34 @@
+#pragma once
+
+#include "AC_CustomControl_Backend.h"
+
+#ifndef CUSTOMCONTROL_LQR_ENABLED
+    #define CUSTOMCONTROL_LQR_ENABLED AP_CUSTOMCONTROL_ENABLED
+#endif
+
+#if CUSTOMCONTROL_LQR_ENABLED
+
+class AC_CustomControl_LQR : public AC_CustomControl_Backend {
+public:
+    AC_CustomControl_LQR(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl*& att_control, AP_MotorsMulticopter*& motors, float dt);
+
+
+    Vector3f update(void) override;
+    void reset(void) override;
+    // user settable parameters
+    static const struct AP_Param::GroupInfo var_info[];
+
+protected:
+    // declare parameters here
+
+    float _dt;
+
+    float _integralX, _integralY, _integralZ;
+    float _kimax_LQR = 0.08;
+
+    AP_Float param1;
+    AP_Float param2;
+    AP_Float param3;
+};
+
+#endif