Blimp: Add relax TC

MichelleRos · Sep 6, 2024 · 3f50655 · 3f50655
1 parent c614bdb
commit 3f50655
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 15 deletions.
diff --git a/Blimp/Loiter.cpp b/Blimp/Loiter.cpp
@@ -838,11 +838,12 @@ const AP_Param::GroupInfo Loiter::var_info[] = {
 
     AP_SUBGROUPINFO(pid_lvl_pitch, "LVLPIT_", 20, Loiter, AC_PID),
     AP_SUBGROUPINFO(pid_lvl_roll, "LVLRLL_", 21, Loiter, AC_PID),
-    AP_GROUPINFO("LVLMAX", 22, Loiter, level_max, 0), //Max throttle output to level, use 0 to disable
-    AP_GROUPINFO("LVLDZ_CEN", 23, Loiter, level_dz_cen, 0.08), //0 to disable
-    AP_GROUPINFO("LVLDZ_K", 24, Loiter, level_dz_k, 150),
+    AP_GROUPINFO("LVLMAX", 22, Loiter, lvl_max, 0), //Max throttle output to level, use 0 to disable
+    AP_GROUPINFO("LVLDZ_CEN", 23, Loiter, lvl_dz_cen, 0.08), //0 to disable
+    AP_GROUPINFO("LVLDZ_K", 24, Loiter, lvl_dz_k, 150),
     AP_GROUPINFO("OPTIONS", 25, Loiter, options, 0), //1=Yaw rate,2=Yaw pos,4=Z rate
     AP_GROUPINFO("LVLSCSPD", 26, Loiter, lvl_scaler_spd, 0.5),
+    AP_GROUPINFO("LVLRELTC", 27, Loiter, lvl_relax_tc, 0.16), //0 to disable
 
     AP_GROUPEND
 };
@@ -1054,25 +1055,28 @@ void Loiter::run_vel(Vector3f& target_vel_ef, float& target_vel_yaw, Vector4b ax
 
 void Loiter::run_level_roll(float& out_right_com)
 {
-    if (is_zero(level_max)) {
+    if (is_zero(lvl_max)) {
         blimp.motors->right_out = out_right_com;
         return;
     }
     const float dt = blimp.scheduler.get_last_loop_time_s();
     const float roll = blimp.ahrs.get_roll();
 
     float lvl_scaler_n = 1;
-    if (level_dz_cen > 0) {
-        lvl_scaler_n = 1/(1+expf(-level_dz_k*(fabsf(roll)-level_dz_cen)));
+    if (lvl_dz_cen > 0) {
+        lvl_scaler_n = 1/(1+expf(-lvl_dz_k*(fabsf(roll)-lvl_dz_cen)));
     }
     lvl_scaler_rll = lvl_scaler_rll*lvl_scaler_spd + lvl_scaler_n*(1-lvl_scaler_spd);
 
     float level_roll = -blimp.loiter->pid_lvl_roll.update_all(0, roll*lvl_scaler_rll, dt, 0);
     if (!blimp.motors->armed()) {
         blimp.loiter->pid_lvl_roll.set_integrator(0);
     }
+    if(!is_zero(lvl_relax_tc)) {
+        pid_lvl_roll.relax_integrator(0.0, dt, lvl_relax_tc);
+    }
 
-    float out_right_lvl = constrain_float(level_roll, -level_max, level_max);
+    float out_right_lvl = constrain_float(level_roll, -lvl_max, lvl_max);
 
     blimp.motors->right_out = out_right_com + out_right_lvl;
 
@@ -1096,16 +1100,16 @@ void Loiter::run_level_roll(float& out_right_com)
 
 void Loiter::run_level_pitch(float& out_front_com)
 {
-    if (is_zero(level_max)) {
+    if (is_zero(lvl_max)) {
         blimp.motors->front_out = out_front_com;
         return;
     }
     const float dt = blimp.scheduler.get_last_loop_time_s();
     const float pitch = blimp.ahrs.get_pitch();
 
     float lvl_scaler_n = 1;
-    if (level_dz_cen > 0) {
-        lvl_scaler_n = 1/(1+expf(-level_dz_k*(fabsf(pitch)-level_dz_cen)));
+    if (lvl_dz_cen > 0) {
+        lvl_scaler_n = 1/(1+expf(-lvl_dz_k*(fabsf(pitch)-lvl_dz_cen)));
         // graph: y=1/(1+e^(−150(x−0.08))) where x is from 0 to 0.4
         // y=1/\left(1+e^{-150(x-0.08)}\right)
     }
@@ -1115,8 +1119,11 @@ void Loiter::run_level_pitch(float& out_front_com)
     if (!blimp.motors->armed()) {
         blimp.loiter->pid_lvl_roll.set_integrator(0);
     }
+    if(!is_zero(lvl_relax_tc)) {
+        pid_lvl_pitch.relax_integrator(0.0, dt, lvl_relax_tc);
+    }
 
-    float out_front_lvl = constrain_float(level_pitch, -level_max, level_max);
+    float out_front_lvl = constrain_float(level_pitch, -lvl_max, lvl_max);
 
     blimp.motors->front_out = out_front_com + out_front_lvl;
 

diff --git a/Blimp/Loiter.h b/Blimp/Loiter.h
@@ -80,10 +80,11 @@ class Loiter
 
     AC_PID pid_lvl_pitch{1, 0.2, 0, 0, 0.5, 0, 0, 0};
     AC_PID pid_lvl_roll{1, 0.2, 0, 0, 0.5, 0, 0, 0};
-    AP_Float level_max;
-    AP_Float level_dz_cen;
-    AP_Float level_dz_k;
+    AP_Float lvl_max;
+    AP_Float lvl_dz_cen;
+    AP_Float lvl_dz_k;
     AP_Float lvl_scaler_spd;
+    AP_Float lvl_relax_tc;
 
     AP_Float    max_vel_x;
     AP_Float    max_vel_y;

diff --git a/Blimp/mode_level.cpp b/Blimp/mode_level.cpp
@@ -5,7 +5,7 @@
 
 bool ModeLevel::init(bool ignore_checks)
 {
-    if (is_zero(blimp.loiter->level_max)){
+    if (is_zero(blimp.loiter->lvl_max)){
         GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "LOIT_LVLMAX is zero. Leveling is disabled.");
     }
     if (loiter->options & Loiter::LVL_EN_YAW_RATE){