Merge pull request #4630 from iNavFlight/dzikuvx-port-iterm-relax

Direct port of BF Iterm relax

Author: DzikuVx

src/main/build/debug.h

@@ -69,5 +69,6 @@ typedef enum {
     DEBUG_SMARTAUDIO,
     DEBUG_ACC,
     DEBUG_GENERIC,
+    DEBUG_ITERM_RELAX,
     DEBUG_COUNT
 } debugType_e;

src/main/common/maths.c

@@ -139,6 +139,15 @@ 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

@@ -123,6 +123,7 @@ void 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

@@ -77,7 +77,7 @@ tables:
   - name: debug_modes
     values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW",
       "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR", "SAG_COMP_VOLTAGE",
-      "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "GENERIC"]
+      "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "GENERIC", "ITERM_RELAX"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
@@ -114,6 +114,12 @@ tables:
     values: ["PT1", "BIQUAD"]
   - name: log_level
     values: ["ERROR", "WARNING", "INFO", "VERBOSE", "DEBUG"]
+  - name: iterm_relax
+    values: ["OFF", "RP", "RPY"]
+    enum: itermRelax_e
+  - name: iterm_relax_type
+    values: ["GYRO", "SETPOINT"]
+    enum: itermRelaxType_e
 
 groups:
   - name: PG_GYRO_CONFIG
@@ -1107,6 +1113,16 @@ groups:
         condition: USE_NAV
         min: 0
         max: 255
+      - name: mc_iterm_relax_type
+        field: iterm_relax_type
+        table: iterm_relax_type
+      - name: mc_iterm_relax
+        field: iterm_relax
+        table: iterm_relax
+      - name: mc_iterm_relax_cutoff
+        field: iterm_relax_cutoff
+        min: 1
+        max: 100
 
   - name: PG_PID_AUTOTUNE_CONFIG
     type: pidAutotuneConfig_t

src/main/flight/pid.c

@@ -110,7 +110,12 @@ int32_t axisPID_P[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_I[FLIGHT_DYNAMICS_INDEX_
 
 STATIC_FASTRAM pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT];
 
-PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 7);
+static EXTENDED_FASTRAM pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
+static EXTENDED_FASTRAM uint8_t itermRelax;
+static EXTENDED_FASTRAM uint8_t itermRelaxType;
+static EXTENDED_FASTRAM float itermRelaxSetpointThreshold;
+
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 8);
 
 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .bank_mc = {
@@ -206,6 +211,9 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
 
         .loiter_direction = NAV_LOITER_RIGHT,
         .navVelXyDTermLpfHz = NAV_ACCEL_CUTOFF_FREQUENCY_HZ,
+        .iterm_relax_type = ITERM_RELAX_SETPOINT,
+        .iterm_relax_cutoff = MC_ITERM_RELAX_CUTOFF_DEFAULT,
+        .iterm_relax = ITERM_RELAX_OFF,
 );
 
 void pidInit(void)
@@ -217,6 +225,10 @@ void pidInit(void)
                            cos_approx(DECIDEGREES_TO_RADIANS(pidProfile()->max_angle_inclination[FD_PITCH]));
 
     pidGainsUpdateRequired = false;
+
+    itermRelax = pidProfile()->iterm_relax;
+    itermRelaxType = pidProfile()->iterm_relax_type;
+    itermRelaxSetpointThreshold = MC_ITERM_RELAX_SETPOINT_THRESHOLD * MC_ITERM_RELAX_CUTOFF_DEFAULT / pidProfile()->iterm_relax_cutoff;
 }
 
 bool pidInitFilters(void)
@@ -266,6 +278,10 @@ bool pidInitFilters(void)
         biquadFilterInitLPF(&pidState[axis].deltaLpfState, pidProfile()->dterm_lpf_hz, refreshRate);
     }
 
+    for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
+        pt1FilterInit(&windupLpf[i], pidProfile()->iterm_relax_cutoff, refreshRate * 1e-6f);
+    }
+
     pidFiltersConfigured = true;
 
     return true;
@@ -552,6 +568,33 @@ static void FAST_CODE pidApplyFixedWingRateController(pidState_t *pidState, flig
 #endif
 }
 
+static void FAST_CODE applyItermRelax(const int axis, const float gyroRate, 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 itermRelaxFactor = MAX(0, 1 - setpointHpf / itermRelaxSetpointThreshold);
+
+            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));
+            }
+        }
+    }
+}
+
 static void FAST_CODE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis)
 {
     const float rateError = pidState->rateTarget - pidState->gyroRate;
@@ -605,7 +648,10 @@ static void FAST_CODE pidApplyMulticopterRateController(pidState_t *pidState, fl
     const float motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f);
     const float antiWindupScaler = constrainf((1.0f - getMotorMixRange()) / motorItermWindupPoint, 0.0f, 1.0f);
 
-    pidState->errorGyroIf += (rateError * pidState->kI * antiWindupScaler * dT)
+    float itermErrorRate = rateError;
+    applyItermRelax(axis, pidState->gyroRate, pidState->rateTarget, &itermErrorRate);
+
+    pidState->errorGyroIf += (itermErrorRate * pidState->kI * antiWindupScaler * dT)
                              + ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT);
 
     // Don't grow I-term if motors are at their limit

src/main/flight/pid.h

@@ -50,6 +50,9 @@ FP-PID has been rescaled to match LuxFloat (and MWRewrite) from Cleanflight 1.13
 #define FP_PID_LEVEL_P_MULTIPLIER   6.56f       // Level P gain units is [1/sec] and angle error is [deg] => [deg/s]
 #define FP_PID_YAWHOLD_P_MULTIPLIER 80.0f
 
+#define MC_ITERM_RELAX_SETPOINT_THRESHOLD 40.0f
+#define MC_ITERM_RELAX_CUTOFF_DEFAULT 20
+
 typedef enum {
     /* PID              MC      FW  */
     PID_ROLL,       //   +       +
@@ -76,6 +79,17 @@ typedef struct pidBank_s {
     pid8_t  pid[PID_ITEM_COUNT];
 } pidBank_t;
 
+typedef enum {
+    ITERM_RELAX_OFF = 0,
+    ITERM_RELAX_RP,
+    ITERM_RELAX_RPY
+} itermRelax_e;
+
+typedef enum {
+    ITERM_RELAX_GYRO = 0,
+    ITERM_RELAX_SETPOINT
+} itermRelaxType_e;
+
 typedef struct pidProfile_s {
     pidBank_t bank_fw;
     pidBank_t bank_mc;
@@ -109,6 +123,10 @@ typedef struct pidProfile_s {
 
     uint8_t     loiter_direction;               // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise)
     float       navVelXyDTermLpfHz;
+
+    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
 } pidProfile_t;
 
 typedef struct pidAutotuneConfig_s {