diff --git a/src/main/common/maths.c b/src/main/common/maths.c
index e7813b132e..dd443ee80b 100644
--- a/src/main/common/maths.c
+++ b/src/main/common/maths.c
@@ -130,6 +130,18 @@ int32_t applyDeadband(int32_t value, int32_t deadband)
     return value;
 }
 
+float fapplyDeadband(float value, float deadband)
+{
+    if (fabsf(value) < deadband) {
+        value = 0;
+    } else if (value > 0) {
+        value -= deadband;
+    } else if (value < 0) {
+        value += deadband;
+    }
+    return value;
+}
+
 void devClear(stdev_t *dev)
 {
     dev->m_n = 0;
diff --git a/src/main/common/maths.h b/src/main/common/maths.h
index 300c18fae6..19842ebb40 100644
--- a/src/main/common/maths.h
+++ b/src/main/common/maths.h
@@ -91,6 +91,7 @@ typedef union {
 int gcd(int num, int denom);
 float powerf(float base, int exp);
 int32_t applyDeadband(int32_t value, int32_t deadband);
+float fapplyDeadband(float value, float deadband);
 
 void devClear(stdev_t *dev);
 void devPush(stdev_t *dev, float x);
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 913a2be853..48c501d524 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -815,8 +815,8 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
         if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) {
             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)) * (currentPidSetpoint - gyroRate);
+            if (itermRelaxType == ITERM_RELAX_SETPOINT && gyroHpf < 60) {
+                itermErrorRate = (1 - gyroHpf / 60.0f) * (currentPidSetpoint - gyroRate);
             }
             const float tolerance = gyroHpf * 1.0f;
             if (axis == FD_ROLL) {
@@ -826,14 +826,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
                 DEBUG_SET(DEBUG_ITERM_RELAX, 3, axisError[axis] * 10);
             }
             if (itermRelaxType == ITERM_RELAX_GYRO ) {
-                const float gmax = gyroTarget + tolerance;
-                const float gmin = gyroTarget - tolerance;
-                
-                if (gyroRate >= gmin && gyroRate <= gmax) {
-                     itermErrorRate = 0;
-                } else {
-                     itermErrorRate = (gyroRate > gmax ? gmax : gmin ) - gyroRate;
-                }
+                itermErrorRate = fapplyDeadband(gyroTarget - gyroRate, tolerance);
             }
             
 #if defined(USE_ABSOLUTE_CONTROL)
@@ -847,8 +840,8 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
                 } else {
                     acErrorRate = acErrorRate2;
                 }
-                if (fabsf(acErrorRate*dT) > fabsf(axisError[axis]) ) {
-                    acErrorRate = -axisError[axis]/dT;
+                if (fabsf(acErrorRate * dT) > fabsf(axisError[axis]) ) {
+                    acErrorRate = -axisError[axis] / dT;
                 }
             } else {
                 acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate;
@@ -857,9 +850,9 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
         } else
 #endif // USE_ITERM_RELAX
         {
-              itermErrorRate = currentPidSetpoint - gyroRate;
+            itermErrorRate = currentPidSetpoint - gyroRate;
 #if defined(USE_ABSOLUTE_CONTROL)
-              acErrorRate = itermErrorRate;
+            acErrorRate = itermErrorRate;
 #endif // USE_ABSOLUTE_CONTROL
         }