Add ez_landing throttle mode

betaflight · Dec 23, 2022 · 22e8a0a · 22e8a0a
1 parent 2ca8460
commit 22e8a0a
Show file tree

Hide file tree

Showing 4 changed files with 82 additions and 22 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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