Skip to content

Commit

Permalink
Optimize throttle-based dynamic filter cutoff updates
Browse files Browse the repository at this point in the history
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.
  • Loading branch information
etracer65 committed Oct 28, 2018
1 parent 71c563f commit cbb1ac0
Showing 1 changed file with 13 additions and 2 deletions.
15 changes: 13 additions & 2 deletions src/main/flight/mixer.c
Expand Up @@ -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;
Expand Down Expand Up @@ -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)
Expand Down

0 comments on commit cbb1ac0

Please sign in to comment.