Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 06:15:16 +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

1
src/main/cli/settings.c
View file

@ -907,6 +907,7 @@ const clivalue_t valueTable[] = {
// PG_MIXER_CONFIG
{ "yaw_motors_reversed", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, yaw_motors_reversed) },
{ "linear_mixer", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, linear_mixer) },
{ "crashflip_motor_percent", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, crashflip_motor_percent) },
{ "crashflip_expo", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, crashflip_expo) },

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

1
src/main/flight/mixer.h
View file

@ -82,6 +82,7 @@ typedef struct mixerConfig_s {
bool yaw_motors_reversed;
uint8_t crashflip_motor_percent;
uint8_t crashflip_expo;
uint8_t linear_mixer;
} mixerConfig_t;
PG_DECLARE(mixerConfig_t, mixerConfig);

3
src/main/flight/mixer_init.c
View file

@ -50,7 +50,8 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
.mixerMode = DEFAULT_MIXER,
.yaw_motors_reversed = false,
.crashflip_motor_percent = 0,
.crashflip_expo = 35
.crashflip_expo = 35,
.linear_mixer = false,
);
PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR_MIXER, 0);