Merge pull request #6065 from mikeller/joelucid_relax2

Joelucid's overworked ITerm relax.

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;

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);

src/main/fc/fc_core.c

@@ -537,6 +537,14 @@ uint8_t calculateThrottlePercent(void)
     return ret;
 }
 
+static bool airmodeIsActivated;
+
+bool isAirmodeActivated() 
+{
+    return airmodeIsActivated;
+}
+
+
 
 /*
  * processRx called from taskUpdateRxMain
@@ -544,7 +552,6 @@ uint8_t calculateThrottlePercent(void)
 bool processRx(timeUs_t currentTimeUs)
 {
     static bool armedBeeperOn = false;
-    static bool airmodeIsActivated;
     static bool sharedPortTelemetryEnabled = false;
 
     if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs)) {

src/main/fc/fc_core.h

@@ -52,3 +52,4 @@ void taskMainPidLoop(timeUs_t currentTimeUs);
 bool isFlipOverAfterCrashMode(void);
 
 void runawayTakeoffTemporaryDisable(uint8_t disableFlag);
+bool isAirmodeActivated();

src/main/flight/pid.c

@@ -147,8 +147,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .iterm_rotation = false,
         .smart_feedforward = false,
         .iterm_relax = ITERM_RELAX_OFF,
-        .iterm_relax_cutoff_low = 3,
+        .iterm_relax_cutoff = 11,
-        .iterm_relax_cutoff_high = 30,
+        .iterm_relax_type = ITERM_RELAX_GYRO,
         .acro_trainer_angle_limit = 20,
         .acro_trainer_lookahead_ms = 50,
         .acro_trainer_debug_axis = FD_ROLL,
@@ -206,10 +206,10 @@ static FAST_RAM_ZERO_INIT pt1Filter_t dtermLowpass2[2];
 static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn;
 static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass;
 #if defined(USE_ITERM_RELAX)
-static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[XYZ_AXIS_COUNT][2];
+static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
 static FAST_RAM_ZERO_INIT uint8_t itermRelax;
-static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffLow;
+static FAST_RAM_ZERO_INIT uint8_t itermRelaxType;
-static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffHigh;
+static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoff;
 #endif
 
 #ifdef USE_RC_SMOOTHING_FILTER
@@ -300,8 +300,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
 #if defined(USE_ITERM_RELAX)
     if (itermRelax) {
         for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
-            pt1FilterInit(&windupLpf[i][0], pt1FilterGain(itermRelaxCutoffLow, dT));
+            pt1FilterInit(&windupLpf[i], pt1FilterGain(itermRelaxCutoff, dT));
-            pt1FilterInit(&windupLpf[i][1], pt1FilterGain(itermRelaxCutoffHigh, dT));
         }
     }
 #endif
@@ -427,8 +426,8 @@ void pidInitConfig(const pidProfile_t *pidProfile)
 #endif
 #if defined(USE_ITERM_RELAX)
     itermRelax = pidProfile->iterm_relax;
-    itermRelaxCutoffLow = pidProfile->iterm_relax_cutoff_low;
+    itermRelaxType = pidProfile->iterm_relax_type;
-    itermRelaxCutoffHigh = pidProfile->iterm_relax_cutoff_high;
+    itermRelaxCutoff = pidProfile->iterm_relax_cutoff;
 #endif
 
 #ifdef USE_ACRO_TRAINER
@@ -803,19 +802,68 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
         }
 #endif // USE_YAW_SPIN_RECOVERY
 
-#if defined(USE_ABSOLUTE_CONTROL)
-        // mix in a correction for accrued attitude error
-        float acCorrection = 0;
-        if (acGain > 0) {
-            acCorrection = constrainf(axisError[axis] * acGain, -acLimit, acLimit);
-            currentPidSetpoint += acCorrection;
-        }   
-#endif
-        
         // -----calculate error rate
         const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
-        float errorRate = currentPidSetpoint - gyroRate; // r - y
 
+#ifdef USE_ABSOLUTE_CONTROL
+        float acCorrection = 0;
+        float acErrorRate;
+#endif        
+        float itermErrorRate = 0.0f;
+
+#if defined(USE_ITERM_RELAX)
+        if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) {
+            const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
+            const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf);
+            if (itermRelaxType == ITERM_RELAX_SETPOINT && setpointHpf < 60) {
+                itermErrorRate = (1 - setpointHpf / 60.0f) * (currentPidSetpoint - gyroRate);
+            }
+            if (axis == FD_ROLL) {
+                DEBUG_SET(DEBUG_ITERM_RELAX, 0, lrintf(setpointLpf));
+                DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(setpointHpf));
+                DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(currentPidSetpoint));
+                DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf(axisError[axis] * 10));
+            }
+            if (itermRelaxType == ITERM_RELAX_GYRO ) {
+                itermErrorRate = fapplyDeadband(setpointLpf - gyroRate, setpointHpf);
+            }
+            
+#if defined(USE_ABSOLUTE_CONTROL)
+            const float gmaxac = setpointLpf + 2 * setpointHpf;
+            const float gminac = setpointLpf - 2 * setpointHpf;
+            if (gyroRate >= gminac && gyroRate <= gmaxac) {
+                float acErrorRate1 = gmaxac - gyroRate;
+                float acErrorRate2 = gminac - gyroRate;
+                if (acErrorRate1 * axisError[axis] < 0) {
+                    acErrorRate = acErrorRate1;
+                } else {
+                    acErrorRate = acErrorRate2;
+                }
+                if (fabsf(acErrorRate * dT) > fabsf(axisError[axis]) ) {
+                    acErrorRate = -axisError[axis] / dT;
+                }
+            } else {
+                acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate;
+            }
+#endif // USE_ABSOLUTE_CONTROL             
+        } else
+#endif // USE_ITERM_RELAX
+        {
+            itermErrorRate = currentPidSetpoint - gyroRate;
+#if defined(USE_ABSOLUTE_CONTROL)
+            acErrorRate = itermErrorRate;
+#endif // USE_ABSOLUTE_CONTROL
+        }
+        
+#if defined(USE_ABSOLUTE_CONTROL)
+        if (acGain > 0 && isAirmodeActivated()) {
+            axisError[axis] = constrainf(axisError[axis] + acErrorRate * dT, -acErrorLimit, acErrorLimit);
+            acCorrection = constrainf(axisError[axis] * acGain, -acLimit, acLimit);
+            currentPidSetpoint += acCorrection;
+            itermErrorRate += acCorrection;
+        }
+#endif
+        float errorRate = currentPidSetpoint - gyroRate; // r - y
         handleCrashRecovery(
             pidProfile->crash_recovery, angleTrim, axis, currentTimeUs, gyroRate,
             &currentPidSetpoint, &errorRate);
@@ -831,48 +879,6 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
         }
 
         // -----calculate I component
-        float itermErrorRate = 0.0f;
-#if defined(USE_ABSOLUTE_CONTROL)
-        float acErrorRate;
-#endif
-#if defined(USE_ITERM_RELAX)
-        if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) {
-            float gyroTargetLow = pt1FilterApply(&windupLpf[axis][0], currentPidSetpoint);
-            float gyroTargetHigh = pt1FilterApply(&windupLpf[axis][1], currentPidSetpoint);
-#if defined(USE_ABSOLUTE_CONTROL)
-            gyroTargetLow += acCorrection;
-            gyroTargetHigh += acCorrection;
-#endif
-            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 = (gyroRate > gmax ? gmax : gmin ) - gyroRate;
-            }
-#if defined(USE_ABSOLUTE_CONTROL)
-            else {
-                itermErrorRate = acCorrection;
-            }
-            acErrorRate = itermErrorRate - acCorrection;
-#endif
-        } else
-#endif // USE_ITERM_RELAX
-        {
-            itermErrorRate = errorRate;
-#if defined(USE_ABSOLUTE_CONTROL)
-            acErrorRate = errorRate;
-#endif
-        }
-        
-#if defined(USE_ABSOLUTE_CONTROL)
-        if (acGain > 0) {
-            axisError[axis] = constrainf(axisError[axis] + acErrorRate * dT, -acErrorLimit, acErrorLimit);
-        }
-#endif
 
         const float ITerm = pidData[axis].I;
         const float ITermNew = constrainf(ITerm + pidCoefficient[axis].Ki * itermErrorRate * dynCi, -itermLimit, itermLimit);

src/main/flight/pid.h

@@ -76,13 +76,17 @@ 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 {
+    ITERM_RELAX_GYRO,
+    ITERM_RELAX_SETPOINT
+} itermRelaxType_e;
+
 typedef struct pidProfile_s {
     uint16_t yaw_lowpass_hz;                // Additional yaw filter when yaw axis too noisy
     uint16_t dterm_lowpass_hz;              // Delta Filter in hz
@@ -124,8 +128,8 @@ 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_type;               // Specifies type of relax algorithm
-    uint8_t iterm_relax_cutoff_high;        // Fastest setpoint response to prevent iterm accumulation
+    uint8_t iterm_relax_cutoff;             // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint
     uint8_t iterm_relax;                    // Enable iterm suppression during stick input
     uint8_t acro_trainer_angle_limit;       // Acro trainer roll/pitch angle limit in degrees
     uint8_t acro_trainer_debug_axis;        // The axis for which record debugging values are captured 0=roll, 1=pitch

src/main/interface/settings.c

@@ -341,6 +341,9 @@ static const char * const lookupTableVideoSystem[] = {
 static const char * const lookupTableItermRelax[] = {
     "OFF", "RP", "RPY"
 };
+static const char * const lookupTableItermRelaxType[] = {
+    "GYRO", "SETPOINT"
+};
 #endif
 
 #ifdef USE_ACRO_TRAINER
@@ -441,6 +444,7 @@ const lookupTableEntry_t lookupTables[] = {
 #endif // USE_MAX7456
 #if defined(USE_ITERM_RELAX)
     LOOKUP_TABLE_ENTRY(lookupTableItermRelax),
+    LOOKUP_TABLE_ENTRY(lookupTableItermRelaxType),
 #endif
 #ifdef USE_ACRO_TRAINER
     LOOKUP_TABLE_ENTRY(lookupTableAcroTrainerDebug),
@@ -801,9 +805,9 @@ const clivalue_t valueTable[] = {
 #endif
 #if defined(USE_ITERM_RELAX)
     { "iterm_relax",                VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ITERM_RELAX }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax) },
+    { "iterm_relax_type",           VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ITERM_RELAX_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_type) },
+    { "iterm_relax_cutoff",     VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_cutoff) },
 #endif
-    { "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

@@ -100,6 +100,7 @@ typedef enum {
 #endif // USE_MAX7456
 #if defined(USE_ITERM_RELAX)
     TABLE_ITERM_RELAX,
+    TABLE_ITERM_RELAX_TYPE,
 #endif
 #ifdef USE_ACRO_TRAINER
     TABLE_ACRO_TRAINER_DEBUG,