Merge pull request #5963 from joelucid/iterm_relax

ITerm relax feature

src/main/build/debug.c

@@ -69,4 +69,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "USB",
     "SMARTAUDIO",
     "RTH",
+    "ITERM_RELAX"
 };

src/main/build/debug.h

@@ -87,6 +87,7 @@ typedef enum {
     DEBUG_USB,
     DEBUG_SMARTAUDIO,
     DEBUG_RTH,
+    DEBUG_ITERM_RELAX,
     DEBUG_COUNT
 } debugType_e;
 

src/main/flight/pid.c

@@ -139,7 +139,10 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .throttle_boost = 0,
         .throttle_boost_cutoff = 15,
         .iterm_rotation = false,
-        .smart_feedforward = false         
+        .smart_feedforward = false,
+        .iterm_relax = ITERM_RELAX_OFF,
+        .iterm_relax_cutoff_low = 3,
+        .iterm_relax_cutoff_high = 15,
     );
 }
 
@@ -195,6 +198,10 @@ static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpass2ApplyFn;
 static FAST_RAM_ZERO_INIT pt1Filter_t dtermLowpass2[2];
 static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn;
 static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass;
+static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[3][2];
+static FAST_RAM_ZERO_INIT itermRelax_e itermRelax;
+static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffLow;
+static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffHigh;
 
 void pidInitFilters(const pidProfile_t *pidProfile)
 {
@@ -271,6 +278,11 @@ void pidInitFilters(const pidProfile_t *pidProfile)
     }
 
     pt1FilterInit(&throttleLpf, pt1FilterGain(pidProfile->throttle_boost_cutoff, dT));
+    if (itermRelax)     
+        for (int i = 0; i < 3; i++) {
+            pt1FilterInit(&windupLpf[i][0], pt1FilterGain(itermRelaxCutoffLow, dT));
+            pt1FilterInit(&windupLpf[i][1], pt1FilterGain(itermRelaxCutoffHigh, dT));
+        }
 }
 
 typedef struct pidCoefficient_s {
@@ -334,8 +346,11 @@ void pidInitConfig(const pidProfile_t *pidProfile)
     crashLimitYaw = pidProfile->crash_limit_yaw;
     itermLimit = pidProfile->itermLimit;
     throttleBoost = pidProfile->throttle_boost * 0.1f;
-    itermRotation = pidProfile->iterm_rotation == 1;
+    itermRotation = pidProfile->iterm_rotation;
-    smartFeedforward = pidProfile->smart_feedforward == 1;
+    smartFeedforward = pidProfile->smart_feedforward;
+    itermRelax = pidProfile->iterm_relax;
+    itermRelaxCutoffLow = pidProfile->iterm_relax_cutoff_low;
+    itermRelaxCutoffHigh = pidProfile->iterm_relax_cutoff_high;
 }
 
 void pidInit(const pidProfile_t *pidProfile)
@@ -601,8 +616,28 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
         }
 
         // -----calculate I component
+        float itermErrorRate;
+        if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) {
+            const float gyroTargetLow = pt1FilterApply(&windupLpf[axis][0], currentPidSetpoint);
+            const float gyroTargetHigh =  pt1FilterApply(&windupLpf[axis][1], currentPidSetpoint);
+            if (axis < FD_YAW) {
+                int itemOffset = (axis << 1);
+                DEBUG_SET(DEBUG_ITERM_RELAX, itemOffset++, gyroTargetHigh);
+                DEBUG_SET(DEBUG_ITERM_RELAX, itemOffset, gyroTargetLow);
+            }
+            const float gmax = MAX(gyroTargetHigh, gyroTargetLow);
+            const float gmin = MIN(gyroTargetHigh, gyroTargetLow);
+            if (gyroRate >= gmin && gyroRate <= gmax) {
+                itermErrorRate = 0.0f;
+            } else {
+                itermErrorRate = (gyroRate > gmax ? gmax : gmin ) - gyroRate;
+            }
+        } else {
+            itermErrorRate = errorRate;
+        }
+        
         const float ITerm = pidData[axis].I;
-        const float ITermNew = constrainf(ITerm + pidCoefficient[axis].Ki * errorRate * dynCi, -itermLimit, itermLimit);
+        const float ITermNew = constrainf(ITerm + pidCoefficient[axis].Ki * itermErrorRate * dynCi, -itermLimit, itermLimit);
         const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate);
         if (outputSaturated == false || ABS(ITermNew) < ABS(ITerm)) {
             // Only increase ITerm if output is not saturated

src/main/flight/pid.h

@@ -76,6 +76,15 @@ typedef struct pid8_s {
     uint8_t D;
 } pid8_t;
 
+typedef enum 
+{
+    ITERM_RELAX_OFF,
+    ITERM_RELAX_RP,
+    ITERM_RELAX_RPY
+} itermRelax_e;
+
+    
+
 typedef struct pidProfile_s {
     pid8_t  pid[PID_ITEM_COUNT];
 
@@ -116,6 +125,10 @@ typedef struct pidProfile_s {
     uint8_t throttle_boost_cutoff;          // Which cutoff frequency to use for throttle boost. higher cutoffs keep the boost on for shorter. Specified in hz.
     uint8_t  iterm_rotation;                // rotates iterm to translate world errors to local coordinate system
     uint8_t  smart_feedforward;             // takes only the larger of P and the D weight feed forward term if they have the same sign.
+    uint8_t iterm_relax_cutoff_low;        // Slowest setpoint response to prevent iterm accumulation
+    uint8_t iterm_relax_cutoff_high;       // Fastest setpoint response to prevent iterm accumulation
+    itermRelax_e iterm_relax;                   // Enable iterm suppression during stick input
+    
 } pidProfile_t;
 
 #ifndef USE_OSD_SLAVE

src/main/interface/settings.c

@@ -333,6 +333,11 @@ static const char * const lookupTableVideoSystem[] = {
 };
 #endif // USE_MAX7456
 
+static const char * const lookupTableItermRelax[] = {
+    "OFF", "RP", "RPY"
+};
+
+
 #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) }
 
 const lookupTableEntry_t lookupTables[] = {
@@ -407,6 +412,7 @@ const lookupTableEntry_t lookupTables[] = {
 #ifdef USE_MAX7456
     LOOKUP_TABLE_ENTRY(lookupTableVideoSystem),
 #endif // USE_MAX7456
+    LOOKUP_TABLE_ENTRY(lookupTableItermRelax),
 };
 
 #undef LOOKUP_TABLE_ENTRY
@@ -741,6 +747,9 @@ const clivalue_t valueTable[] = {
 
     { "iterm_rotation",             VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_rotation) },
     { "smart_feedforward",          VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, smart_feedforward) },
+    { "iterm_relax",                VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ITERM_RELAX }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax) },
+    { "iterm_relax_cutoff_low",     VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_cutoff_low) },
+    { "iterm_relax_cutoff_high",    VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_cutoff_high) },
     { "iterm_windup",               VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 30, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermWindupPointPercent) },
     { "iterm_limit",                VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermLimit) },
     { "pidsum_limit",               VAR_UINT16 | PROFILE_VALUE, .config.minmax = { PIDSUM_LIMIT_MIN, PIDSUM_LIMIT_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimit) },

src/main/interface/settings.h

@@ -97,6 +97,7 @@ typedef enum {
 #ifdef USE_MAX7456
     TABLE_VIDEO_SYSTEM,
 #endif // USE_MAX7456
+    TABLE_ITERM_RELAX,
     LOOKUP_TABLE_COUNT
 } lookupTableIndex_e;
 

src/test/unit/pid_unittest.cc

@@ -22,7 +22,7 @@
 
 #include "unittest_macros.h"
 #include "gtest/gtest.h"
-
+#include "build/debug.h"
 
 bool simulateMixerSaturated = false;
 float simulatedSetpointRate[3] = { 0,0,0 };
@@ -30,6 +30,9 @@ float simulatedRcDeflection[3] = { 0,0,0 };
 float simulatedThrottlePIDAttenuation = 1.0f;
 float simulatedMotorMixRange = 0.0f;
 
+int16_t debug[DEBUG16_VALUE_COUNT];
+uint8_t debugMode;
+
 extern "C" {
     #include "build/debug.h"
     #include "common/axis.h"