Luxfloat integrator attenuation

Integrator is disabled when angle error > 30 dps.
Yaw integrator is attenuated proportional to stick position.

src/main/flight/pid.c

@@ -47,6 +47,9 @@
 
 #include "config/runtime_config.h"
 
+//! Integrator is disabled when rate error exceeds this limit
+#define LUXFLOAT_INTEGRATOR_DISABLE_LIMIT_DPS (30.0f)
+
 extern uint16_t cycleTime;
 extern uint8_t motorCount;
 extern float dT;
@@ -58,7 +61,7 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
 #endif
 
 // PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction
-uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
+uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3], Iweigth[3] = {0};
 
 static int32_t errorGyroI[3] = { 0, 0, 0 };
 static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
@@ -173,7 +176,10 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100;
 
         // -----calculate I component.
-        errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
+        if (fabsf(RateError) < LUXFLOAT_INTEGRATOR_DISABLE_LIMIT_DPS)
+        {
+            errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10 * Iweigth[axis] / 100, -250.0f, 250.0f);
+        }
 
         // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
         // I coefficient (I8) moved before integration to make limiting independent from PID settings

src/main/mw.c

@@ -112,7 +112,7 @@ uint8_t motorControlEnable = false;
 int16_t telemTemperature1;      // gyro sensor temperature
 static uint32_t disarmAt;     // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
 
-extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
+extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3], Iweigth[3];
 
 static bool isRXDataNew;
 
@@ -204,7 +204,7 @@ void filterRc(void){
 void annexCode(void)
 {
     int32_t tmp, tmp2;
-    int32_t axis, prop1 = 0, prop2, yawWeigth;
+    int32_t axis, prop1 = 0, prop2, yawPDWeigth, yawIWeigth;
 
     static uint32_t vbatLastServiced = 0;
     static uint32_t ibatLastServiced = 0;
@@ -221,29 +221,34 @@ void annexCode(void)
 
     // YAW dynamic PID adjustment
     if (rcData[THROTTLE] < currentControlRateProfile->tpa_yaw_breakpoint) {
-        yawWeigth = 100;
+        yawPDWeigth = 100;
     } else {
         if (rcData[THROTTLE] < 2000) {
             if (currentControlRateProfile->tpa_yaw_rate >= 100)
             {
-                yawWeigth = 100 + (uint16_t)(currentControlRateProfile->tpa_yaw_rate - 100) * (rcData[THROTTLE] - currentControlRateProfile->tpa_yaw_breakpoint) / (2000 - currentControlRateProfile->tpa_yaw_breakpoint);
+                yawPDWeigth = 100 + (uint16_t)(currentControlRateProfile->tpa_yaw_rate - 100) * (rcData[THROTTLE] - currentControlRateProfile->tpa_yaw_breakpoint) / (2000 - currentControlRateProfile->tpa_yaw_breakpoint);
             }
             else
             {
-                yawWeigth = 100 - (uint16_t)currentControlRateProfile->tpa_yaw_rate * (rcData[THROTTLE] - currentControlRateProfile->tpa_yaw_breakpoint) / (2000 - currentControlRateProfile->tpa_yaw_breakpoint);
+                yawPDWeigth = 100 - (uint16_t)currentControlRateProfile->tpa_yaw_rate * (rcData[THROTTLE] - currentControlRateProfile->tpa_yaw_breakpoint) / (2000 - currentControlRateProfile->tpa_yaw_breakpoint);
             }
         } else {
             if (currentControlRateProfile->tpa_yaw_rate >= 100)
             {
-                yawWeigth = currentControlRateProfile->tpa_yaw_rate;
+                yawPDWeigth = currentControlRateProfile->tpa_yaw_rate;
             }
             else
             {
-                yawWeigth = 100 - currentControlRateProfile->tpa_yaw_rate;
+                yawPDWeigth = 100 - currentControlRateProfile->tpa_yaw_rate;
             }
         }
     }
 
+    // YAW dynamic integral adjustment
+    // Attenuate integral term proportional to the stick position, 100% attenuation starting at full stick position
+    int32_t yawCommandFromCenter = ABS(rcData[YAW] - 1500);
+    yawIWeigth = MAX(0, (100 - (yawCommandFromCenter * 100 / 500)));
+
     for (axis = 0; axis < 3; axis++) {
         tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500);
         if (axis == ROLL || axis == PITCH) {
@@ -278,10 +283,12 @@ void annexCode(void)
 
         // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids
         if (axis == YAW) {
-            PIDweight[axis] = yawWeigth;
+            PIDweight[axis] = yawPDWeigth;
+            Iweigth[axis] = yawIWeigth;
         }
         else {
             PIDweight[axis] = prop2;
+            Iweigth[axis] = 100;
         }
 
         if (rcData[axis] < masterConfig.rxConfig.midrc)
@@ -915,3 +922,25 @@ void loop(void)
     }
 #endif
 }
+
+bool isRcAxisWithinDeadband(int32_t axis)
+{
+    int32_t tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500);
+    bool ret = false;
+    if (axis == ROLL || axis == PITCH)
+    {
+        if (tmp <= currentProfile->rcControlsConfig.deadband)
+        {
+            ret = true;
+        }
+    }
+    else
+    {
+        if (tmp <= currentProfile->rcControlsConfig.yaw_deadband)
+        {
+            ret = true;
+        }
+    }
+
+    return ret;
+}

src/main/mw.h

@@ -24,3 +24,5 @@ void handleInflightCalibrationStickPosition();
 
 void mwDisarm(void);
 void mwArm(void);
+
+bool isRcAxisWithinDeadband(int32_t axis);