Use error_decay_time_cyclic in PID mode 3

rotorflight · Apr 27, 2024 · 1f1e474 · 1f1e474
1 parent eafd3e3
commit 1f1e474
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 22 deletions.
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
@@ -179,13 +179,13 @@ void INIT_CODE pidInitProfile(const pidProfile_t *pidProfile)
         pid.offsetLimit[i] = pidProfile->offset_limit[i];
 
     // Exponential error decay rates
-    pid.errorDecayRateGround = (pidProfile->error_decay_time_ground) ? (10 * pid.dT / pidProfile->error_decay_time_ground) : 0;
-    pid.errorDecayRateCyclic = (pidProfile->error_decay_time_cyclic) ? (10 * pid.dT / pidProfile->error_decay_time_cyclic) : 0;
-    pid.errorDecayRateYaw    = (pidProfile->error_decay_time_yaw)    ? (10 * pid.dT / pidProfile->error_decay_time_yaw) : 0;
+    pid.errorDecayRateGround = (pidProfile->error_decay_time_ground) ? (10.0f / pidProfile->error_decay_time_ground) : 0;
+    pid.errorDecayRateCyclic = (pidProfile->error_decay_time_cyclic) ? (10.0f / pidProfile->error_decay_time_cyclic) : 0;
+    pid.errorDecayRateYaw    = (pidProfile->error_decay_time_yaw)    ? (10.0f / pidProfile->error_decay_time_yaw) : 0;
 
     // Max decay speeds in degs/s (linear decay)
-    pid.errorDecayLimitCyclic = (pidProfile->error_decay_limit_cyclic) ? (pid.dT * pidProfile->error_decay_limit_cyclic) : 1e6;
-    pid.errorDecayLimitYaw    = (pidProfile->error_decay_limit_yaw)    ? (pid.dT * pidProfile->error_decay_limit_yaw) : 1e6;
+    pid.errorDecayLimitCyclic = (pidProfile->error_decay_limit_cyclic) ? (pidProfile->error_decay_limit_cyclic) : 250;
+    pid.errorDecayLimitYaw    = (pidProfile->error_decay_limit_yaw)    ? (pidProfile->error_decay_limit_yaw) : 250;
 
     // Error Rotation enable
     pid.errorRotation = pidProfile->error_rotation;
@@ -769,9 +769,9 @@ static void pidApplyCyclicMode2(uint8_t axis)
     pid.data[axis].I = pid.coef[axis].Ki * pid.data[axis].axisError;
 
     // Apply I-term error decay
-    pid.data[axis].axisError -= isAirborne() ?
-      limitf(pid.data[axis].axisError * pid.errorDecayRateCyclic, pid.errorDecayLimitCyclic):
-      pid.data[axis].axisError * pid.errorDecayRateGround;
+    pid.data[axis].axisError -= pid.dT * limitf(pid.data[axis].axisError *
+      (isAirborne() ? pid.errorDecayRateCyclic : pid.errorDecayRateGround),
+      pid.errorDecayLimitCyclic);
 
 
   //// Feedforward
@@ -847,9 +847,9 @@ static void pidApplyYawMode2(void)
     pid.data[axis].I = pid.coef[axis].Ki * pid.data[axis].axisError;
 
     // Apply I-term error decay
-    pid.data[axis].axisError -= isSpooledUp() ?
-      limitf(pid.data[axis].axisError * pid.errorDecayRateYaw, pid.errorDecayLimitYaw):
-      pid.data[axis].axisError * pid.errorDecayRateGround;
+    pid.data[axis].axisError -= pid.dT * limitf(pid.data[axis].axisError *
+      (isSpooledUp() ? pid.errorDecayRateYaw : pid.errorDecayRateGround),
+      pid.errorDecayLimitYaw);
 
 
   //// Feedforward
@@ -945,16 +945,16 @@ static void pidApplyCyclicMode3(uint8_t axis, const pidProfile_t * pidProfile)
     float decayRate, decayLimit, errorDecay;
 
     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);
+      decayRate  = pid.errorDecayRateCyclic * pidTableLookup(curve, pidProfile->error_decay_rate_curve, LOOKUP_CURVE_POINTS) * 0.08f;
+      decayLimit = pid.errorDecayLimitCyclic * pidTableLookup(curve, pidProfile->error_decay_limit_curve, LOOKUP_CURVE_POINTS) * 0.08f;
     }
     else {
-      decayRate  = pid.errorDecayRateGround / pid.dT;
-      decayLimit = 0;
-      errorDecay = pid.data[axis].axisError * decayRate;
+      decayRate  = pid.errorDecayRateGround;
+      decayLimit = pid.errorDecayLimitCyclic;
     }
 
+    errorDecay = limitf(pid.data[axis].axisError * decayRate, decayLimit);
+
     pid.data[axis].axisError -= errorDecay * pid.dT;
 
     DEBUG_AXIS(ERROR_DECAY, axis, 0, decayRate * 100);
@@ -1079,9 +1079,9 @@ static void pidApplyYawMode3(void)
     pid.data[axis].I = pid.coef[axis].Ki * pid.data[axis].axisError;
 
     // Apply error decay
-    pid.data[axis].axisError -= isSpooledUp() ?
-      limitf(pid.data[axis].axisError * pid.errorDecayRateYaw, pid.errorDecayLimitYaw):
-      pid.data[axis].axisError * pid.errorDecayRateGround;
+    pid.data[axis].axisError -= pid.dT * limitf(pid.data[axis].axisError *
+      (isSpooledUp() ? pid.errorDecayRateYaw : pid.errorDecayRateGround),
+      pid.errorDecayLimitYaw);
 
 
   //// Feedforward

diff --git a/src/main/pg/pid.c b/src/main/pg/pid.c
@@ -53,9 +53,9 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .dterm_mode = 0,
         .dterm_mode_yaw = 0,
         .error_decay_time_ground = 25,
-        .error_decay_time_cyclic = 18,
+        .error_decay_time_cyclic = 20,
         .error_decay_time_yaw = 0,
-        .error_decay_limit_cyclic = 20,
+        .error_decay_limit_cyclic = 12,
         .error_decay_limit_yaw = 0,
         .error_decay_rate_curve = { 12,13,14,15,17,20,23,28,36,49,78,187,250,250,250,250 },
         .error_decay_limit_curve = { 12,12,12,12,12,12,12,12,12,12,12,12,13,14,15,16 },