1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +03:00

Merge pull request #7006 from etracer65/dynfilter_throttle_update_optimize

Optimize throttle-based dynamic filter cutoff updates
This commit is contained in:
Michael Keller 2018-11-04 13:54:52 +13:00 committed by GitHub
commit a815486d56
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23

View file

@ -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,
@ -772,6 +776,26 @@ void applyMotorStop(void)
}
}
#ifdef USE_DYN_LPF
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()) {
@ -860,9 +884,9 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa
}
pidUpdateAntiGravityThrottleFilter(throttle);
#ifdef USE_DYN_LPF
dynLpfGyroUpdate(throttle);
dynLpfDTermUpdate(throttle);
updateDynLpfCutoffs(currentTimeUs, throttle);
#endif
#if defined(USE_THROTTLE_BOOST)