diff --git a/src/main/common/filter.h b/src/main/common/filter.h index be3bacd85..6f3b58854 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -5,7 +5,7 @@ * Author: borisb */ -#define YAW_PTERM_FILTER 30 + typedef struct filterStatePt1_s { float state; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 24616c95c..ef92375ce 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -96,7 +96,6 @@ void pidResetErrorGyro(void) const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; static filterStatePt1_t DTermState[3]; -static filterStatePt1_t yawPTermState; static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) @@ -173,10 +172,6 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // -----calculate P component PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100; - if (axis == YAW) { - PTerm = filterApplyPt1(PTerm, &yawPTermState, YAW_PTERM_FILTER, dT); - } - // -----calculate I component. errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f); @@ -289,10 +284,6 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat // -----calculate P component PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; - if (axis == YAW) { - PTerm = filterApplyPt1(PTerm, &yawPTermState, YAW_PTERM_FILTER, dT); - } - // -----calculate I component // there should be no division before accumulating the error to integrator, because the precision would be reduced. // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.