From cbb1ac02cf4506d8b77c35e65158a8aba5786f62 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Sun, 28 Oct 2018 19:00:05 -0400 Subject: [PATCH] Optimize throttle-based dynamic filter cutoff updates Current logic produces excessive load because it updates the filter cutoffs every PID loop based on the throttle value. The throttle values only change based on receive RX packets so they change relatively infrequently compared to the PID loop. However with rc smoothing on throttle the value can change every PID loop. But in reality we don't really need to adjust the filter cutoffs for every tiny change to the floating point throttle value. This change quantizes the throttle in to 100 steps and uses that to compare to the previous value to decide if the filter cutoffs need to be updated. While this reduces the resolution of the filter cutoffs it in turn dramatically reduces the processing overhead. IF needed the quantization steps can be increased for more resolution but at the cost of some performance. --- src/main/flight/mixer.c | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) 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)