Merge tag 'v3.1.3'

src/main/fc/config.c

@@ -184,7 +184,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->yawRateAccelLimit = 10.0f;
     pidProfile->rateAccelLimit = 0.0f;
     pidProfile->itermThrottleThreshold = 350;
-    pidProfile->itermAcceleratorGain = 1.5f;
+    pidProfile->itermAcceleratorGain = 2.0f;
     pidProfile->itermAcceleratorRateLimit = 80;
 }
 

src/main/flight/pid.c

@@ -152,7 +152,9 @@ void pidInitFilters(const pidProfile_t *pidProfile)
     }
 }
 
-static float Kp[3], Ki[3], Kd[3], c[3], maxVelocity[3], relaxFactor[3];
+static float Kp[3], Ki[3], Kd[3], maxVelocity[3];
+static float relaxFactor;
+static float dtermSetpointWeight;
 static float levelGain, horizonGain, horizonTransition, ITermWindupPoint, ITermWindupPointInv, itermAcceleratorRateLimit;
 
 void pidInitConfig(const pidProfile_t *pidProfile) {
@@ -160,9 +162,9 @@ void pidInitConfig(const pidProfile_t *pidProfile) {
         Kp[axis] = PTERM_SCALE * pidProfile->P8[axis];
         Ki[axis] = ITERM_SCALE * pidProfile->I8[axis];
         Kd[axis] = DTERM_SCALE * pidProfile->D8[axis];
-        c[axis] = pidProfile->dtermSetpointWeight / 100.0f;
-        relaxFactor[axis] = 1.0f / (pidProfile->setpointRelaxRatio / 100.0f);
     }
+    dtermSetpointWeight = pidProfile->dtermSetpointWeight / 100.0f;
+    relaxFactor = 1.0f / (pidProfile->setpointRelaxRatio / 100.0f);
     levelGain = pidProfile->P8[PIDLEVEL] / 10.0f;
     horizonGain = pidProfile->I8[PIDLEVEL] / 10.0f;
     horizonTransition = 100.0f / pidProfile->D8[PIDLEVEL];
@@ -223,7 +225,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
     const float motorMixRange = getMotorMixRange();
 
     // Dynamic ki component to gradually scale back integration when above windup point
-    float dynKi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f);
+    const float dynKi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f);
 
     // ----------PID controller----------
     for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
@@ -240,7 +242,8 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
         const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
 
         // --------low-level gyro-based PID based on 2DOF PID controller. ----------
-        //  ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error).  ----------
+        // 2-DOF PID controller with optional filter on derivative term.
+        // b = 1 and only c (dtermSetpointWeight) can be tuned (amount derivative on measurement or error).
 
         // -----calculate error rate
         const float errorRate = currentPidSetpoint - gyroRate;       // r - y
@@ -254,45 +257,53 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
         // -----calculate I component
         float ITerm = previousGyroIf[axis];
         if (motorMixRange < 1.0f) {
-            // Only increase ITerm if motor output is not saturated and errorRate exceeds noise threshold
+            // Only increase ITerm if motor output is not saturated
-            // Iterm will only be accelerated below steady rate threshold
-            if (ABS(currentPidSetpoint) < itermAcceleratorRateLimit)
-                dynKi *= itermAccelerator;
             float ITermDelta = Ki[axis] * errorRate * dT * dynKi;
+            if (ABS(currentPidSetpoint) < itermAcceleratorRateLimit) {
+                // ITerm will only be accelerated below steady rate threshold
+                ITermDelta *= itermAccelerator;
+            }
             ITerm += ITermDelta;
             previousGyroIf[axis] = ITerm;
         }
 
-        // -----calculate D component (Yaw D not yet supported)
+        // -----calculate D component
-        float DTerm = 0.0;
+        if (axis == FD_YAW) {
-        if (axis != FD_YAW) {
+            // no DTerm for yaw axis
-            float dynC = c[axis];
+            // -----calculate total PID output
+            axisPIDf[FD_YAW] = PTerm + ITerm;
+#ifdef BLACKBOX
+            axisPID_P[FD_YAW] = PTerm;
+            axisPID_I[FD_YAW] = ITerm;
+            axisPID_D[FD_YAW] = 0;
+#endif
+        } else {
+            float dynC = dtermSetpointWeight;
             if (pidProfile->setpointRelaxRatio < 100) {
-                dynC *= MIN(getRcDeflectionAbs(axis) * relaxFactor[axis], 1.0f);
+                dynC *= MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f);
             }
             const float rD = dynC * currentPidSetpoint - gyroRate;    // cr - y
             // Divide rate change by dT to get differential (ie dr/dt)
             const float delta = (rD - previousRateError[axis]) / dT;
             previousRateError[axis] = rD;
 
-            DTerm = Kd[axis] * delta * tpaFactor;
+            float DTerm = Kd[axis] * delta * tpaFactor;
             DEBUG_SET(DEBUG_DTERM_FILTER, axis, DTerm);
 
             // apply filters
             DTerm = dtermNotchFilterApplyFn(dtermFilterNotch[axis], DTerm);
             DTerm = dtermLpfApplyFn(dtermFilterLpf[axis], DTerm);
 
+            // -----calculate total PID output
+            axisPIDf[axis] = PTerm + ITerm + DTerm;
+#ifdef BLACKBOX
+            axisPID_P[axis] = PTerm;
+            axisPID_I[axis] = ITerm;
+            axisPID_D[axis] = DTerm;
+#endif
         }
 
-        // -----calculate total PID output
-        axisPIDf[axis] = PTerm + ITerm + DTerm;
         // Disable PID control at zero throttle
         if (!pidStabilisationEnabled) axisPIDf[axis] = 0;
-
-#ifdef BLACKBOX
-        axisPID_P[axis] = PTerm;
-        axisPID_I[axis] = ITerm;
-        axisPID_D[axis] = DTerm;
-#endif
     }
 }