From 32748a328a6f6bc6f87310cbace1e352018d6a39 Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Thu, 24 May 2018 15:05:03 +0200 Subject: [PATCH 01/10] ITerm relax feature --- src/main/flight/pid.c | 38 ++++++++++++++++++++++++++++++----- src/main/flight/pid.h | 4 ++++ src/main/interface/settings.c | 3 +++ 3 files changed, 40 insertions(+), 5 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 035f4e97e8..91950f1e0b 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -139,7 +139,10 @@ void resetPidProfile(pidProfile_t *pidProfile) .throttle_boost = 0, .throttle_boost_cutoff = 15, .iterm_rotation = false, - .smart_feedforward = false + .smart_feedforward = false, + .iterm_relax = false, + .iterm_relax_cutoff_low = 3, + .iterm_relax_cutoff_high = 15, ); } @@ -198,6 +201,10 @@ static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpass2ApplyFn; static FAST_RAM_ZERO_INIT pt1Filter_t dtermLowpass2[2]; static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn; static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass; +static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[3][2]; +static FAST_RAM_ZERO_INIT bool itermRelax; +static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffLow; +static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffHigh; void pidInitFilters(const pidProfile_t *pidProfile) { @@ -282,6 +289,11 @@ void pidInitFilters(const pidProfile_t *pidProfile) } pt1FilterInit(&throttleLpf, pt1FilterGain(pidProfile->throttle_boost_cutoff, dT)); + if (itermRelax) + for (int i = 0; i < 3; i++) { + pt1FilterInit(&windupLpf[i][0], pt1FilterGain(itermRelaxCutoffLow, dT)); + pt1FilterInit(&windupLpf[i][1], pt1FilterGain(itermRelaxCutoffHigh, dT)); + } } typedef struct pidCoefficient_s { @@ -345,8 +357,11 @@ void pidInitConfig(const pidProfile_t *pidProfile) crashLimitYaw = pidProfile->crash_limit_yaw; itermLimit = pidProfile->itermLimit; throttleBoost = pidProfile->throttle_boost * 0.1f; - itermRotation = pidProfile->iterm_rotation == 1; - smartFeedforward = pidProfile->smart_feedforward == 1; + itermRotation = pidProfile->iterm_rotation; + smartFeedforward = pidProfile->smart_feedforward; + itermRelax = pidProfile->iterm_relax; + itermRelaxCutoffLow = pidProfile->iterm_relax_cutoff_low; + itermRelaxCutoffHigh = pidProfile->iterm_relax_cutoff_high; } void pidInit(const pidProfile_t *pidProfile) @@ -592,7 +607,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT currentPidSetpoint = 0.0f; } #endif // USE_YAW_SPIN_RECOVERY - + // -----calculate error rate const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec float errorRate = currentPidSetpoint - gyroRate; // r - y @@ -612,8 +627,21 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT } // -----calculate I component + float itermErrorRate; + if (itermRelax) { + const float gyroTargetHigh = pt1FilterApply(&windupLpf[axis][0], currentPidSetpoint); + const float gyroTargetLow = pt1FilterApply(&windupLpf[axis][1], currentPidSetpoint); + const float gmax = MAX(gyroTargetHigh, gyroTargetLow); + const float gmin = MIN(gyroTargetHigh, gyroTargetLow); + if (gyroRate >= gmin && gyroRate <= gmax) + itermErrorRate = 0.0f; + else + itermErrorRate = (gyroRate > gmax ? gmax : gmin ) - gyroRate; + } + else itermErrorRate = errorRate; + const float ITerm = pidData[axis].I; - const float ITermNew = constrainf(ITerm + pidCoefficient[axis].Ki * errorRate * dynCi, -itermLimit, itermLimit); + 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 diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 81705db067..56d00a9aa6 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -116,6 +116,10 @@ typedef struct pidProfile_s { uint8_t throttle_boost_cutoff; // Which cutoff frequency to use for throttle boost. higher cutoffs keep the boost on for shorter. Specified in hz. uint8_t iterm_rotation; // rotates iterm to translate world errors to local coordinate system uint8_t smart_feedforward; // takes only the larger of P and the D weight feed forward term if they have the same sign. + uint8_t iterm_relax_cutoff_low; // Slowest setpoint response to prevent iterm accumulation + uint8_t iterm_relax_cutoff_high; // Fastest setpoint response to prevent iterm accumulation + uint8_t iterm_relax; // Enable iterm suppression during stick input + } pidProfile_t; #ifndef USE_OSD_SLAVE diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index f46dc13b25..5b890821c5 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -757,6 +757,9 @@ const clivalue_t valueTable[] = { { "iterm_rotation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_rotation) }, { "smart_feedforward", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, smart_feedforward) }, + { "iterm_relax", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax) }, + { "iterm_relax_cutoff_low", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_cutoff_low) }, + { "iterm_relax_cutoff_high", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_cutoff_high) }, { "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) }, From 791c584d813051422c20be31c448afcbae93b47c Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Thu, 24 May 2018 22:08:13 +0200 Subject: [PATCH 02/10] add debug option DEBUG_ITERM_RELAX --- src/main/build/debug.c | 1 + src/main/build/debug.h | 1 + src/main/flight/pid.c | 2 ++ 3 files changed, 4 insertions(+) diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 9ad6d60404..90fddec09b 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -69,4 +69,5 @@ const char * const debugModeNames[DEBUG_COUNT] = { "USB", "SMARTAUDIO", "RTH", + "ITERM_RELAX" }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 9432e75e48..82134d3ef4 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -87,6 +87,7 @@ typedef enum { DEBUG_USB, DEBUG_SMARTAUDIO, DEBUG_RTH, + DEBUG_ITERM_RELAX, DEBUG_COUNT } debugType_e; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 91950f1e0b..e87268a1ed 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -631,6 +631,8 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT if (itermRelax) { const float gyroTargetHigh = pt1FilterApply(&windupLpf[axis][0], currentPidSetpoint); const float gyroTargetLow = pt1FilterApply(&windupLpf[axis][1], currentPidSetpoint); + DEBUG_SET(DEBUG_ITERM_RELAX, 0, gyroTargetHigh); + DEBUG_SET(DEBUG_ITERM_RELAX, 1, gyroTargetLow); const float gmax = MAX(gyroTargetHigh, gyroTargetLow); const float gmin = MIN(gyroTargetHigh, gyroTargetLow); if (gyroRate >= gmin && gyroRate <= gmax) From e1ff2566c64365110253871079554ced4e8ec63b Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Thu, 24 May 2018 23:20:58 +0200 Subject: [PATCH 03/10] make unit tests happy --- src/test/Makefile | 1 + 1 file changed, 1 insertion(+) diff --git a/src/test/Makefile b/src/test/Makefile index f7f89443b0..346a08747a 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -298,6 +298,7 @@ pid_unittest_SRC := \ $(USER_DIR)/drivers/accgyro/gyro_sync.c \ $(USER_DIR)/flight/pid.c \ $(USER_DIR)/pg/pg.c \ + $(USER_DIR)/build/debug.c \ $(USER_DIR)/fc/runtime_config.c rcdevice_unittest_DEFINES := \ From 00869329468974541ffd9d343ed0e1a1b37dffcf Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Fri, 25 May 2018 07:16:23 +0200 Subject: [PATCH 04/10] address style requests --- src/main/flight/pid.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e87268a1ed..eca22b3c34 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -607,7 +607,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT currentPidSetpoint = 0.0f; } #endif // USE_YAW_SPIN_RECOVERY - + // -----calculate error rate const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec float errorRate = currentPidSetpoint - gyroRate; // r - y @@ -635,10 +635,12 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT DEBUG_SET(DEBUG_ITERM_RELAX, 1, gyroTargetLow); const float gmax = MAX(gyroTargetHigh, gyroTargetLow); const float gmin = MIN(gyroTargetHigh, gyroTargetLow); - if (gyroRate >= gmin && gyroRate <= gmax) + if (gyroRate >= gmin && gyroRate <= gmax) { itermErrorRate = 0.0f; - else + } + else { itermErrorRate = (gyroRate > gmax ? gmax : gmin ) - gyroRate; + } } else itermErrorRate = errorRate; From 5a1d84cdae7b33dcf422a9e632ac37192724fa81 Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Fri, 25 May 2018 11:32:15 +0200 Subject: [PATCH 05/10] incorporate style feedback --- src/main/flight/pid.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index eca22b3c34..367e6c559a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -629,20 +629,20 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT // -----calculate I component float itermErrorRate; if (itermRelax) { - const float gyroTargetHigh = pt1FilterApply(&windupLpf[axis][0], currentPidSetpoint); - const float gyroTargetLow = pt1FilterApply(&windupLpf[axis][1], currentPidSetpoint); + const float gyroTargetLow = pt1FilterApply(&windupLpf[axis][0], currentPidSetpoint); + const float gyroTargetHigh = pt1FilterApply(&windupLpf[axis][1], currentPidSetpoint); DEBUG_SET(DEBUG_ITERM_RELAX, 0, gyroTargetHigh); DEBUG_SET(DEBUG_ITERM_RELAX, 1, gyroTargetLow); const float gmax = MAX(gyroTargetHigh, gyroTargetLow); const float gmin = MIN(gyroTargetHigh, gyroTargetLow); if (gyroRate >= gmin && gyroRate <= gmax) { itermErrorRate = 0.0f; - } - else { + } else { itermErrorRate = (gyroRate > gmax ? gmax : gmin ) - gyroRate; } + } else { + itermErrorRate = errorRate; } - else itermErrorRate = errorRate; const float ITerm = pidData[axis].I; const float ITermNew = constrainf(ITerm + pidCoefficient[axis].Ki * itermErrorRate * dynCi, -itermLimit, itermLimit); From 2c02c14a1dcfea4eabda9b55b0fe9c6961f1135e Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Sun, 27 May 2018 09:33:20 +0200 Subject: [PATCH 06/10] debug log only roll and pitch, but separately --- src/main/flight/pid.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 367e6c559a..63a2022cdb 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -631,8 +631,11 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT if (itermRelax) { const float gyroTargetLow = pt1FilterApply(&windupLpf[axis][0], currentPidSetpoint); const float gyroTargetHigh = pt1FilterApply(&windupLpf[axis][1], currentPidSetpoint); - DEBUG_SET(DEBUG_ITERM_RELAX, 0, gyroTargetHigh); - DEBUG_SET(DEBUG_ITERM_RELAX, 1, gyroTargetLow); + if (axis < FD_YAW) { + int itemOffset = (axis << 1); + DEBUG_SET(DEBUG_ITERM_RELAX, itemOffset++, gyroTargetHigh); + DEBUG_SET(DEBUG_ITERM_RELAX, itemOffset, gyroTargetLow); + } const float gmax = MAX(gyroTargetHigh, gyroTargetLow); const float gmin = MIN(gyroTargetHigh, gyroTargetLow); if (gyroRate >= gmin && gyroRate <= gmax) { From 7e12e9da18ff01f9e17e4644d0b2a31fff43645f Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Mon, 28 May 2018 16:03:49 +0200 Subject: [PATCH 07/10] don't link debug.c to pid unit test --- src/test/Makefile | 1 - src/test/unit/pid_unittest.cc | 5 ++++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/test/Makefile b/src/test/Makefile index 346a08747a..f7f89443b0 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -298,7 +298,6 @@ pid_unittest_SRC := \ $(USER_DIR)/drivers/accgyro/gyro_sync.c \ $(USER_DIR)/flight/pid.c \ $(USER_DIR)/pg/pg.c \ - $(USER_DIR)/build/debug.c \ $(USER_DIR)/fc/runtime_config.c rcdevice_unittest_DEFINES := \ diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index a526c39c7d..46dc7cede3 100644 --- a/src/test/unit/pid_unittest.cc +++ b/src/test/unit/pid_unittest.cc @@ -22,7 +22,7 @@ #include "unittest_macros.h" #include "gtest/gtest.h" - +#include "build/debug.h" bool simulateMixerSaturated = false; float simulatedSetpointRate[3] = { 0,0,0 }; @@ -30,6 +30,9 @@ float simulatedRcDeflection[3] = { 0,0,0 }; float simulatedThrottlePIDAttenuation = 1.0f; float simulatedMotorMixRange = 0.0f; +int16_t debug[DEBUG16_VALUE_COUNT]; +uint8_t debugMode; + extern "C" { #include "build/debug.h" #include "common/axis.h" From 6239b8258a1c079cc5f579f02398e1ef9306e292 Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Mon, 28 May 2018 16:34:25 +0200 Subject: [PATCH 08/10] make itermrelax on yaw optional --- src/main/flight/pid.c | 2 +- src/main/interface/settings.c | 10 ++++++++-- src/main/interface/settings.h | 1 + 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 63a2022cdb..31dc30ef27 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -628,7 +628,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT // -----calculate I component float itermErrorRate; - if (itermRelax) { + if (itermRelax && (axis < FD_YAW || itermRelax == 2 )) { const float gyroTargetLow = pt1FilterApply(&windupLpf[axis][0], currentPidSetpoint); const float gyroTargetHigh = pt1FilterApply(&windupLpf[axis][1], currentPidSetpoint); if (axis < FD_YAW) { diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 5b890821c5..f31e4bdf36 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -344,6 +344,11 @@ static const char * const lookupTableVideoSystem[] = { }; #endif // USE_MAX7456 +static const char * const lookupTableItermRelax[] = { + "OFF", "RP", "RPY" +}; + + #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) } const lookupTableEntry_t lookupTables[] = { @@ -418,6 +423,7 @@ const lookupTableEntry_t lookupTables[] = { #ifdef USE_MAX7456 LOOKUP_TABLE_ENTRY(lookupTableVideoSystem), #endif // USE_MAX7456 + LOOKUP_TABLE_ENTRY(lookupTableItermRelax), }; #undef LOOKUP_TABLE_ENTRY @@ -757,9 +763,9 @@ const clivalue_t valueTable[] = { { "iterm_rotation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_rotation) }, { "smart_feedforward", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, smart_feedforward) }, - { "iterm_relax", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax) }, + { "iterm_relax", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ITERM_RELAX }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax) }, { "iterm_relax_cutoff_low", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_cutoff_low) }, - { "iterm_relax_cutoff_high", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_cutoff_high) }, + { "iterm_relax_cutoff_high", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_cutoff_high) }, { "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) }, diff --git a/src/main/interface/settings.h b/src/main/interface/settings.h index 0aad62e96b..83726e24a3 100644 --- a/src/main/interface/settings.h +++ b/src/main/interface/settings.h @@ -97,6 +97,7 @@ typedef enum { #ifdef USE_MAX7456 TABLE_VIDEO_SYSTEM, #endif // USE_MAX7456 + TABLE_ITERM_RELAX, LOOKUP_TABLE_COUNT } lookupTableIndex_e; From 5bdc2ead619351bddc8f6895696a0ef3989427e9 Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Mon, 28 May 2018 16:44:13 +0200 Subject: [PATCH 09/10] make itermRelax an uint8_t --- src/main/flight/pid.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 31dc30ef27..078a8b247e 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -202,7 +202,7 @@ static FAST_RAM_ZERO_INIT pt1Filter_t dtermLowpass2[2]; static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn; static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass; static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[3][2]; -static FAST_RAM_ZERO_INIT bool itermRelax; +static FAST_RAM_ZERO_INIT uint8_t itermRelax; static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffLow; static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffHigh; From dc929baf75ede6247358c132b4377fab8fbd14a0 Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Tue, 29 May 2018 16:40:23 +0200 Subject: [PATCH 10/10] make itermRelax into enum --- src/main/flight/pid.c | 6 +++--- src/main/flight/pid.h | 11 ++++++++++- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 078a8b247e..17f002bc00 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -140,7 +140,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .throttle_boost_cutoff = 15, .iterm_rotation = false, .smart_feedforward = false, - .iterm_relax = false, + .iterm_relax = ITERM_RELAX_OFF, .iterm_relax_cutoff_low = 3, .iterm_relax_cutoff_high = 15, ); @@ -202,7 +202,7 @@ static FAST_RAM_ZERO_INIT pt1Filter_t dtermLowpass2[2]; static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn; static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass; static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[3][2]; -static FAST_RAM_ZERO_INIT uint8_t itermRelax; +static FAST_RAM_ZERO_INIT itermRelax_e itermRelax; static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffLow; static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffHigh; @@ -628,7 +628,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT // -----calculate I component float itermErrorRate; - if (itermRelax && (axis < FD_YAW || itermRelax == 2 )) { + if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) { const float gyroTargetLow = pt1FilterApply(&windupLpf[axis][0], currentPidSetpoint); const float gyroTargetHigh = pt1FilterApply(&windupLpf[axis][1], currentPidSetpoint); if (axis < FD_YAW) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 56d00a9aa6..71852390e2 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -76,6 +76,15 @@ typedef struct pid8_s { uint8_t D; } pid8_t; +typedef enum +{ + ITERM_RELAX_OFF, + ITERM_RELAX_RP, + ITERM_RELAX_RPY +} itermRelax_e; + + + typedef struct pidProfile_s { pid8_t pid[PID_ITEM_COUNT]; @@ -118,7 +127,7 @@ typedef struct pidProfile_s { uint8_t smart_feedforward; // takes only the larger of P and the D weight feed forward term if they have the same sign. uint8_t iterm_relax_cutoff_low; // Slowest setpoint response to prevent iterm accumulation uint8_t iterm_relax_cutoff_high; // Fastest setpoint response to prevent iterm accumulation - uint8_t iterm_relax; // Enable iterm suppression during stick input + itermRelax_e iterm_relax; // Enable iterm suppression during stick input } pidProfile_t;