diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 33df10d431..defdf00a60 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -747,6 +747,11 @@ void applyMotorStop(void) FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation) { +#ifdef USE_DYN_LPF +#define DYN_LPF_THROTTLE_STEPS 100 + static int dynLpfPreviousQuantizedThrottle = -1; // to allow an initial zero throttle to set the filter cutoff +#endif + if (isFlipOverAfterCrashMode()) { applyFlipOverAfterCrashModeToMotors(); return; @@ -816,8 +821,14 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa pidUpdateAntiGravityThrottleFilter(throttle); #ifdef USE_DYN_LPF - dynLpfGyroUpdate(throttle); - dynLpfDTermUpdate(throttle); + const int quantizedThrottle = lrintf(throttle * DYN_LPF_THROTTLE_STEPS); // quantize the throttle reduce the number of filter updates + if (quantizedThrottle != dynLpfPreviousQuantizedThrottle) { + // scale the quantized value back to the throttle range so the filter cutoff steps are repeatable + const float dynLpfThrottle = (float)quantizedThrottle / DYN_LPF_THROTTLE_STEPS; + dynLpfGyroUpdate(dynLpfThrottle); + dynLpfDTermUpdate(dynLpfThrottle); + dynLpfPreviousQuantizedThrottle = quantizedThrottle; + } #endif #if defined(USE_THROTTLE_BOOST)