Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 21:35:44 +03:00
Code Issues Wiki Activity

Merge pull request #6729 from betaflight/anti_windup_fix

Browse source 
Change itermWindupPointPercent default
This commit is contained in:
borisbstyle 2018-09-11 06:51:40 +02:00 • committed by GitHub
commit fff8045ba7
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
5 changed files with 43 additions and 25 deletions
Download patch file Download diff file Expand all files Collapse all files

9
src/main/flight/mixer.c
View file

@ -361,15 +361,6 @@ bool mixerIsTricopter(void)
#endif
}
bool mixerIsOutputSaturated(int axis, float errorRate)
{
if (axis == FD_YAW && mixerIsTricopter()) {
return mixerTricopterIsServoSaturated(errorRate);
}
return motorMixRange >= 1.0f;
}
// All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator
// DSHOT scaling is done to the actual dshot range
void initEscEndpoints(void)

1
src/main/flight/mixer.h
View file

@ -117,7 +117,6 @@ struct rxConfig_s;
uint8_t getMotorCount(void);
float getMotorMixRange(void);
bool areMotorsRunning(void);
bool mixerIsOutputSaturated(int axis, float errorRate);
void mixerLoadMix(int index, motorMixer_t *customMixers);
void mixerInit(mixerMode_e mixerMode);

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

@ -132,7 +132,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.dterm_notch_hz = 0,
.dterm_notch_cutoff = 0,
.dterm_filter_type = FILTER_PT1,
.itermWindupPointPercent = 40,
.itermWindupPointPercent = 100,
.vbatPidCompensation = 0,
.pidAtMinThrottle = PID_STABILISATION_ON,
.levelAngleLimit = 55,
@ -445,8 +445,13 @@ void pidInitConfig(const pidProfile_t *pidProfile)
horizonFactorRatio = (100 - pidProfile->horizon_tilt_effect) * 0.01f;
maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * dT;
maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * dT;
const float ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f;
ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint);
ITermWindupPointInv = 0.0f;
if (pidProfile->itermWindupPointPercent < 100) {
float ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f;
ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint);
} else {
ITermWindupPointInv = 0.0f;
}
itermAcceleratorGain = pidProfile->itermAcceleratorGain;
crashTimeLimitUs = pidProfile->crash_time * 1000;
crashTimeDelayUs = pidProfile->crash_delay * 1000;
@ -859,8 +864,10 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(itermAccelerator * 1000));
// gradually scale back integration when above windup point
const float dynCi = constrainf((1.0f - motorMixRange) * ITermWindupPointInv, 0.0f, 1.0f)
* dT * itermAccelerator;
float dynCi = dT * itermAccelerator;
if (ITermWindupPointInv > 0) {
dynCi *= constrainf((1.0f - motorMixRange) * ITermWindupPointInv, 0.0f, 1.0f);
}
// Precalculate gyro deta for D-term here, this allows loop unrolling
float gyroRateDterm[XYZ_AXIS_COUNT];
@ -985,12 +992,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
}
// -----calculate I component
const float ITermNew = constrainf(ITerm + pidCoefficient[axis].Ki * itermErrorRate * dynCi, -itermLimit, itermLimit);
const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate);
if (outputSaturated == false || ABS(ITermNew) < ABS(ITerm)) {
// Only increase ITerm if output is not saturated
pidData[axis].I = ITermNew;
}
pidData[axis].I = constrainf(ITerm + pidCoefficient[axis].Ki * itermErrorRate * dynCi, -itermLimit, itermLimit);
// -----calculate D component
if (pidCoefficient[axis].Kd > 0) {

2
src/main/interface/settings.c
View file

@ -846,7 +846,7 @@ const clivalue_t valueTable[] = {
{ "iterm_relax_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ITERM_RELAX_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_type) },
{ "iterm_relax_cutoff", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_cutoff) },
#endif
{ "iterm_windup", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 30, 99 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermWindupPointPercent) },
{ "iterm_windup", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 30, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermWindupPointPercent) },
{ "iterm_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermLimit) },
{ "pidsum_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { PIDSUM_LIMIT_MIN, PIDSUM_LIMIT_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimit) },
{ "pidsum_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { PIDSUM_LIMIT_MIN, PIDSUM_LIMIT_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimitYaw) },

32
src/test/unit/pid_unittest.cc
View file

@ -67,7 +67,6 @@ extern "C" {
float getThrottlePIDAttenuation(void) { return simulatedThrottlePIDAttenuation; }
float getMotorMixRange(void) { return simulatedMotorMixRange; }
float getSetpointRate(int axis) { return simulatedSetpointRate[axis]; }
bool mixerIsOutputSaturated(int, float) { return simulateMixerSaturated; }
float getRcDeflectionAbs(int axis) { return ABS(simulatedRcDeflection[axis]); }
void systemBeep(bool) { }
bool gyroOverflowDetected(void) { return false; }
@ -431,14 +430,41 @@ TEST(pidControllerTest, testMixerSaturation) {
// Test full stick response
setStickPosition(FD_ROLL, 1.0f);
setStickPosition(FD_PITCH, -1.0f);
simulateMixerSaturated = true;
setStickPosition(FD_YAW, 1.0f);
simulatedMotorMixRange = 2.0f;
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
// Expect no iterm accumulation
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].P);
EXPECT_FLOAT_EQ(0, pidData[FD_ROLL].I);
EXPECT_FLOAT_EQ(0, pidData[FD_PITCH].I);
EXPECT_FLOAT_EQ(0, pidData[FD_YAW].I);
// Test itermWindup limit
// First store values without exceeding iterm windup limit
resetTest();
ENABLE_ARMING_FLAG(ARMED);
pidStabilisationState(PID_STABILISATION_ON);
setStickPosition(FD_ROLL, 0.1f);
setStickPosition(FD_PITCH, -0.1f);
setStickPosition(FD_YAW, 0.1f);
simulatedMotorMixRange = 0.0f;
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
float rollTestIterm = pidData[FD_ROLL].I;
float pitchTestIterm = pidData[FD_PITCH].I;
float yawTestIterm = pidData[FD_YAW].I;
// Now compare values when exceeding the limit
resetTest();
ENABLE_ARMING_FLAG(ARMED);
pidStabilisationState(PID_STABILISATION_ON);
setStickPosition(FD_ROLL, 0.1f);
setStickPosition(FD_PITCH, -0.1f);
setStickPosition(FD_YAW, 0.1f);
simulatedMotorMixRange = (pidProfile->itermWindupPointPercent + 1) / 100.0f;
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
ASSERT_LT(pidData[FD_ROLL].I, rollTestIterm);
ASSERT_GE(pidData[FD_PITCH].I, pitchTestIterm);
ASSERT_LT(pidData[FD_YAW].I, yawTestIterm);
}
// TODO - Add more scenarios