From fefedbd686ab6f116ec0d31bc2c697483eb4378c Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Tue, 5 Jun 2018 13:33:20 +0200 Subject: [PATCH 1/4] attempts to improve relax attempts to improve relax fix attempts to improve relax fix bug fixes spaces changed settings and added isAirmodeActive() fix rebase error --- src/main/fc/fc_core.c | 9 ++- src/main/fc/fc_core.h | 1 + src/main/flight/pid.c | 139 +++++++++++++++++++--------------- src/main/flight/pid.h | 10 ++- src/main/interface/settings.c | 8 +- src/main/interface/settings.h | 1 + 6 files changed, 101 insertions(+), 67 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 748020edaa..d0aaf44fcd 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -537,6 +537,14 @@ uint8_t calculateThrottlePercent(void) return ret; } +static bool airmodeIsActivated; + +bool isAirmodeActivated() +{ + return airmodeIsActivated; +} + + /* * processRx called from taskUpdateRxMain @@ -544,7 +552,6 @@ uint8_t calculateThrottlePercent(void) bool processRx(timeUs_t currentTimeUs) { static bool armedBeeperOn = false; - static bool airmodeIsActivated; static bool sharedPortTelemetryEnabled = false; if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs)) { diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 95d9655857..e2ed7fcc9a 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -52,3 +52,4 @@ void taskMainPidLoop(timeUs_t currentTimeUs); bool isFlipOverAfterCrashMode(void); void runawayTakeoffTemporaryDisable(uint8_t disableFlag); +bool isAirmodeActivated(); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e44b470766..854981704b 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -147,8 +147,8 @@ void resetPidProfile(pidProfile_t *pidProfile) .iterm_rotation = false, .smart_feedforward = false, .iterm_relax = ITERM_RELAX_OFF, - .iterm_relax_cutoff_low = 3, - .iterm_relax_cutoff_high = 30, + .iterm_relax_cutoff = 11, + .iterm_relax_type = ITERM_RELAX_GYRO, .acro_trainer_angle_limit = 20, .acro_trainer_lookahead_ms = 50, .acro_trainer_debug_axis = FD_ROLL, @@ -206,10 +206,10 @@ static FAST_RAM_ZERO_INIT pt1Filter_t dtermLowpass2[2]; static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn; static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass; #if defined(USE_ITERM_RELAX) -static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[XYZ_AXIS_COUNT][2]; +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 itermRelaxCutoffLow; -static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffHigh; +static FAST_RAM_ZERO_INIT uint8_t itermRelaxType; +static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoff; #endif #ifdef USE_RC_SMOOTHING_FILTER @@ -300,8 +300,7 @@ void pidInitFilters(const pidProfile_t *pidProfile) #if defined(USE_ITERM_RELAX) if (itermRelax) { for (int i = 0; i < XYZ_AXIS_COUNT; i++) { - pt1FilterInit(&windupLpf[i][0], pt1FilterGain(itermRelaxCutoffLow, dT)); - pt1FilterInit(&windupLpf[i][1], pt1FilterGain(itermRelaxCutoffHigh, dT)); + pt1FilterInit(&windupLpf[i], pt1FilterGain(itermRelaxCutoff, dT)); } } #endif @@ -427,8 +426,8 @@ void pidInitConfig(const pidProfile_t *pidProfile) #endif #if defined(USE_ITERM_RELAX) itermRelax = pidProfile->iterm_relax; - itermRelaxCutoffLow = pidProfile->iterm_relax_cutoff_low; - itermRelaxCutoffHigh = pidProfile->iterm_relax_cutoff_high; + itermRelaxType = pidProfile->iterm_relax_type; + itermRelaxCutoff = pidProfile->iterm_relax_cutoff; #endif #ifdef USE_ACRO_TRAINER @@ -803,19 +802,77 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT } #endif // USE_YAW_SPIN_RECOVERY -#if defined(USE_ABSOLUTE_CONTROL) - // mix in a correction for accrued attitude error - float acCorrection = 0; - if (acGain > 0) { - acCorrection = constrainf(axisError[axis] * acGain, -acLimit, acLimit); - currentPidSetpoint += acCorrection; - } -#endif - // -----calculate error rate const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec - float errorRate = currentPidSetpoint - gyroRate; // r - y +#ifdef USE_ABSOLUTE_CONTROL + float acCorrection = 0; + float acErrorRate; +#endif + float itermErrorRate = 0.0f; + +#if defined(USE_ITERM_RELAX) + float stickSetpoint = currentPidSetpoint; + if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) { + const float gyroTarget = pt1FilterApply(&windupLpf[axis], stickSetpoint); + const float gyroHpf = fabsf(stickSetpoint - gyroTarget); + if (itermRelaxType == ITERM_RELAX_SETPOINT) { + itermErrorRate = (1 - MIN(1, fabsf(gyroHpf) / 60.0f)) * (stickSetpoint - gyroRate); + } + const float tolerance = gyroHpf * 1.0f; + if (axis == FD_ROLL) { + DEBUG_SET(DEBUG_ITERM_RELAX, 0, gyroTarget); + DEBUG_SET(DEBUG_ITERM_RELAX, 1, gyroTarget + tolerance); + DEBUG_SET(DEBUG_ITERM_RELAX, 2, gyroTarget - tolerance); + DEBUG_SET(DEBUG_ITERM_RELAX, 3, axisError[axis] * 10); + } + if (itermRelaxType == ITERM_RELAX_GYRO ) { + const float gmax = gyroTarget + tolerance; + const float gmin = gyroTarget - tolerance; + + if (gyroRate >= gmin && gyroRate <= gmax) { + itermErrorRate = 0; + } else { + itermErrorRate = (gyroRate > gmax ? gmax : gmin ) - gyroRate; + } + } + +#if defined(USE_ABSOLUTE_CONTROL) + const float gmaxac = gyroTarget + 2 * tolerance; + const float gminac = gyroTarget - 2 * tolerance; + if (gyroRate >= gminac && gyroRate <= gmaxac) { + float acErrorRate1 = gmaxac - gyroRate; + float acErrorRate2 = gminac - gyroRate; + if (acErrorRate1 * axisError[axis] < 0) { + acErrorRate = acErrorRate1; + } else { + acErrorRate = acErrorRate2; + } + if (fabsf(acErrorRate*dT) > fabsf(axisError[axis]) ) { + acErrorRate = -axisError[axis]/dT; + } + } else { + acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate; + } +#endif // USE_ABSOLUTE_CONTROL + } else +#endif // USE_ITERM_RELAX + { + itermErrorRate = currentPidSetpoint - gyroRate; +#if defined(USE_ABSOLUTE_CONTROL) + acErrorRate = itermErrorRate; +#endif // USE_ABSOLUTE_CONTROL + } + +#if defined(USE_ABSOLUTE_CONTROL) + if (acGain > 0 && isAirmodeActivated()) { + axisError[axis] = constrainf(axisError[axis] + acErrorRate * dT, -acErrorLimit, acErrorLimit); + acCorrection = constrainf(axisError[axis] * acGain, -acLimit, acLimit); + currentPidSetpoint += acCorrection; + itermErrorRate += acCorrection; + } +#endif + float errorRate = currentPidSetpoint - gyroRate; // r - y handleCrashRecovery( pidProfile->crash_recovery, angleTrim, axis, currentTimeUs, gyroRate, ¤tPidSetpoint, &errorRate); @@ -831,49 +888,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT } // -----calculate I component - float itermErrorRate = 0.0f; -#if defined(USE_ABSOLUTE_CONTROL) - float acErrorRate; -#endif -#if defined(USE_ITERM_RELAX) - if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) { - float gyroTargetLow = pt1FilterApply(&windupLpf[axis][0], currentPidSetpoint); - float gyroTargetHigh = pt1FilterApply(&windupLpf[axis][1], currentPidSetpoint); -#if defined(USE_ABSOLUTE_CONTROL) - gyroTargetLow += acCorrection; - gyroTargetHigh += acCorrection; -#endif - 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 = (gyroRate > gmax ? gmax : gmin ) - gyroRate; - } -#if defined(USE_ABSOLUTE_CONTROL) - else { - itermErrorRate = acCorrection; - } - acErrorRate = itermErrorRate - acCorrection; -#endif - } else -#endif // USE_ITERM_RELAX - { - itermErrorRate = errorRate; -#if defined(USE_ABSOLUTE_CONTROL) - acErrorRate = errorRate; -#endif - } - -#if defined(USE_ABSOLUTE_CONTROL) - if (acGain > 0) { - axisError[axis] = constrainf(axisError[axis] + acErrorRate * dT, -acErrorLimit, acErrorLimit); - } -#endif - + const float ITerm = pidData[axis].I; const float ITermNew = constrainf(ITerm + pidCoefficient[axis].Ki * itermErrorRate * dynCi, -itermLimit, itermLimit); const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index cb53cea862..5d873d363d 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -83,6 +83,12 @@ typedef enum ITERM_RELAX_RPY } itermRelax_e; +typedef enum +{ + ITERM_RELAX_GYRO, + ITERM_RELAX_SETPOINT +} itermRelaxType_e; + typedef struct pidProfile_s { uint16_t yaw_lowpass_hz; // Additional yaw filter when yaw axis too noisy uint16_t dterm_lowpass_hz; // Delta Filter in hz @@ -124,8 +130,8 @@ 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_type; // Specifies type of relax algorithm + uint8_t iterm_relax_cutoff; // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint uint8_t iterm_relax; // Enable iterm suppression during stick input uint8_t acro_trainer_angle_limit; // Acro trainer roll/pitch angle limit in degrees uint8_t acro_trainer_debug_axis; // The axis for which record debugging values are captured 0=roll, 1=pitch diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 29044b276f..45fb4cd349 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -341,6 +341,9 @@ static const char * const lookupTableVideoSystem[] = { static const char * const lookupTableItermRelax[] = { "OFF", "RP", "RPY" }; +static const char * const lookupTableItermRelaxType[] = { + "GYRO", "SETPOINT" +}; #endif #ifdef USE_ACRO_TRAINER @@ -441,6 +444,7 @@ const lookupTableEntry_t lookupTables[] = { #endif // USE_MAX7456 #if defined(USE_ITERM_RELAX) LOOKUP_TABLE_ENTRY(lookupTableItermRelax), + LOOKUP_TABLE_ENTRY(lookupTableItermRelaxType), #endif #ifdef USE_ACRO_TRAINER LOOKUP_TABLE_ENTRY(lookupTableAcroTrainerDebug), @@ -801,9 +805,9 @@ const clivalue_t valueTable[] = { #endif #if defined(USE_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_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_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 ba0a8727c9..cbcc60266f 100644 --- a/src/main/interface/settings.h +++ b/src/main/interface/settings.h @@ -100,6 +100,7 @@ typedef enum { #endif // USE_MAX7456 #if defined(USE_ITERM_RELAX) TABLE_ITERM_RELAX, + TABLE_ITERM_RELAX_TYPE, #endif #ifdef USE_ACRO_TRAINER TABLE_ACRO_TRAINER_DEBUG, From db3c0d1447237a6faeb3eff025e57b3b71f4856e Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Fri, 8 Jun 2018 21:18:54 +1200 Subject: [PATCH 2/4] Some fixes. --- src/main/flight/pid.c | 11 +++++------ src/main/flight/pid.h | 6 ++---- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 854981704b..913a2be853 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -812,12 +812,11 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT float itermErrorRate = 0.0f; #if defined(USE_ITERM_RELAX) - float stickSetpoint = currentPidSetpoint; if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) { - const float gyroTarget = pt1FilterApply(&windupLpf[axis], stickSetpoint); - const float gyroHpf = fabsf(stickSetpoint - gyroTarget); + const float gyroTarget = pt1FilterApply(&windupLpf[axis], currentPidSetpoint); + const float gyroHpf = fabsf(currentPidSetpoint - gyroTarget); if (itermRelaxType == ITERM_RELAX_SETPOINT) { - itermErrorRate = (1 - MIN(1, fabsf(gyroHpf) / 60.0f)) * (stickSetpoint - gyroRate); + itermErrorRate = (1 - MIN(1, fabsf(gyroHpf) / 60.0f)) * (currentPidSetpoint - gyroRate); } const float tolerance = gyroHpf * 1.0f; if (axis == FD_ROLL) { @@ -831,9 +830,9 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT const float gmin = gyroTarget - tolerance; if (gyroRate >= gmin && gyroRate <= gmax) { - itermErrorRate = 0; + itermErrorRate = 0; } else { - itermErrorRate = (gyroRate > gmax ? gmax : gmin ) - gyroRate; + itermErrorRate = (gyroRate > gmax ? gmax : gmin ) - gyroRate; } } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 5d873d363d..d99e870824 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -76,15 +76,13 @@ typedef struct pid8_s { uint8_t D; } pid8_t; -typedef enum -{ +typedef enum { ITERM_RELAX_OFF, ITERM_RELAX_RP, ITERM_RELAX_RPY } itermRelax_e; -typedef enum -{ +typedef enum { ITERM_RELAX_GYRO, ITERM_RELAX_SETPOINT } itermRelaxType_e; From d43c1784881acee1d8cde8c4e99b3d570a9478f1 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sat, 9 Jun 2018 13:11:37 +1200 Subject: [PATCH 3/4] Fixes from review. --- src/main/common/maths.c | 12 ++++++++++++ src/main/common/maths.h | 1 + src/main/flight/pid.c | 21 +++++++-------------- 3 files changed, 20 insertions(+), 14 deletions(-) diff --git a/src/main/common/maths.c b/src/main/common/maths.c index e7813b132e..dd443ee80b 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -130,6 +130,18 @@ int32_t applyDeadband(int32_t value, int32_t deadband) return value; } +float fapplyDeadband(float value, float deadband) +{ + if (fabsf(value) < deadband) { + value = 0; + } else if (value > 0) { + value -= deadband; + } else if (value < 0) { + value += deadband; + } + return value; +} + void devClear(stdev_t *dev) { dev->m_n = 0; diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 300c18fae6..19842ebb40 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -91,6 +91,7 @@ typedef union { int gcd(int num, int denom); float powerf(float base, int exp); int32_t applyDeadband(int32_t value, int32_t deadband); +float fapplyDeadband(float value, float deadband); void devClear(stdev_t *dev); void devPush(stdev_t *dev, float x); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 913a2be853..48c501d524 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -815,8 +815,8 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) { const float gyroTarget = pt1FilterApply(&windupLpf[axis], currentPidSetpoint); const float gyroHpf = fabsf(currentPidSetpoint - gyroTarget); - if (itermRelaxType == ITERM_RELAX_SETPOINT) { - itermErrorRate = (1 - MIN(1, fabsf(gyroHpf) / 60.0f)) * (currentPidSetpoint - gyroRate); + if (itermRelaxType == ITERM_RELAX_SETPOINT && gyroHpf < 60) { + itermErrorRate = (1 - gyroHpf / 60.0f) * (currentPidSetpoint - gyroRate); } const float tolerance = gyroHpf * 1.0f; if (axis == FD_ROLL) { @@ -826,14 +826,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT DEBUG_SET(DEBUG_ITERM_RELAX, 3, axisError[axis] * 10); } if (itermRelaxType == ITERM_RELAX_GYRO ) { - const float gmax = gyroTarget + tolerance; - const float gmin = gyroTarget - tolerance; - - if (gyroRate >= gmin && gyroRate <= gmax) { - itermErrorRate = 0; - } else { - itermErrorRate = (gyroRate > gmax ? gmax : gmin ) - gyroRate; - } + itermErrorRate = fapplyDeadband(gyroTarget - gyroRate, tolerance); } #if defined(USE_ABSOLUTE_CONTROL) @@ -847,8 +840,8 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT } else { acErrorRate = acErrorRate2; } - if (fabsf(acErrorRate*dT) > fabsf(axisError[axis]) ) { - acErrorRate = -axisError[axis]/dT; + if (fabsf(acErrorRate * dT) > fabsf(axisError[axis]) ) { + acErrorRate = -axisError[axis] / dT; } } else { acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate; @@ -857,9 +850,9 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT } else #endif // USE_ITERM_RELAX { - itermErrorRate = currentPidSetpoint - gyroRate; + itermErrorRate = currentPidSetpoint - gyroRate; #if defined(USE_ABSOLUTE_CONTROL) - acErrorRate = itermErrorRate; + acErrorRate = itermErrorRate; #endif // USE_ABSOLUTE_CONTROL } From 06428c1b057ee8e36b31c25d1eb8f912c09e95a9 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sat, 9 Jun 2018 19:20:46 +1200 Subject: [PATCH 4/4] More fixes. --- src/main/flight/pid.c | 23 +++++++++++------------ 1 file changed, 11 insertions(+), 12 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 48c501d524..4707b74abf 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -813,25 +813,24 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT #if defined(USE_ITERM_RELAX) if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) { - const float gyroTarget = pt1FilterApply(&windupLpf[axis], currentPidSetpoint); - const float gyroHpf = fabsf(currentPidSetpoint - gyroTarget); - if (itermRelaxType == ITERM_RELAX_SETPOINT && gyroHpf < 60) { - itermErrorRate = (1 - gyroHpf / 60.0f) * (currentPidSetpoint - gyroRate); + const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint); + const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf); + if (itermRelaxType == ITERM_RELAX_SETPOINT && setpointHpf < 60) { + itermErrorRate = (1 - setpointHpf / 60.0f) * (currentPidSetpoint - gyroRate); } - const float tolerance = gyroHpf * 1.0f; if (axis == FD_ROLL) { - DEBUG_SET(DEBUG_ITERM_RELAX, 0, gyroTarget); - DEBUG_SET(DEBUG_ITERM_RELAX, 1, gyroTarget + tolerance); - DEBUG_SET(DEBUG_ITERM_RELAX, 2, gyroTarget - tolerance); - DEBUG_SET(DEBUG_ITERM_RELAX, 3, axisError[axis] * 10); + DEBUG_SET(DEBUG_ITERM_RELAX, 0, lrintf(setpointLpf)); + DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(setpointHpf)); + DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(currentPidSetpoint)); + DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf(axisError[axis] * 10)); } if (itermRelaxType == ITERM_RELAX_GYRO ) { - itermErrorRate = fapplyDeadband(gyroTarget - gyroRate, tolerance); + itermErrorRate = fapplyDeadband(setpointLpf - gyroRate, setpointHpf); } #if defined(USE_ABSOLUTE_CONTROL) - const float gmaxac = gyroTarget + 2 * tolerance; - const float gminac = gyroTarget - 2 * tolerance; + const float gmaxac = setpointLpf + 2 * setpointHpf; + const float gminac = setpointLpf - 2 * setpointHpf; if (gyroRate >= gminac && gyroRate <= gmaxac) { float acErrorRate1 = gmaxac - gyroRate; float acErrorRate2 = gminac - gyroRate;