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 43e69b12f0..b4a444ad3e 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 = ITERM_RELAX_OFF, + .iterm_relax_cutoff_low = 3, + .iterm_relax_cutoff_high = 15, ); } @@ -195,6 +198,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 itermRelax_e itermRelax; +static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffLow; +static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffHigh; void pidInitFilters(const pidProfile_t *pidProfile) { @@ -271,6 +278,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 { @@ -334,8 +346,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) @@ -601,8 +616,28 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT } // -----calculate I component + float itermErrorRate; + 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) { + 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) { + 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..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]; @@ -116,6 +125,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 + itermRelax_e 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 284ada8fe9..c62e609531 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -333,6 +333,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[] = { @@ -407,6 +412,7 @@ const lookupTableEntry_t lookupTables[] = { #ifdef USE_MAX7456 LOOKUP_TABLE_ENTRY(lookupTableVideoSystem), #endif // USE_MAX7456 + LOOKUP_TABLE_ENTRY(lookupTableItermRelax), }; #undef LOOKUP_TABLE_ENTRY @@ -741,6 +747,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_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_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; 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"