Reduce I-term decay if no stick input

rotorflight · Apr 24, 2024 · 400bc06 · 400bc06
1 parent ef7d769
commit 400bc06
Show file tree

Hide file tree

Showing 6 changed files with 23 additions and 7 deletions.
diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
@@ -1111,6 +1111,8 @@ const clivalue_t valueTable[] = {
     { "offset_bleed_limit_curve",   VAR_UINT8 | PROFILE_VALUE | MODE_ARRAY, .config.array.length = LOOKUP_CURVE_POINTS, PG_PID_PROFILE, offsetof(pidProfile_t, offset_bleed_limit_curve) },
     { "offset_charge_curve",        VAR_UINT8 | PROFILE_VALUE | MODE_ARRAY, .config.array.length = LOOKUP_CURVE_POINTS, PG_PID_PROFILE, offsetof(pidProfile_t, offset_charge_curve) },
 
+    { "decay_cutoff_level",         VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, decay_cutoff_level) },
+
     { "iterm_relax_type",           VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ITERM_RELAX_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_type) },
     { "iterm_relax_level",          VAR_UINT8  | PROFILE_VALUE | MODE_ARRAY, .config.array.length = 3, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_level) },
     { "iterm_relax_cutoff",         VAR_UINT8  | PROFILE_VALUE | MODE_ARRAY, .config.array.length = 3, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_cutoff) },

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
@@ -944,16 +944,22 @@ static void pidApplyCyclicMode3(uint8_t axis, const pidProfile_t * pidProfile)
     // Apply error decay
     float decayRate, decayLimit, errorDecay;
 
-    if (isAirborne()) {
+    // if (isAirborne())
+    {
       decayRate  = pidTableLookup(curve, pidProfile->error_decay_rate_curve, LOOKUP_CURVE_POINTS) * 0.04f;
       decayLimit = pidTableLookup(curve, pidProfile->error_decay_limit_curve, LOOKUP_CURVE_POINTS);
-      errorDecay = limitf(pid.data[axis].axisError * decayRate, decayLimit);
-    }
-    else {
-      decayRate  = pid.errorDecayRateGround / pid.dT;
-      decayLimit = 0;
-      errorDecay = pid.data[axis].axisError * decayRate;
+      // Reduce decay rate if no stick movement
+      float activity = fmaxf(getStickActivity(FD_ROLL), getStickActivity(FD_PITCH));
+      // Full decay >10% deflection
+      float multiplier = fminf(activity / (pidProfile->decay_cutoff_level / 100.0f), 1);
+      // Total decay
+      errorDecay = limitf(pid.data[axis].axisError * decayRate * multiplier, decayLimit);
     }
+    // else {
+    //   decayRate  = pid.errorDecayRateGround / pid.dT;
+    //   decayLimit = 0;
+    //   errorDecay = pid.data[axis].axisError * decayRate;
+    // }
 
     pid.data[axis].axisError -= errorDecay * pid.dT;
 

diff --git a/src/main/flight/setpoint.c b/src/main/flight/setpoint.c
@@ -83,6 +83,10 @@ float getDeflection(int axis)
     return sp.deflection[axis];
 }
 
+float getStickActivity(int axis)
+{
+    return sqrtf(sp.maximum[axis]);
+}
 
 static float setpointAutoSmoothingCutoff(float frameTimeUs)
 {

diff --git a/src/main/flight/setpoint.h b/src/main/flight/setpoint.h
@@ -27,6 +27,7 @@
 
 float getSetpoint(int axis);
 float getDeflection(int axis);
+float getStickActivity(int axis);
 
 void setpointInit(void);
 void setpointInitProfile(void);

diff --git a/src/main/pg/pid.c b/src/main/pg/pid.c
@@ -64,6 +64,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .offset_bleed_rate_curve = { 0,0,0,0,0,0,2,4,30,250,250,250,250,250,250,250 },
         .offset_bleed_limit_curve = { 0,0,0,0,0,0,15,40,100,150,200,250,250,250,250,250 },
         .offset_charge_curve = { 0,100,100,100,100,100,95,90,82,76,72,68,65,62,60,58 },
+        .decay_cutoff_level = 10,
         .error_rotation = true,
         .iterm_relax_type = ITERM_RELAX_RPY,
         .iterm_relax_level = { 40, 40, 40 },

diff --git a/src/main/pg/pid.h b/src/main/pg/pid.h
@@ -141,6 +141,8 @@ typedef struct pidProfile_s {
     uint8_t             offset_bleed_limit_curve[LOOKUP_CURVE_POINTS];
     uint8_t             offset_charge_curve[LOOKUP_CURVE_POINTS];
 
+    uint8_t             decay_cutoff_level;
+
     uint8_t             error_rotation;
 
     uint8_t             iterm_relax_type;