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

Merge pull request #7652 from ctzsnooze/itermRelax-Setpoint-Fix 

iTermRelaxFactor fixed to only attenuate amount added to I each loop
This commit is contained in:
Michael Keller 2019-03-01 01:53:03 +13:00 committed by GitHub
parent bf1d72c568 03dc1a1aee
commit fc9b39ddd7
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 12 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/main/flight/pid.c
View File

@ -267,7 +267,8 @@ static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass;
static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT uint8_t itermRelax;
static FAST_RAM_ZERO_INIT uint8_t itermRelaxType;
static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoff;
static uint8_t itermRelaxCutoff;
static FAST_RAM_ZERO_INIT float itermRelaxSetpointThreshold;
#endif
#if defined(USE_ABSOLUTE_CONTROL)
@ -615,10 +616,12 @@ void pidInitConfig(const pidProfile_t *pidProfile)
#if defined(USE_SMART_FEEDFORWARD)
smartFeedforward = pidProfile->smart_feedforward;
#endif
#if defined(USE_ITERM_RELAX)
itermRelax = pidProfile->iterm_relax;
itermRelaxType = pidProfile->iterm_relax_type;
itermRelaxCutoff = pidProfile->iterm_relax_cutoff;
itermRelaxSetpointThreshold = ITERM_RELAX_SETPOINT_THRESHOLD * 20.0f / itermRelaxCutoff;
#endif
#ifdef USE_ACRO_TRAINER
@ -1107,16 +1110,13 @@ STATIC_UNIT_TESTED void applyItermRelax(const int axis, const float iterm,
const float setpointLpf = pt1FilterApply(&windupLpf[axis], *currentPidSetpoint);
const float setpointHpf = fabsf(*currentPidSetpoint - setpointLpf);
if (itermRelax &&
(axis < FD_YAW || itermRelax == ITERM_RELAX_RPY ||
itermRelax == ITERM_RELAX_RPY_INC)) {
const float itermRelaxFactor = 1 - setpointHpf / ITERM_RELAX_SETPOINT_THRESHOLD;
if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY || itermRelax == ITERM_RELAX_RPY_INC)) {
const float itermRelaxFactor = MAX(0, 1 - setpointHpf / itermRelaxSetpointThreshold);
const bool isDecreasingI =
((iterm > 0) && (*itermErrorRate < 0)) || ((iterm < 0) && (*itermErrorRate > 0));
if ((itermRelax >= ITERM_RELAX_RP_INC) && isDecreasingI) {
// Do Nothing, use the precalculed itermErrorRate
} else if (itermRelaxType == ITERM_RELAX_SETPOINT && setpointHpf < ITERM_RELAX_SETPOINT_THRESHOLD) {
} else if (itermRelaxType == ITERM_RELAX_SETPOINT) {
*itermErrorRate *= itermRelaxFactor;
} else if (itermRelaxType == ITERM_RELAX_GYRO ) {
*itermErrorRate = fapplyDeadband(setpointLpf - gyroRate, setpointHpf);

1
src/main/flight/pid.h
View File

@ -44,6 +44,7 @@
// This value gives the same "feel" as the previous Kd default of 26 (26 * DTERM_SCALE)
#define FEEDFORWARD_SCALE 0.013754f
// Full iterm suppression at 40deg/sec * default cutoff of 20
#define ITERM_RELAX_SETPOINT_THRESHOLD 30.0f
typedef enum {

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

@ -529,12 +529,12 @@ TEST(pidControllerTest, testItermRelax) {
applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, &currentPidSetpoint);
ASSERT_NEAR(-6.66, itermErrorRate, calculateTolerance(-6.66));
ASSERT_NEAR(-8.16, itermErrorRate, calculateTolerance(-6.66));
currentPidSetpoint += ITERM_RELAX_SETPOINT_THRESHOLD;
applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, &currentPidSetpoint);
EXPECT_FLOAT_EQ(itermErrorRate, 0);
ASSERT_NEAR(-2.17, itermErrorRate, calculateTolerance(-2.17));
applyItermRelax(FD_PITCH, pidData[FD_PITCH].I, gyroRate, &itermErrorRate, &currentPidSetpoint);
EXPECT_FLOAT_EQ(itermErrorRate, 0);
ASSERT_NEAR(-0.58, itermErrorRate, calculateTolerance(-0.58));
pidProfile->iterm_relax_type = ITERM_RELAX_GYRO;
pidInit(pidProfile);
@ -578,7 +578,7 @@ TEST(pidControllerTest, testItermRelax) {
pidProfile->iterm_relax = ITERM_RELAX_RPY;
pidInit(pidProfile);
applyItermRelax(FD_YAW, pidData[FD_YAW].I, gyroRate, &itermErrorRate, &currentPidSetpoint);
ASSERT_NEAR(-3.6, itermErrorRate, calculateTolerance(-3.6));
ASSERT_NEAR(-6.46, itermErrorRate, calculateTolerance(-3.6));
}
// TODO - Add more tests