diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 3c17c4bd02..58a847ecf8 100755 --- a/src/main/fc/config.c +++ b/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; } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 1f855739f9..b7106432fc 100644 --- a/src/main/flight/pid.c +++ b/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 - // Iterm will only be accelerated below steady rate threshold - if (ABS(currentPidSetpoint) < itermAcceleratorRateLimit) - dynKi *= itermAccelerator; + // Only increase ITerm if motor output is not saturated 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) - float DTerm = 0.0; - if (axis != FD_YAW) { - float dynC = c[axis]; + // -----calculate D component + if (axis == FD_YAW) { + // no DTerm for yaw 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 } }