Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-16 12:55:16 +03:00
Code Issues Wiki Activity

Switch Control Derivative to PT2 filter

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2021-06-26 14:30:48 +02:00
parent 92bc8364ce
commit e0860d1fa8
1 changed files with 3 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

6
src/main/flight/pid.c
View file

@ -110,7 +110,7 @@ typedef struct {
bool itermLimitActive; bool itermLimitActive;
bool itermFreezeActive; bool itermFreezeActive;
biquadFilter_t rateTargetFilter; pt2Filter_t rateTargetFilter;
smithPredictor_t smithPredictor; smithPredictor_t smithPredictor;
} pidState_t; } pidState_t;
@ -356,7 +356,7 @@ bool pidInitFilters(void)
if (pidProfile()->controlDerivativeLpfHz) { if (pidProfile()->controlDerivativeLpfHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { 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 newDTerm = dTermProcess(pidState, dT);
const float rateTargetDelta = pidState->rateTarget - pidState->previousRateTarget; const float rateTargetDelta = pidState->rateTarget - pidState->previousRateTarget;
const float rateTargetDeltaFiltered = biquadFilterApply(&pidState->rateTargetFilter, rateTargetDelta); const float rateTargetDeltaFiltered = pt2FilterApply(&pidState->rateTargetFilter, rateTargetDelta);
/* /*
* Compute Control Derivative * Compute Control Derivative