Some fixes.

betaflight · Jun 8, 2018 · db3c0d1 · db3c0d1
1 parent fefedbd
commit db3c0d1
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 10 deletions.
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
@@ -812,12 +812,11 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
         float itermErrorRate = 0.0f;
 
 #if defined(USE_ITERM_RELAX)
-        float stickSetpoint = currentPidSetpoint;
         if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) {
-            const float gyroTarget = pt1FilterApply(&windupLpf[axis], stickSetpoint);
-            const float gyroHpf = fabsf(stickSetpoint - gyroTarget);
+            const float gyroTarget = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
+            const float gyroHpf = fabsf(currentPidSetpoint - gyroTarget);
             if (itermRelaxType == ITERM_RELAX_SETPOINT) {
-                itermErrorRate = (1 - MIN(1, fabsf(gyroHpf) / 60.0f)) * (stickSetpoint - gyroRate);
+                itermErrorRate = (1 - MIN(1, fabsf(gyroHpf) / 60.0f)) * (currentPidSetpoint - gyroRate);
             }
             const float tolerance = gyroHpf * 1.0f;
             if (axis == FD_ROLL) {
@@ -831,9 +830,9 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
                 const float gmin = gyroTarget - tolerance;
 
                 if (gyroRate >= gmin && gyroRate <= gmax) {
-                    itermErrorRate = 0;
+                     itermErrorRate = 0;
                 } else {
-                    itermErrorRate = (gyroRate > gmax ? gmax : gmin ) - gyroRate;
+                     itermErrorRate = (gyroRate > gmax ? gmax : gmin ) - gyroRate;
                 }
             }
 

diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
@@ -76,15 +76,13 @@ typedef struct pid8_s {
     uint8_t D;
 } pid8_t;
 
-typedef enum 
-{
+typedef enum {
     ITERM_RELAX_OFF,
     ITERM_RELAX_RP,
     ITERM_RELAX_RPY
 } itermRelax_e;
 
-typedef enum 
-{
+typedef enum {
     ITERM_RELAX_GYRO,
     ITERM_RELAX_SETPOINT
 } itermRelaxType_e;