Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 21:35:44 +03:00
Code Issues Wiki Activity

Merge pull request #7006 from etracer65/dynfilter_throttle_update_optimize

Browse source 
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
1 changed files with 26 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

28
src/main/flight/mixer.c
View file

@ -69,6 +69,10 @@ PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0);
#ifndef TARGET_DEFAULT_MIXER #ifndef TARGET_DEFAULT_MIXER
#define TARGET_DEFAULT_MIXER MIXER_QUADX #define TARGET_DEFAULT_MIXER MIXER_QUADX
#endif #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, PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
.mixerMode = TARGET_DEFAULT_MIXER, .mixerMode = TARGET_DEFAULT_MIXER,
.yaw_motors_reversed = false, .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) FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation)
{ {
if (isFlipOverAfterCrashMode()) { if (isFlipOverAfterCrashMode()) {
@ -860,9 +884,9 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa
} }
pidUpdateAntiGravityThrottleFilter(throttle); pidUpdateAntiGravityThrottleFilter(throttle);
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
dynLpfGyroUpdate(throttle); updateDynLpfCutoffs(currentTimeUs, throttle);
dynLpfDTermUpdate(throttle);
#endif #endif
#if defined(USE_THROTTLE_BOOST) #if defined(USE_THROTTLE_BOOST)