From b4130ee726bde7bd167b2e1bcdb55361a30c84d7 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 13 Nov 2015 11:50:32 +0100 Subject: [PATCH] Revert "Remove YawPtermState filter" This reverts commit 1a667442f2dd5d59630b83bb01140f42f2ded89c. --- src/main/common/filter.h | 2 +- src/main/flight/pid.c | 9 +++++++++ 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 6f3b58854..be3bacd85 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 ef92375ce..24616c95c 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -96,6 +96,7 @@ 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) @@ -172,6 +173,10 @@ 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); @@ -284,6 +289,10 @@ 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.