corrected errors and tuned gains

Astik-2002 · Mar 11, 2024 · 0420f83 · 0420f83
1 parent c54072d
commit 0420f83
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 8 deletions.
diff --git a/libraries/AC_CustomControl/AC_CustomControl.cpp b/libraries/AC_CustomControl/AC_CustomControl.cpp
@@ -16,7 +16,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
+    // @Values: 0:None, 1:Empty, 2:PID, 3:LQR-integral
     // @RebootRequired: True
     // @User: Advanced
     AP_GROUPINFO_FLAGS("_TYPE", 1, AC_CustomControl, _controller_type, 0, AP_PARAM_FLAG_ENABLE),

diff --git a/libraries/AC_CustomControl/AC_CustomControl_LQR.cpp b/libraries/AC_CustomControl/AC_CustomControl_LQR.cpp
@@ -83,20 +83,20 @@ Vector3f AC_CustomControl_LQR::update(void)
 //       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;
+    _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.45*attitude_error_angle.x - 0.094*gyro_latest.x;
+    motor_out.x = -_integralX - 0.375*attitude_error_angle.x - 0.09*gyro_latest.x;
 
-    motor_out.y = -_integralY - 0.45*attitude_error_angle.y - 0.094*gyro_latest.y;
+    motor_out.y = -_integralY - 0.375*attitude_error_angle.y - 0.125*gyro_latest.y;
 
-    motor_out.z = -_integralZ - 0.45*attitude_error_angle.z - 0.094*gyro_latest.z;
+    motor_out.z = -_integralZ - 0.40*attitude_error_angle.z - 0.09*gyro_latest.z;
 
    return motor_out;
 

diff --git a/libraries/AC_CustomControl/AC_CustomControl_LQR.h b/libraries/AC_CustomControl/AC_CustomControl_LQR.h
@@ -24,7 +24,7 @@ class AC_CustomControl_LQR : public AC_CustomControl_Backend {
     float _dt;
 
     float _integralX, _integralY, _integralZ;
-    float _kimax_LQR = 0.08;
+    float _kimax_LQR = 0.1;
 
     AP_Float param1;
     AP_Float param2;