From f96fd6402aa06a1c9bf933e6cc4b0cb94d546b14 Mon Sep 17 00:00:00 2001 From: shota Date: Fri, 4 Aug 2023 19:14:37 +0900 Subject: [PATCH] refactoring on pid i term --- src/main/flight/pid.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 13a7c45d78..fc7fa132fe 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -799,7 +799,7 @@ static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint, 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); + const float itermRelaxFactor = MAX(0, 1 - setpointHpf / (FLIGHT_MODE(ANGLE_MODE) ? (MC_ITERM_RELAX_SETPOINT_THRESHOLD * 0.2f):MC_ITERM_RELAX_SETPOINT_THRESHOLD)); return itermErrorRate * itermRelaxFactor; } } @@ -1037,7 +1037,7 @@ void checkItermLimitingActive(pidState_t *pidState) shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition); } else { - shouldActivate = mixerIsOutputSaturated(); + shouldActivate = mixerIsOutputSaturated(); //just in case, since it is already managed by itermWindupPointPercent } pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate; @@ -1135,8 +1135,8 @@ void FAST_CODE pidController(float dT) pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees); } - // Prevent strong Iterm accumulation during stick inputs - antiWindupScaler = constrainf((1.0f - getMotorMixRange()) / motorItermWindupPoint, 0.0f, 1.0f); + // Prevent Iterm accumulation when motors are near its saturation + antiWindupScaler = constrainf((1.0f - getMotorMixRange() * MAX(2*motorItermWindupPoint,1)) / motorItermWindupPoint, 0.0f, 1.0f); for (int axis = 0; axis < 3; axis++) { // Apply setpoint rate of change limits