From cbb1ac02cf4506d8b77c35e65158a8aba5786f62 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Sun, 28 Oct 2018 19:00:05 -0400 Subject: [PATCH 1/2] 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) From 957561c14c6242cb69aa231e1618c6e4d10b9f6e Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Thu, 1 Nov 2018 19:32:28 -0400 Subject: [PATCH 2/2] Add minimum time interval for updates and move into separate function Prevents updates more often than 5ms even if throttle is very active. Separated the logic into its own function to tidy up `mixTable()`. --- src/main/flight/mixer.c | 35 ++++++++++++++++++++++++----------- 1 file changed, 24 insertions(+), 11 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index defdf00a60..d17b1a7742 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -69,6 +69,10 @@ PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0); #ifndef TARGET_DEFAULT_MIXER #define TARGET_DEFAULT_MIXER MIXER_QUADX #endif + +#define DYN_LPF_THROTTLE_STEPS 100 +#define DYN_LPF_THROTTLE_UPDATE_DELAY_US 5000 // minimum of 5ms between updates + PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, .mixerMode = TARGET_DEFAULT_MIXER, .yaw_motors_reversed = false, @@ -745,13 +749,28 @@ void applyMotorStop(void) } } -FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation) -{ #ifdef USE_DYN_LPF -#define DYN_LPF_THROTTLE_STEPS 100 +void updateDynLpfCutoffs(timeUs_t currentTimeUs, float throttle) +{ + static timeUs_t lastDynLpfUpdateUs = 0; static int dynLpfPreviousQuantizedThrottle = -1; // to allow an initial zero throttle to set the filter cutoff + + if (cmpTimeUs(currentTimeUs, lastDynLpfUpdateUs) >= DYN_LPF_THROTTLE_UPDATE_DELAY_US) { + 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; + lastDynLpfUpdateUs = currentTimeUs; + } + } +} #endif +FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation) +{ if (isFlipOverAfterCrashMode()) { applyFlipOverAfterCrashModeToMotors(); return; @@ -820,15 +839,9 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa } pidUpdateAntiGravityThrottleFilter(throttle); + #ifdef USE_DYN_LPF - 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; - } + updateDynLpfCutoffs(currentTimeUs, throttle); #endif #if defined(USE_THROTTLE_BOOST)