Merge pull request #6006 from iNavFlight/dzikuvx-iterm-relax-simplifi…

…cation

Drop gyro method of ITerm relax

src/main/build/debug.h

@@ -63,7 +63,6 @@ typedef enum {
     DEBUG_REM_FLIGHT_TIME,
     DEBUG_SMARTAUDIO,
     DEBUG_ACC,
-    DEBUG_ITERM_RELAX,
     DEBUG_ERPM,
     DEBUG_RPM_FILTER,
     DEBUG_RPM_FREQ,

src/main/common/maths.c

@@ -142,15 +142,6 @@ int32_t applyDeadband(int32_t value, int32_t deadband)
     return value;
 }
 
-float fapplyDeadbandf(float value, float deadband)
-{
-    if (fabsf(value) < deadband) {
-        return 0;
-    }
-
-    return value >= 0 ? value - deadband : value + deadband;
-}
-
 int constrain(int amt, int low, int high)
 {
     if (amt < low)

src/main/common/maths.h

@@ -130,7 +130,6 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu
 
 int gcd(int num, int denom);
 int32_t applyDeadband(int32_t value, int32_t deadband);
-float fapplyDeadbandf(float value, float deadband);
 
 int constrain(int amt, int low, int high);
 float constrainf(float amt, float low, float high);

src/main/fc/settings.yaml

@@ -82,7 +82,7 @@ tables:
   - name: debug_modes
     values: ["NONE", "GYRO", "AGL", "FLOW_RAW",
       "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
-      "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ITERM_RELAX",
+      "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
       "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
       "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574"]
   - name: async_mode
@@ -124,9 +124,6 @@ tables:
   - name: iterm_relax
     values: ["OFF", "RP", "RPY"]
     enum: itermRelax_e
-  - name: iterm_relax_type
-    values: ["GYRO", "SETPOINT"]
-    enum: itermRelaxType_e
   - name: airmodeHandlingType
     values: ["STICK_CENTER", "THROTTLE_THRESHOLD"]
   - name: nav_extra_arming_safety
@@ -1727,9 +1724,6 @@ groups:
         field: navFwPosHdgPidsumLimit
         min: PID_SUM_LIMIT_MIN
         max: PID_SUM_LIMIT_MAX
-      - name: mc_iterm_relax_type
-        field: iterm_relax_type
-        table: iterm_relax_type
       - name: mc_iterm_relax
         field: iterm_relax
         table: iterm_relax

src/main/flight/pid.c

@@ -124,7 +124,6 @@ int32_t axisPID_P[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_I[FLIGHT_DYNAMICS_INDEX_
 static EXTENDED_FASTRAM pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT];
 static EXTENDED_FASTRAM pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
 static EXTENDED_FASTRAM uint8_t itermRelax;
-static EXTENDED_FASTRAM uint8_t itermRelaxType;
 
 #ifdef USE_ANTIGRAVITY
 static EXTENDED_FASTRAM pt1Filter_t antigravityThrottleLpf;
@@ -155,7 +154,7 @@ static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
 static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn;
 static EXTENDED_FASTRAM bool levelingEnabled = false;
 
-PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 14);
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 15);
 
 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .bank_mc = {
@@ -264,7 +263,6 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .navVelXyDtermAttenuation = 90,
         .navVelXyDtermAttenuationStart = 10,
         .navVelXyDtermAttenuationEnd = 60,
-        .iterm_relax_type = ITERM_RELAX_SETPOINT,
         .iterm_relax_cutoff = MC_ITERM_RELAX_CUTOFF_DEFAULT,
         .iterm_relax = ITERM_RELAX_RP,
         .dBoostFactor = 1.25f,
@@ -636,31 +634,20 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
 #endif
 }
 
-static void FAST_CODE applyItermRelax(const int axis, const float gyroRate, float currentPidSetpoint, float *itermErrorRate)
+static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint, float itermErrorRate)
 {
-    const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
-    const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf);
-
     if (itermRelax) {
         if (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY) {
 
+            const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
+            const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf);
+
             const float itermRelaxFactor = MAX(0, 1 - setpointHpf / MC_ITERM_RELAX_SETPOINT_THRESHOLD);
-
-            if (itermRelaxType == ITERM_RELAX_SETPOINT) {
-                *itermErrorRate *= itermRelaxFactor;
-            } else if (itermRelaxType == ITERM_RELAX_GYRO ) {
-                *itermErrorRate = fapplyDeadbandf(setpointLpf - gyroRate, setpointHpf);
-            } else {
-                *itermErrorRate = 0.0f;
-            }
-
-            if (axis == FD_ROLL) {
-                DEBUG_SET(DEBUG_ITERM_RELAX, 0, lrintf(setpointHpf));
-                DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(itermRelaxFactor * 100.0f));
-                DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(*itermErrorRate));
-            }
+            return itermErrorRate * itermRelaxFactor;
         }
     }
+
+    return itermErrorRate;
 }
 #ifdef USE_D_BOOST
 static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) {
@@ -727,8 +714,7 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
     const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm;
     const float newOutputLimited = constrainf(newOutput, -pidState->pidSumLimit, +pidState->pidSumLimit);
 
-    float itermErrorRate = rateError;
-    applyItermRelax(axis, pidState->gyroRate, pidState->rateTarget, &itermErrorRate);
+    float itermErrorRate = applyItermRelax(axis, pidState->rateTarget, rateError);
 
 #ifdef USE_ANTIGRAVITY
     itermErrorRate *= iTermAntigravityGain;
@@ -1046,7 +1032,6 @@ void pidInit(void)
     pidGainsUpdateRequired = false;
 
     itermRelax = pidProfile()->iterm_relax;
-    itermRelaxType = pidProfile()->iterm_relax_type;
 
     yawLpfHz = pidProfile()->yaw_lpf_hz;
     motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f);

src/main/flight/pid.h

@@ -97,11 +97,6 @@ typedef enum {
     ITERM_RELAX_RPY
 } itermRelax_e;
 
-typedef enum {
-    ITERM_RELAX_GYRO = 0,
-    ITERM_RELAX_SETPOINT
-} itermRelaxType_e;
-
 typedef struct pidProfile_s {
     uint8_t pidControllerType;
     pidBank_t bank_fw;
@@ -138,7 +133,6 @@ typedef struct pidProfile_s {
     uint8_t navVelXyDtermAttenuation;       // VEL_XY dynamic Dterm scale: Dterm will be attenuatedby this value (in percent) when UAV is traveling with more than navVelXyDtermAttenuationStart percents of max velocity
     uint8_t navVelXyDtermAttenuationStart;  // VEL_XY dynamic Dterm scale: Dterm attenuation will begin at this percent of max velocity
     uint8_t navVelXyDtermAttenuationEnd;    // VEL_XY dynamic Dterm scale: Dterm will be fully attenuated at this percent of max velocity
-    uint8_t iterm_relax_type;               // Specifies type of relax algorithm
     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