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:
commit
a815486d56
1 changed files with 26 additions and 2 deletions
|
@ -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)
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue