Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 08:15:30 +03:00
Code Issues Wiki Activity

New linear mixer (credits to @tylercorleone)

Browse source
This commit is contained in:
borisbstyle 2020-11-24 21:59:39 +01:00 • committed by Michael Keller
parent 7154abc48e
commit 75da17a00e
4 changed files with 48 additions and 26 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -425,6 +425,47 @@ static void updateDynLpfCutoffs(timeUs_t currentTimeUs, float throttle)
}
#endif
static void applyLinearMixerAdjustment(float *motorMix, float motorMixRange, bool airmodeEnabled) {
float motorMixNormalizationFactor = motorMixRange > 1.0f ? motorMixRange : 1.0f;
float motorMixDelta = 0.5f * motorMixRange;
for (int i = 0; i < mixerRuntime.motorCount; ++i) {
if (airmodeEnabled || throttle > 0.5f) {
motorMix[i] = scaleRangef(throttle, 0.0f, 1.0f, motorMix[i] + motorMixDelta, motorMix[i] - motorMixDelta);
}
motorMix[i] /= motorMixNormalizationFactor;
}
}
static void applyMixerAdjustment(float *motorMix, float motorMixMin, float motorMixMax, float motorMixRange, bool airmodeEnabled) {
#ifdef USE_AIRMODE_LPF
const float unadjustedThrottle = throttle;
throttle += pidGetAirmodeThrottleOffset();
float airmodeThrottleChange = 0;
#endif
if (motorMixRange > 1.0f) {
for (int i = 0; i < mixerRuntime.motorCount; i++) {
motorMix[i] /= motorMixRange;
}
// Get the maximum correction by setting offset to center when airmode enabled
if (airmodeEnabled) {
throttle = 0.5f;
}
} else {
if (airmodeEnabled || throttle > 0.5f) {
throttle = constrainf(throttle, -motorMixMin, 1.0f - motorMixMax);
#ifdef USE_AIRMODE_LPF
airmodeThrottleChange = constrainf(unadjustedThrottle, -motorMixMin, 1.0f - motorMixMax) - unadjustedThrottle;
#endif
}
}
#ifdef USE_AIRMODE_LPF
pidUpdateAirmodeLpf(airmodeThrottleChange);
#endif
}
FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
{
// Find min and max throttle based on conditions. Throttle has to be known before mixing
@ -549,35 +590,13 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
}
#endif
#ifdef USE_AIRMODE_LPF
const float unadjustedThrottle = throttle;
throttle += pidGetAirmodeThrottleOffset();
float airmodeThrottleChange = 0;
#endif
// apply airmode
motorMixRange = motorMixMax - motorMixMin;
if (motorMixRange > 1.0f) {
for (int i = 0; i < mixerRuntime.motorCount; i++) {
motorMix[i] /= motorMixRange;
}
// Get the maximum correction by setting offset to center when airmode enabled
if (airmodeEnabled) {
throttle = 0.5f;
}
if (mixerConfig()->linear_mixer) {
applyLinearMixerAdjustment(motorMix, motorMixRange, airmodeEnabled);
} else {
if (airmodeEnabled || throttle > 0.5f) { // Only automatically adjust throttle when airmode enabled. Airmode logic is always active on high throttle
throttle = constrainf(throttle, -motorMixMin, 1.0f - motorMixMax);
#ifdef USE_AIRMODE_LPF
airmodeThrottleChange = constrainf(unadjustedThrottle, -motorMixMin, 1.0f - motorMixMax) - unadjustedThrottle;
#endif
}
applyMixerAdjustment(motorMix, motorMixMin, motorMixMax, motorMixRange, airmodeEnabled);
}
#ifdef USE_AIRMODE_LPF
pidUpdateAirmodeLpf(airmodeThrottleChange);
#endif
if (featureIsEnabled(FEATURE_MOTOR_STOP)
&& ARMING_FLAG(ARMED)
&& !mixerRuntime.feature3dEnabled