diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 99dca93712..78c2cc13f2 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -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) }, diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 289c5ef969..597e17b3aa 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -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 diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 61e4785d86..b3031a6e1c 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -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); diff --git a/src/main/flight/mixer_init.c b/src/main/flight/mixer_init.c index 93f326d8a2..830d2fe2a3 100644 --- a/src/main/flight/mixer_init.c +++ b/src/main/flight/mixer_init.c @@ -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);