From 22e8a0a417df30544dd55d3ae964de05dc0f3446 Mon Sep 17 00:00:00 2001 From: Tobias Bolin Date: Tue, 22 Nov 2022 22:45:36 +0100 Subject: [PATCH] Add ez_landing throttle mode --- src/main/cli/settings.c | 2 +- src/main/flight/mixer.c | 88 ++++++++++++++++++++++++++++-------- src/main/flight/mixer.h | 2 + src/main/flight/mixer_init.c | 12 +++-- 4 files changed, 82 insertions(+), 22 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 4a6189aa51a..a42ab2ad87d 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -497,7 +497,7 @@ const char * const lookupTableSimplifiedTuningPidsMode[] = { }; static const char* const lookupTableMixerType[] = { - "LEGACY", "LINEAR", "DYNAMIC", + "LEGACY", "LINEAR", "DYNAMIC", "EZLANDING_THROTTLE", "EZLANDING_CLIP" }; #ifdef USE_OSD diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 3275ca1df7e..1e679dcd6cb 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -99,6 +99,21 @@ void stopMotors(void) delay(50); // give the timers and ESCs a chance to react. } +static float calcEzLandStrength(float throttlePercent, float maxDeflection) +{ + // easy landing code, limits motor output when sticks and throttle are below threshold + float ezLandAttenuator = 0.0f; + if (throttlePercent < mixerRuntime.ezLandingThreshold + && maxDeflection < mixerRuntime.ezLandingThreshold) { // throttle low + // all sticks, including throttle, under threshold + ezLandAttenuator = fmaxf(maxDeflection, throttlePercent); // value range 0 -> threshold + ezLandAttenuator /= mixerRuntime.ezLandingThreshold; // normalised 0 - 1 + ezLandAttenuator = 1.0f - ezLandAttenuator; // 1 -> 0 + ezLandAttenuator *= mixerRuntime.ezLandingLimit; // eg 0.9 -> 0.0 if limit is 10 + } + return constrainf(ezLandAttenuator, 0.0f, 1.0f); +} + static FAST_DATA_ZERO_INIT float throttle = 0; static FAST_DATA_ZERO_INIT float mixerThrottle = 0; static FAST_DATA_ZERO_INIT float motorOutputMin; @@ -257,28 +272,17 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) #else motorRangeMax = mixerRuntime.motorOutputHigh; #endif - motorRangeMin = mixerRuntime.motorOutputLow + motorRangeMinIncrease * (mixerRuntime.motorOutputHigh - mixerRuntime.motorOutputLow); motorOutputMin = motorRangeMin; motorOutputRange = motorRangeMax - motorRangeMin; motorOutputMixSign = 1; - - // easy landing code, limits motor output when sticks and throttle are below threshold - const float throttlePercent = throttle / 1000.0f; - if (mixerRuntime.ezLandingThreshold && throttlePercent < mixerRuntime.ezLandingThreshold) { // throttle low - float ezLandAttenuator = 0.0f; - float maxDeflection = getMaxRcDeflectionAbs(); - if (maxDeflection < mixerRuntime.ezLandingThreshold) { // all sticks, including throttle, under threshold - ezLandAttenuator = fmaxf(maxDeflection, throttlePercent); // value range 0 -> threshold - ezLandAttenuator /= mixerRuntime.ezLandingThreshold; // normalised 0 - 1 - ezLandAttenuator = 1.0f - ezLandAttenuator; // 1 -> 0 - ezLandAttenuator *= mixerRuntime.ezLandingLimit; // eg 0.9 -> 0.0 if limit is 10 - } - const float motorRange = motorRangeMax - mixerRuntime.motorOutputLow; - motorRangeMax -= ezLandAttenuator * motorRange; // available motor range limited to 15% if threshold is 15 - } } + if (mixerConfig()->mixer_type == MIXER_EZLANDING_CLIP) { + const float motorRange = motorRangeMax - mixerRuntime.motorOutputLow; + const float throttlePercentage = throttle/1000.f; + motorRangeMax -= calcEzLandStrength(throttlePercentage, getMaxRcDeflectionAbs()) * motorRange; + } throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f); } @@ -475,6 +479,41 @@ static void applyMixerAdjustmentLinear(float *motorMix, const bool airmodeEnable throttle = constrainf(throttle, -minMotor, 1.0f - maxMotor); } +static void applyMixerAdjustmentEzLand(float *motorMix, const float motorMixMin, const float motorMixMax) +{ + // Calculate the 'airmode throttle' as usual + const float motorMixNormalizationFactor = motorMixRange > 1.0f ? 1.0f / motorMixRange : 1.0f; + + for (int i = 0; i < mixerRuntime.motorCount; i++) { + motorMix[i] *= motorMixNormalizationFactor; + } + + const float normalizedMotorMixMin = motorMixMin * motorMixNormalizationFactor; + const float normalizedMotorMixMax = motorMixMax * motorMixNormalizationFactor; + motorMixRange *= motorMixNormalizationFactor; + + float airmodeThrottle = constrainf(throttle, -normalizedMotorMixMin, 1.0f - normalizedMotorMixMax); + + // Calculate throttle limit based on stick positions + float throttleLimit = 1.0f - calcEzLandStrength(0.0, getMaxRcDeflectionAbs()); + // Use the largest of throttle and limit calculated from stick positions + throttleLimit = fmaxf(throttle, throttleLimit); + + if (airmodeThrottle > throttleLimit) { + // Scale down motor mix from PIDs so that the limit is respected + const float ezLandCoefficient = throttleLimit / fabsf(normalizedMotorMixMin); + for (int i = 0; i < mixerRuntime.motorCount; i++) { + motorMix[i] *= ezLandCoefficient; + } + throttle = throttleLimit; + // Hack to make anti windup recognize the reduced authority range + motorMixRange /= ezLandCoefficient; + // end of hack + } else { + throttle = airmodeThrottle; + } +} + static void applyMixerAdjustment(float *motorMix, const float motorMixMin, const float motorMixMax, const bool airmodeEnabled) { #ifdef USE_AIRMODE_LPF @@ -634,10 +673,23 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) #endif motorMixRange = motorMixMax - motorMixMin; - if (mixerConfig()->mixer_type > MIXER_LEGACY) { + + switch (mixerConfig()->mixer_type) + { + case MIXER_LEGACY: + applyMixerAdjustment(motorMix, motorMixMin, motorMixMax, airmodeEnabled); + break; + case MIXER_LINEAR: + case MIXER_DYNAMIC: applyMixerAdjustmentLinear(motorMix, airmodeEnabled); - } else { + break; + case MIXER_EZLANDING_THROTTLE: + applyMixerAdjustmentEzLand(motorMix, motorMixMin, motorMixMax); + break; + case MIXER_EZLANDING_CLIP: + default: applyMixerAdjustment(motorMix, motorMixMin, motorMixMax, airmodeEnabled); + break; } if (featureIsEnabled(FEATURE_MOTOR_STOP) diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 9e7ea6712c1..3fcf5714c89 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -65,6 +65,8 @@ typedef enum mixerType MIXER_LEGACY = 0, MIXER_LINEAR = 1, MIXER_DYNAMIC = 2, + MIXER_EZLANDING_THROTTLE = 3, + MIXER_EZLANDING_CLIP = 4, } mixerType_e; // Custom mixer data per motor diff --git a/src/main/flight/mixer_init.c b/src/main/flight/mixer_init.c index 14df26c38ac..5befa64db41 100644 --- a/src/main/flight/mixer_init.c +++ b/src/main/flight/mixer_init.c @@ -322,9 +322,15 @@ void mixerInitProfile(void) } } #endif - - mixerRuntime.ezLandingThreshold = currentPidProfile->ez_landing_threshold / 100.0f; - mixerRuntime.ezLandingLimit = 1.0f - currentPidProfile->ez_landing_limit / 100.0f; + // Hack to make setings for CLIP and THROTTLE versions + // roughly equivalent while testing, remove if integrated in main + if (mixerConfig()->mixer_type == MIXER_EZLANDING_THROTTLE) { + mixerRuntime.ezLandingThreshold = (currentPidProfile->ez_landing_threshold / 100.0f) * 2.0f; + mixerRuntime.ezLandingLimit = 1.0f - (currentPidProfile->ez_landing_limit / 100.0f) * 0.5f; + } else { // This part should remain if integrated in main + mixerRuntime.ezLandingThreshold = currentPidProfile->ez_landing_threshold / 100.0f; + mixerRuntime.ezLandingLimit = 1.0f - currentPidProfile->ez_landing_limit / 100.0f; + } } #ifdef USE_LAUNCH_CONTROL