rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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()`.
This commit is contained in:
Bruce Luckcuck 2018-11-01 19:32:28 -04:00
parent cbb1ac02cf
commit 957561c14c
1 changed files with 24 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
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,
@ -745,13 +749,28 @@ void applyMotorStop(void)
} }
} }
FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation)
{
#ifdef USE_DYN_LPF #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 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 #endif
FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation)
{
if (isFlipOverAfterCrashMode()) { if (isFlipOverAfterCrashMode()) {
applyFlipOverAfterCrashModeToMotors(); applyFlipOverAfterCrashModeToMotors();
return; return;
@ -820,15 +839,9 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa
} }
pidUpdateAntiGravityThrottleFilter(throttle); pidUpdateAntiGravityThrottleFilter(throttle);
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
const int quantizedThrottle = lrintf(throttle * DYN_LPF_THROTTLE_STEPS); // quantize the throttle reduce the number of filter updates updateDynLpfCutoffs(currentTimeUs, throttle);
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 #endif
#if defined(USE_THROTTLE_BOOST) #if defined(USE_THROTTLE_BOOST)