From f6900c0a66767dc21026e8609a33a3d9cfac6706 Mon Sep 17 00:00:00 2001 From: astik <41.astiksrivastava@gmail.com> Date: Sun, 18 Feb 2024 09:42:30 +0530 Subject: [PATCH] cleanup3 --- .../AC_CustomControl/AC_CustomControl.cpp | 12 +- libraries/AC_CustomControl/AC_CustomControl.h | 5 +- .../AC_CustomControl_Backend.h | 2 +- .../AC_CustomControl/AC_CustomControl_LQR.cpp | 114 ------------------ .../AC_CustomControl/AC_CustomControl_LQR.h | 34 ------ 5 files changed, 5 insertions(+), 162 deletions(-) delete mode 100644 libraries/AC_CustomControl/AC_CustomControl_LQR.cpp delete mode 100644 libraries/AC_CustomControl/AC_CustomControl_LQR.h diff --git a/libraries/AC_CustomControl/AC_CustomControl.cpp b/libraries/AC_CustomControl/AC_CustomControl.cpp index 80f5fd576b5807..e7791d3f7ad7d3 100644 --- a/libraries/AC_CustomControl/AC_CustomControl.cpp +++ b/libraries/AC_CustomControl/AC_CustomControl.cpp @@ -8,7 +8,6 @@ #include "AC_CustomControl_Backend.h" // #include "AC_CustomControl_Empty.h" #include "AC_CustomControl_PID.h" -#include "AC_CustomControl_LQR.h" #include // table of user settable parameters @@ -16,7 +15,7 @@ const AP_Param::GroupInfo AC_CustomControl::var_info[] = { // @Param: _TYPE // @DisplayName: Custom control type // @Description: Custom control type to be used - // @Values: 0:None, 1:Empty, 2:PID, 3:LQR-integral + // @Values: 0:None, 1:Empty, 2:PID // @RebootRequired: True // @User: Advanced AP_GROUPINFO_FLAGS("_TYPE", 1, AC_CustomControl, _controller_type, 0, AP_PARAM_FLAG_ENABLE), @@ -34,9 +33,6 @@ 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 }; @@ -66,10 +62,6 @@ 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; } @@ -201,4 +193,4 @@ void AC_CustomControl::set_notch_sample_rate(float sample_rate) #endif } -#endif +#endif \ No newline at end of file diff --git a/libraries/AC_CustomControl/AC_CustomControl.h b/libraries/AC_CustomControl/AC_CustomControl.h index d3cb42aafed9d0..126bb058a77abe 100644 --- a/libraries/AC_CustomControl/AC_CustomControl.h +++ b/libraries/AC_CustomControl/AC_CustomControl.h @@ -13,7 +13,7 @@ #if AP_CUSTOMCONTROL_ENABLED #ifndef CUSTOMCONTROL_MAX_TYPES -#define CUSTOMCONTROL_MAX_TYPES 3 +#define CUSTOMCONTROL_MAX_TYPES 2 #endif class AC_CustomControl_Backend; @@ -48,7 +48,6 @@ class AC_CustomControl { CONT_NONE = 0, CONT_EMPTY = 1, CONT_PID = 2, - CONT_LQR = 3, }; // controller that should be used enum class CustomControlOption { @@ -73,4 +72,4 @@ class AC_CustomControl { AC_CustomControl_Backend *_backend; }; -#endif +#endif \ No newline at end of file diff --git a/libraries/AC_CustomControl/AC_CustomControl_Backend.h b/libraries/AC_CustomControl/AC_CustomControl_Backend.h index 5355ef9cbc18d6..62b5a86a13aeab 100644 --- a/libraries/AC_CustomControl/AC_CustomControl_Backend.h +++ b/libraries/AC_CustomControl/AC_CustomControl_Backend.h @@ -34,4 +34,4 @@ class AC_CustomControl_Backend AC_CustomControl& _frontend; }; -#endif +#endif \ No newline at end of file diff --git a/libraries/AC_CustomControl/AC_CustomControl_LQR.cpp b/libraries/AC_CustomControl/AC_CustomControl_LQR.cpp deleted file mode 100644 index dce1b357bdfdc9..00000000000000 --- a/libraries/AC_CustomControl/AC_CustomControl_LQR.cpp +++ /dev/null @@ -1,114 +0,0 @@ -#include "AC_CustomControl_LQR.h" - -#if CUSTOMCONTROL_LQR_ENABLED - -#include - -// 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.03*attitude_error_angle.x*_dt; - _integralY += 0.08*attitude_error_angle.y*_dt; - _integralZ += 0.005*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.375*attitude_error_angle.x - 0.09*gyro_latest.x; - - motor_out.y = -_integralY - 0.375*attitude_error_angle.y - 0.125*gyro_latest.y; - - motor_out.z = -_integralZ - 0.40*attitude_error_angle.z - 0.09*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 diff --git a/libraries/AC_CustomControl/AC_CustomControl_LQR.h b/libraries/AC_CustomControl/AC_CustomControl_LQR.h deleted file mode 100644 index 6a44c1e83473c0..00000000000000 --- a/libraries/AC_CustomControl/AC_CustomControl_LQR.h +++ /dev/null @@ -1,34 +0,0 @@ -#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.1; - - AP_Float param1; - AP_Float param2; - AP_Float param3; -}; - -#endif