Airmode Plus with Insane Acrobility Factor

src/main/config/config.c

@@ -179,6 +179,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->gyro_soft_lpf = 1;   // filtering ON by default
     pidProfile->dterm_cut_hz = 0;
     pidProfile->delta_from_gyro_error = 0;
+    pidProfile->airModeInsaneAcrobilityFactor = 0;
 
     pidProfile->P_f[ROLL] = 1.5f;     // new PID with preliminary defaults test carefully
     pidProfile->I_f[ROLL] = 0.3f;

src/main/flight/pid.c

@@ -22,6 +22,7 @@
 #include <platform.h>
 
 #include "build_config.h"
+#include "debug.h"
 
 #include "common/axis.h"
 #include "common/maths.h"
@@ -96,10 +97,48 @@ void setPidDeltaSamples(uint8_t filter) {
     }
 }
 
+void airModePlus(airModePlus_t *axisState, int axis, pidProfile_t *pidProfile, float referenceTerm) {
+    float rcCommandReflection = (float)rcCommand[axis] / 500.0f;
+
+    //Ki scaler
+    axisState->iTermScaler = constrainf(1.0f - (1.5f * ABS(rcCommandReflection)), 0.0f, 1.0f);
+
+    //dynamic Ki handler
+    if (axisState->isCurrentlyAtZero) {
+        if (axisState->previousReferenceIsPositive ^ IS_POSITIVE(referenceTerm)) {
+            axisState->isCurrentlyAtZero = false;
+        } else {
+            axisState->iTermScaler = 0;
+            errorGyroIf[axis] = 0;
+            errorGyroI[axis] = 0;
+        }
+    }
+
+    if (!axisState->iTermScaler) {
+        if (!axisState->isCurrentlyAtZero) {
+            if (IS_POSITIVE(referenceTerm)) {
+                axisState->previousReferenceIsPositive = true;
+            } else {
+                axisState->previousReferenceIsPositive = false;
+            }
+        } else {
+            axisState->isCurrentlyAtZero = true;
+        }
+    }
+
+    if (axis != YAW && pidProfile->airModeInsaneAcrobilityFactor) {
+        axisState->wowFactor = 1.0f - (ABS(rcCommandReflection) * ((float)pidProfile->airModeInsaneAcrobilityFactor / 100.0f)); //0-1f
+        axisState->factor = (axisState->wowFactor * rcCommandReflection) * 1000;
+    } else {
+        axisState->wowFactor = 1;
+        axisState->factor = 0;
+    }
+}
+
 const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
 
 static filterStatePt1_t DTermState[3];
-//static filterStatePt1_t yawPTermState;
+static airModePlus_t airModePlusAxisState[3];
 
 static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
@@ -173,7 +212,12 @@ 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] + 0.5f * (lastError[axis] + RateError) * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
+        errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
+
+        if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
+            airModePlus(&airModePlusAxisState[axis], axis, pidProfile, PTerm);
+            errorGyroIf[axis] *= airModePlusAxisState[axis].iTermScaler;
+        }
 
         if (allowITermShrinkOnly || motorLimitReached) {
             if (ABS(errorGyroIf[axis]) < ABS(previousErrorGyroIf[axis])) {
@@ -190,12 +234,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         ITerm = errorGyroIf[axis];
 
         //-----calculate D-term
-        if (!pidProfile->delta_from_gyro_error) {
+        if (pidProfile->delta_from_gyro_error || axis == YAW) {
-            delta = RateError - lastError[axis];
-            lastError[axis] = RateError;
-        } else {
             delta = -(gyroRate - lastGyroRate[axis]);  // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
             lastGyroRate[axis] = gyroRate;
+        } else {
+            delta = RateError - lastError[axis];
+            lastError[axis] = RateError;
         }
 
         // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
@@ -221,6 +265,10 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         // -----calculate total PID output
         axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
 
+        if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
+            axisPID[axis] = lrintf(airModePlusAxisState[axis].factor + airModePlusAxisState[axis].wowFactor * axisPID[axis]);
+        }
+
 #ifdef GTUNE
         if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
             calculate_Gtune(axis);
@@ -310,12 +358,18 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
         // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
         // Time correction (to avoid different I scaling for different builds based on average cycle time)
         // is normalized to cycle time = 2048.
-        errorGyroI[axis] = errorGyroI[axis] + ((((lastError[axis] + RateError) / 2) * (uint16_t)targetLooptime) >> 11) * pidProfile->I8[axis];
+        errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetLooptime) >> 11) * pidProfile->I8[axis];
 
         // 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
         errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);
 
+
+        if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
+            airModePlus(&airModePlusAxisState[axis], axis, pidProfile, (float) PTerm);
+            errorGyroI[axis] *= airModePlusAxisState[axis].iTermScaler;
+        }
+
         ITerm = errorGyroI[axis] >> 13;
 
         if (allowITermShrinkOnly || motorLimitReached) {
@@ -329,12 +383,12 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
         }
 
         //-----calculate D-term
-        if (!pidProfile->delta_from_gyro_error) {   // quick and dirty solution for testing
+        if (pidProfile->delta_from_gyro_error || axis == YAW) {   // quick and dirty solution for testing
-            delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
-            lastError[axis] = RateError;
-        } else {
             delta = -(gyroRate - lastGyroRate[axis]);  // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
             lastGyroRate[axis] = gyroRate;
+        } else {
+            delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
+            lastError[axis] = RateError;
         }
 
         // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
@@ -359,6 +413,10 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
         // -----calculate total PID output
         axisPID[axis] = PTerm + ITerm + DTerm;
 
+        if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
+            axisPID[axis] = lrintf(airModePlusAxisState[axis].factor + airModePlusAxisState[axis].wowFactor * axisPID[axis]);
+        }
+
 #ifdef GTUNE
         if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
              calculate_Gtune(axis);

src/main/flight/pid.h

@@ -20,6 +20,7 @@
 #define GYRO_I_MAX 256                      // Gyro I limiter
 #define YAW_P_LIMIT_MIN 100                 // Maximum value for yaw P limiter
 #define YAW_P_LIMIT_MAX 500                 // Maximum value for yaw P limiter
+#define IS_POSITIVE(x) ((x > 0) ? true : false)
 
 typedef enum {
     PIDROLL,
@@ -57,7 +58,7 @@ typedef struct pidProfile_s {
     float H_level;
     uint8_t H_sensitivity;
 
-    uint16_t yaw_p_limit;                   // set P term limit (fixed value was 300)
+    uint16_t airModeInsaneAcrobilityFactor; // Air mode acrobility factor
     uint8_t dterm_cut_hz;                   // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
     uint8_t delta_from_gyro_error;          // Used for filering Pterm noise on noisy frames
     uint8_t gyro_soft_lpf;                  // Gyro FIR filter
@@ -71,6 +72,14 @@ typedef struct pidProfile_s {
 #endif
 } pidProfile_t;
 
+typedef struct airModePlus {
+    float factor;
+    float wowFactor;
+    float iTermScaler;
+    bool isCurrentlyAtZero;
+    bool previousReferenceIsPositive;
+} airModePlus_t;
+
 extern int16_t axisPID[XYZ_AXIS_COUNT];
 extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
 

src/main/io/serial_cli.c

@@ -658,6 +658,7 @@ const clivalue_t valueTable[] = {
     { "gyro_soft_lpf",              VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.gyro_soft_lpf, .config.lookup = { TABLE_OFF_ON } },
     { "dterm_cut_hz",               VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, .config.minmax = {0, 200 } },
     { "delta_from_gyro_error",      VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.delta_from_gyro_error, .config.minmax = {0, 1} },
+    { "insane_acrobility_factor",   VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.airModeInsaneAcrobilityFactor, .config.minmax = {0, 30 } },
 
 #ifdef BLACKBOX
     { "blackbox_rate_num",          VAR_UINT8  | MASTER_VALUE,  &masterConfig.blackbox_rate_num, .config.minmax = { 1,  32 } },