rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Change default iterm_windup

This commit is contained in:
borisbstyle 2018-09-10 12:31:43 +02:00
parent 00dbaf9fa7
commit aa44fd6bbd
2 changed files with 16 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
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 - 0.001f) / 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];

8
src/test/unit/pid_unittest.cc
View File

@ -460,11 +460,11 @@ TEST(pidControllerTest, testMixerSaturation) {
setStickPosition(FD_ROLL, 0.1f);
setStickPosition(FD_PITCH, -0.1f);
setStickPosition(FD_YAW, 0.1f);
simulatedMotorMixRange = (pidProfile->itermWindupPointPercent + 1 / 100.0f);
simulatedMotorMixRange = (pidProfile->itermWindupPointPercent + 1) / 100.0f;
pidController(pidProfile, &rollAndPitchTrims, currentTestTime());
ASSERT_NE(pidData[FD_ROLL].I, rollTestIterm);
ASSERT_NE(pidData[FD_PITCH].I, pitchTestIterm);
ASSERT_NE(pidData[FD_YAW].I, yawTestIterm);
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