diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c index feb71ec1b3..a95cab2555 100644 --- a/src/main/flight/rpm_filter.c +++ b/src/main/flight/rpm_filter.c @@ -121,6 +121,9 @@ FAST_CODE_NOINLINE void rpmFilterUpdate(void) return; } + const float dtCompensation = schedulerGetCycleTimeMultiplier(); + const float correctedLooptime = rpmFilter.looptimeUs * dtCompensation; + // update RPM notches for (int i = 0; i < notchUpdatesPerIteration; i++) { @@ -143,7 +146,7 @@ FAST_CODE_NOINLINE void rpmFilterUpdate(void) weight *= rpmFilter.weights[harmonicIndex]; // update notch - biquadFilterUpdate(template, frequencyHz, rpmFilter.looptimeUs, rpmFilter.q, FILTER_NOTCH, weight); + biquadFilterUpdate(template, frequencyHz, correctedLooptime, rpmFilter.q, FILTER_NOTCH, weight); // copy notch properties to corresponding notches on PITCH and YAW for (int axis = 1; axis < XYZ_AXIS_COUNT; axis++) {