Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 01:05:21 +03:00
Code Issues Wiki Activity

refactoring on pid i term

Browse source
This commit is contained in:
shota 2023-08-04 19:14:37 +09:00
parent 258a88ce15
commit f96fd6402a
1 changed files with 4 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

8
src/main/flight/pid.c
View file

@ -799,7 +799,7 @@ static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint,
const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint); const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf); 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; return itermErrorRate * itermRelaxFactor;
} }
} }
@ -1037,7 +1037,7 @@ void checkItermLimitingActive(pidState_t *pidState)
shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition); shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition);
} else } else
{ {
shouldActivate = mixerIsOutputSaturated(); shouldActivate = mixerIsOutputSaturated(); //just in case, since it is already managed by itermWindupPointPercent
} }
pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate; pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate;
@ -1135,8 +1135,8 @@ void FAST_CODE pidController(float dT)
pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees); pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees);
} }
// Prevent strong Iterm accumulation during stick inputs // Prevent Iterm accumulation when motors are near its saturation
antiWindupScaler = constrainf((1.0f - getMotorMixRange()) / motorItermWindupPoint, 0.0f, 1.0f); antiWindupScaler = constrainf((1.0f - getMotorMixRange() * MAX(2*motorItermWindupPoint,1)) / motorItermWindupPoint, 0.0f, 1.0f);
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
// Apply setpoint rate of change limits // Apply setpoint rate of change limits