Add D-term to control loop on fixed wing

docs/Settings.md

@@ -114,6 +114,9 @@
 | fw_autotune_threshold | 50 | Threshold [%] of max rate to consider overshoot/undershoot detection |
 | fw_autotune_undershoot_time | 200 | Time [ms] to detect sustained undershoot |
 | fw_d_level | 75 | Fixed-wing attitude stabilisation HORIZON transition point |
+| fw_d_pitch | 0 | Fixed wing rate stabilisation D-gain for PITCH |
+| fw_d_roll | 0 | Fixed wing rate stabilisation D-gain for ROLL |
+| fw_d_yaw | 0 | Fixed wing rate stabilisation D-gain for YAW |
 | fw_ff_pitch | 50 | Fixed-wing rate stabilisation FF-gain for PITCH |
 | fw_ff_roll | 50 | Fixed-wing rate stabilisation FF-gain for ROLL |
 | fw_ff_yaw | 60 | Fixed-wing rate stabilisation FF-gain for YAW |

src/main/fc/settings.yaml

@@ -1464,6 +1464,12 @@ groups:
         field: bank_fw.pid[PID_PITCH].I
         min: 0
         max: 200
+      - name: fw_d_pitch
+        description: "Fixed wing rate stabilisation D-gain for PITCH"
+        default_value: "0"
+        field: bank_fw.pid[PID_PITCH].D
+        min: 0
+        max: 200
       - name: fw_ff_pitch
         description: "Fixed-wing rate stabilisation FF-gain for PITCH"
         default_value: "50"
@@ -1482,6 +1488,12 @@ groups:
         field: bank_fw.pid[PID_ROLL].I
         min: 0
         max: 200
+      - name: fw_d_roll
+        description: "Fixed wing rate stabilisation D-gain for ROLL"
+        default_value: "0"
+        field: bank_fw.pid[PID_ROLL].D
+        min: 0
+        max: 200
       - name: fw_ff_roll
         description: "Fixed-wing rate stabilisation FF-gain for ROLL"
         default_value: "50"
@@ -1500,6 +1512,12 @@ groups:
         field: bank_fw.pid[PID_YAW].I
         min: 0
         max: 200
+      - name: fw_d_yaw
+        description: "Fixed wing rate stabilisation D-gain for YAW"
+        default_value: "0"
+        field: bank_fw.pid[PID_YAW].D
+        min: 0
+        max: 200
       - name: fw_ff_yaw
         description: "Fixed-wing rate stabilisation FF-gain for YAW"
         default_value: "60"

src/main/flight/pid.c

@@ -483,7 +483,7 @@ void updatePIDCoefficients()
             // Airplanes - scale all PIDs according to TPA
             pidState[axis].kP  = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER  * tpaFactor;
             pidState[axis].kI  = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER  * tpaFactor;
-            pidState[axis].kD  = 0.0f;
+            pidState[axis].kD  = pidBank()->pid[axis].D / FP_PID_RATE_D_MULTIPLIER * tpaFactor;
             pidState[axis].kFF = pidBank()->pid[axis].FF / FP_PID_RATE_FF_MULTIPLIER * tpaFactor;
             pidState[axis].kCD = 0.0f;
             pidState[axis].kT  = 0.0f;
@@ -497,7 +497,7 @@ void updatePIDCoefficients()
             pidState[axis].kFF = 0.0f;
 
             // Tracking anti-windup requires P/I/D to be all defined which is only true for MC
-            if ((pidBank()->pid[axis].P != 0) && (pidBank()->pid[axis].I != 0)) {
+            if ((pidBank()->pid[axis].P != 0) && (pidBank()->pid[axis].I != 0) && (usedPidControllerType == PID_TYPE_PID)) {
                 pidState[axis].kT = 2.0f / ((pidState[axis].kP / pidState[axis].kI) + (pidState[axis].kD / pidState[axis].kP));
             } else {
                 pidState[axis].kT = 0;
@@ -610,6 +610,50 @@ static float pTermProcess(pidState_t *pidState, float rateError, float dT) {
     return pidState->ptermFilterApplyFn(&pidState->ptermLpfState, newPTerm, yawLpfHz, dT);
 }
 
+#ifdef USE_D_BOOST
+static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) {
+
+    float dBoost = 1.0f;
+
+    if (dBoostFactor > 1) {
+        const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) / dT;
+        const float dBoostGyroAcceleration = fabsf(biquadFilterApply(&pidState->dBoostGyroLpf, dBoostGyroDelta));
+        const float dBoostRateAcceleration = fabsf((pidState->rateTarget - pidState->previousRateTarget) / dT);
+
+        const float acceleration = MAX(dBoostGyroAcceleration, dBoostRateAcceleration);
+        dBoost = scaleRangef(acceleration, 0.0f, dBoostMaxAtAlleceleration, 1.0f, dBoostFactor);
+        dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT);
+        dBoost = constrainf(dBoost, 1.0f, dBoostFactor);
+    } 
+
+    return dBoost;
+}
+#else 
+static float applyDBoost(pidState_t *pidState, float dT) {
+    UNUSED(pidState);
+    UNUSED(dT);
+    return 1.0f;
+}
+#endif
+
+static float dTermProcess(pidState_t *pidState, float dT) {
+    // Calculate new D-term
+    float newDTerm = 0;
+    if (pidState->kD == 0) {
+        // optimisation for when D is zero, often used by YAW axis
+        newDTerm = 0;
+    } else {
+        float delta = pidState->previousRateGyro - pidState->gyroRate;
+
+        delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta);
+        delta = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, delta);
+
+        // Calculate derivative
+        newDTerm =  delta * (pidState->kD / dT) * applyDBoost(pidState, dT);
+    }
+    return(newDTerm);
+}
+
 static void applyItermLimiting(pidState_t *pidState) {
     if (pidState->itermLimitActive) {
         pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
@@ -629,6 +673,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
 {
     const float rateError = pidState->rateTarget - pidState->gyroRate;
     const float newPTerm = pTermProcess(pidState, rateError, dT);
+    const float newDTerm = dTermProcess(pidState, dT);
     const float newFFTerm = pidState->rateTarget * pidState->kFF;
 
     /*
@@ -650,14 +695,17 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
     }
 #endif
 
-    axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf, -pidState->pidSumLimit, +pidState->pidSumLimit);
+    axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -pidState->pidSumLimit, +pidState->pidSumLimit);
 
 #ifdef USE_BLACKBOX
     axisPID_P[axis] = newPTerm;
     axisPID_I[axis] = pidState->errorGyroIf;
-    axisPID_D[axis] = newFFTerm;
+    axisPID_D[axis] = newDTerm;
     axisPID_Setpoint[axis] = pidState->rateTarget;
 #endif
+
+    pidState->previousRateGyro = pidState->gyroRate;
+
 }
 
 static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint, float itermErrorRate)
@@ -675,36 +723,12 @@ static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint,
 
     return itermErrorRate;
 }
-#ifdef USE_D_BOOST
-static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) {
-
-    float dBoost = 1.0f;
-
-    if (dBoostFactor > 1) {
-        const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) / dT;
-        const float dBoostGyroAcceleration = fabsf(biquadFilterApply(&pidState->dBoostGyroLpf, dBoostGyroDelta));
-        const float dBoostRateAcceleration = fabsf((pidState->rateTarget - pidState->previousRateTarget) / dT);
-
-        const float acceleration = MAX(dBoostGyroAcceleration, dBoostRateAcceleration);
-        dBoost = scaleRangef(acceleration, 0.0f, dBoostMaxAtAlleceleration, 1.0f, dBoostFactor);
-        dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT);
-        dBoost = constrainf(dBoost, 1.0f, dBoostFactor);
-    } 
-
-    return dBoost;
-}
-#else 
-static float applyDBoost(pidState_t *pidState, float dT) {
-    UNUSED(pidState);
-    UNUSED(dT);
-    return 1.0f;
-}
-#endif
 
 static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
 {
     const float rateError = pidState->rateTarget - pidState->gyroRate;
     const float newPTerm = pTermProcess(pidState, rateError, dT);
+    const float newDTerm = dTermProcess(pidState, dT);
 
     const float rateTargetDelta = pidState->rateTarget - pidState->previousRateTarget;
     const float rateTargetDeltaFiltered = biquadFilterApply(&pidState->rateTargetFilter, rateTargetDelta);
@@ -721,21 +745,6 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
     }
     DEBUG_SET(DEBUG_CD, axis, newCDTerm);
 
-    // Calculate new D-term
-    float newDTerm;
-    if (pidState->kD == 0) {
-        // optimisation for when D8 is zero, often used by YAW axis
-        newDTerm = 0;
-    } else {
-        float delta = pidState->previousRateGyro - pidState->gyroRate;
-
-        delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta);
-        delta = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, delta);
-
-        // Calculate derivative
-        newDTerm =  delta * (pidState->kD / dT) * applyDBoost(pidState, dT);
-    }
-
     // TODO: Get feedback from mixer on available correction range for each axis
     const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm;
     const float newOutputLimited = constrainf(newOutput, -pidState->pidSumLimit, +pidState->pidSumLimit);

src/main/flight/pid.h

@@ -76,7 +76,7 @@ typedef enum {
 typedef enum {
     PID_TYPE_NONE = 0,  // Not used in the current platform/mixer/configuration
     PID_TYPE_PID,   // Uses P, I and D terms
-    PID_TYPE_PIFF,  // Uses P, I and FF, ignoring D
+    PID_TYPE_PIFF,  // Uses P, I, D and FF
     PID_TYPE_AUTO,  // Autodetect
 } pidType_e;