From e0860d1fa86a30c0f99b7fc8ae7eff0e8b54a107 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 26 Jun 2021 14:30:48 +0200 Subject: [PATCH] Switch Control Derivative to PT2 filter --- src/main/flight/pid.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index d54f5ff443..146d53f177 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -110,7 +110,7 @@ typedef struct { bool itermLimitActive; bool itermFreezeActive; - biquadFilter_t rateTargetFilter; + pt2Filter_t rateTargetFilter; smithPredictor_t smithPredictor; } pidState_t; @@ -356,7 +356,7 @@ bool pidInitFilters(void) if (pidProfile()->controlDerivativeLpfHz) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - biquadFilterInitLPF(&pidState[axis].rateTargetFilter, pidProfile()->controlDerivativeLpfHz, getLooptime()); + pt2FilterInit(&pidState[axis].rateTargetFilter, pt2FilterGain(pidProfile()->controlDerivativeLpfHz, refreshRate * 1e-6f)); } } @@ -816,7 +816,7 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid const float newDTerm = dTermProcess(pidState, dT); const float rateTargetDelta = pidState->rateTarget - pidState->previousRateTarget; - const float rateTargetDeltaFiltered = biquadFilterApply(&pidState->rateTargetFilter, rateTargetDelta); + const float rateTargetDeltaFiltered = pt2FilterApply(&pidState->rateTargetFilter, rateTargetDelta); /* * Compute Control Derivative