Throttlebased EzLanding (#12094)

* ezLanding

* Add ez_landing throttle mode

* Correct EzLanding scaling of motorMixRange

* Correct mixer_type switch bracing style

* Remove motor value cliping ez landing mode

- rename mixer type cli setting to EZLANDING from EZLANDING_THROTTLE
- remove EZLANDING_CLIP cli setting
- double default ez_landing_threshold
- halve default ez_landing_limit
- check  and  limits in cli settings
- remove mixer type dependent settings in mixer_init
- remove clip based code in mixer.c

* Change ez_landing setting values and refactoring

- Halve defaul ez_landing_threshold setting and double in init instead.
  Now stick deflection equal to ez_landing_threshold should give approimately full authority.
  Previously it was the point where the mixer was allowed to raise the throttle to 100 % (which wouuld never be required)
- Increase ez_landing_threshold maximum to 200 (from 100) to allow settings that increase authority by a little at full stick deflection
- Increase ez_landing_limit maximum to 75 which is the point where EzLanding should act identical to the Legacy mixer with airmode on
- remove throttle percent from
- simplify calculation of , since throttle stick deflection is no longer involved
- update/remove outdated comments

* Remove old EZLANDING entries in mixerType enum

* Add mixer_type setting to blackbox log header

---------

Co-authored-by: ctzsnooze <chris.thompson@sydney.edu.au>
Co-authored-by: Mark Haslinghuis <mark@numloq.nl>

src/main/blackbox/blackbox.c

@@ -45,6 +45,8 @@
 #include "config/config.h"
 #include "config/feature.h"
 
+#include "cli/settings.h"
+
 #include "drivers/compass/compass.h"
 #include "drivers/sensor.h"
 #include "drivers/time.h"
@@ -1405,6 +1407,10 @@ static bool blackboxWriteSysinfo(void)
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_LOW_RATE, "%d",           currentPidProfile->tpa_low_rate);
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_LOW_BREAKPOINT, "%d",     currentPidProfile->tpa_low_breakpoint);
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_LOW_ALWAYS, "%d",         currentPidProfile->tpa_low_always);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MIXER_TYPE, "%s",             lookupTableMixerType[mixerConfig()->mixer_type]);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_EZ_LANDING_THRESHOLD, "%d",   currentPidProfile->ez_landing_threshold);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_EZ_LANDING_LIMIT, "%d",       currentPidProfile->ez_landing_limit);
+
         BLACKBOX_PRINT_HEADER_LINE("rc_rates", "%d,%d,%d",                  currentControlRateProfile->rcRates[ROLL],
                                                                             currentControlRateProfile->rcRates[PITCH],
                                                                             currentControlRateProfile->rcRates[YAW]);

src/main/build/debug.c

@@ -116,4 +116,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "RC_STATS",
     "MAG_CALIB",
     "MAG_TASK_RATE",
+    "EZLANDING",
 };

src/main/build/debug.h

@@ -118,6 +118,7 @@ typedef enum {
     DEBUG_RC_STATS,
     DEBUG_MAG_CALIB,
     DEBUG_MAG_TASK_RATE,
+    DEBUG_EZLANDING,
     DEBUG_COUNT
 } debugType_e;
 

src/main/cli/settings.c

@@ -501,8 +501,8 @@ const char * const lookupTableSimplifiedTuningPidsMode[] = {
     "OFF", "RP", "RPY",
 };
 
-static const char* const lookupTableMixerType[] = {
-    "LEGACY", "LINEAR", "DYNAMIC",
+const char* const lookupTableMixerType[] = {
+    "LEGACY", "LINEAR", "DYNAMIC", "EZLANDING",
 };
 
 #ifdef USE_OSD
@@ -945,7 +945,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) },
-    { "mixer_type",                 VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MIXER_TYPE }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, mixer_type) },
+    { PARAM_NAME_MIXER_TYPE,        VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MIXER_TYPE }, PG_MIXER_CONFIG, offsetof(mixerConfig_t, mixer_type) },
     { "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) },
 #ifdef USE_RPM_LIMIT
@@ -1258,6 +1258,9 @@ const clivalue_t valueTable[] = {
     { PARAM_NAME_TPA_LOW_BREAKPOINT,      VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { PWM_RANGE_MIN, PWM_RANGE_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_low_breakpoint) },
     { PARAM_NAME_TPA_LOW_ALWAYS, VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_low_always) },
 
+    { PARAM_NAME_EZ_LANDING_THRESHOLD,      VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, ez_landing_threshold) },
+    { PARAM_NAME_EZ_LANDING_LIMIT,          VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 75 }, PG_PID_PROFILE, offsetof(pidProfile_t, ez_landing_limit) },
+
 // PG_TELEMETRY_CONFIG
 #ifdef USE_TELEMETRY
     { "tlm_inverted",               VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, telemetry_inverted) },

src/main/cli/settings.h

@@ -269,6 +269,8 @@ extern const char * const lookupTableOffOn[];
 
 extern const char * const lookupTableSimplifiedTuningPidsMode[];
 
+extern const char * const lookupTableMixerType[];
+
 extern const char * const lookupTableCMSMenuBackgroundType[];
 
 extern const char * const lookupTableThrottleLimitType[];

src/main/fc/parameter_names.h

@@ -58,6 +58,9 @@
 #define PARAM_NAME_TPA_LOW_BREAKPOINT "tpa_low_breakpoint"
 #define PARAM_NAME_TPA_LOW_ALWAYS "tpa_low_always"
 #define PARAM_NAME_TPA_MODE "tpa_mode"
+#define PARAM_NAME_MIXER_TYPE "mixer_type"
+#define PARAM_NAME_EZ_LANDING_THRESHOLD "ez_landing_threshold"
+#define PARAM_NAME_EZ_LANDING_LIMIT "ez_landing_limit"
 #define PARAM_NAME_THROTTLE_LIMIT_TYPE "throttle_limit_type"
 #define PARAM_NAME_THROTTLE_LIMIT_PERCENT "throttle_limit_percent"
 #define PARAM_NAME_GYRO_CAL_ON_FIRST_ARM "gyro_cal_on_first_arm"

src/main/fc/rc.c

@@ -60,7 +60,10 @@ typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCom
 // note that rcCommand[] is an external float
 
 static float rawSetpoint[XYZ_AXIS_COUNT];
+
 static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3]; // deflection range -1 to 1
+static float maxRcDeflectionAbs;
+
 static bool reverseMotors = false;
 static applyRatesFn *applyRates;
 
@@ -140,6 +143,11 @@ float getRcDeflectionAbs(int axis)
     return rcDeflectionAbs[axis];
 }
 
+float getMaxRcDeflectionAbs(void)
+{
+    return maxRcDeflectionAbs;
+}
+
 #define THROTTLE_LOOKUP_LENGTH 12
 static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH];    // lookup table for expo & mid THROTTLE
 
@@ -620,6 +628,7 @@ FAST_CODE_NOINLINE void calculateFeedforward(const pidRuntime_t *pid, int axis)
 FAST_CODE void processRcCommand(void)
 {
     if (isRxDataNew) {
+        maxRcDeflectionAbs = 0.0f;
         for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
 
             float angleRate;
@@ -646,6 +655,7 @@ FAST_CODE void processRcCommand(void)
                 rcDeflection[axis] = rcCommandf;
                 const float rcCommandfAbs = fabsf(rcCommandf);
                 rcDeflectionAbs[axis] = rcCommandfAbs;
+                maxRcDeflectionAbs = fmaxf(maxRcDeflectionAbs, rcCommandfAbs);
 
                 angleRate = applyRates(axis, rcCommandf, rcCommandfAbs);
             }

src/main/fc/rc.h

@@ -34,6 +34,7 @@ float getSetpointRate(int axis);
 float getRcDeflection(int axis);
 float getRcDeflectionRaw(int axis);
 float getRcDeflectionAbs(int axis);
+float getMaxRcDeflectionAbs(void);
 void updateRcCommands(void);
 void resetYawAxis(void);
 void initRcProcessing(void);

src/main/flight/mixer.c

@@ -255,7 +255,6 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
 #else
         motorRangeMax = mixerRuntime.motorOutputHigh;
 #endif
-
         motorRangeMin = mixerRuntime.motorOutputLow + motorRangeMinIncrease * (mixerRuntime.motorOutputHigh - mixerRuntime.motorOutputLow);
         motorOutputMin = motorRangeMin;
         motorOutputRange = motorRangeMax - motorRangeMin;
@@ -423,6 +422,8 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t
             motor[i] = motor_disarmed[i];
         }
     }
+    DEBUG_SET(DEBUG_EZLANDING, 1, throttle * 10000U);
+    // DEBUG_EZLANDING 0 is the ezLanding factor 2 is the throttle limit
 }
 
 static float applyThrottleLimit(float throttle)
@@ -502,6 +503,60 @@ static void applyMixerAdjustmentLinear(float *motorMix, const bool airmodeEnable
     throttle = constrainf(throttle, -minMotor, 1.0f - maxMotor);
 }
 
+static float calcEzLandLimit(float maxDeflection)
+{
+    // calculate limit to where the mixer can raise the throttle based on RPY stick deflection
+    // 0.0 = no increas allowed, 1.0 = 100% increase allowed
+    float ezLandLimit = 1.0f;
+    if (maxDeflection < mixerRuntime.ezLandingThreshold) { // roll, pitch and yaw sticks under threshold
+        ezLandLimit = maxDeflection / mixerRuntime.ezLandingThreshold; // normalised 0 - 1
+        ezLandLimit = fmaxf(ezLandLimit, mixerRuntime.ezLandingLimit); // stay above the minimum
+    }
+    return ezLandLimit;
+}
+
+static void applyMixerAdjustmentEzLand(float *motorMix, const float motorMixMin, const float motorMixMax)
+{
+    // Calculate factor for normalizing motor mix range to <= 1.0
+    const float baseNormalizationFactor = motorMixRange > 1.0f ? 1.0f / motorMixRange : 1.0f;
+    const float normalizedMotorMixMin = motorMixMin * baseNormalizationFactor;
+    const float normalizedMotorMixMax = motorMixMax * baseNormalizationFactor;
+
+    // Upper throttle limit
+    // range default 0.05 - 1.0 with ezLandingLimit = 5, no stick deflection -> 0.05
+    const float ezLandLimit = calcEzLandLimit(getMaxRcDeflectionAbs());
+    // use the largest of throttle and limit calculated from RPY stick positions
+    float upperLimit = fmaxf(ezLandLimit, throttle);
+    // limit throttle to avoid clipping the highest motor output
+    upperLimit = fminf(upperLimit, 1.0f - normalizedMotorMixMax);
+
+    // Lower throttle Limit
+    const float epsilon = 1.0e-6f;  // add small value to avoid divisions by zero
+    const float absMotorMixMin = fabsf(normalizedMotorMixMin) + epsilon;
+    const float lowerLimit = fminf(upperLimit, absMotorMixMin);
+
+    // represents how much motor values have to be scaled to avoid clipping
+    const float ezLandFactor = upperLimit / absMotorMixMin;
+
+    // scale motor values
+    const float normalizationFactor = baseNormalizationFactor * fminf(1.0f, ezLandFactor);
+    for (int i = 0; i < mixerRuntime.motorCount; i++) {
+        motorMix[i] *= normalizationFactor;
+    }
+    motorMixRange *= baseNormalizationFactor;
+    // Make anti windup recognize reduced authority range
+    motorMixRange = fmaxf(motorMixRange, 1.0f / ezLandFactor);
+
+    // Constrain throttle
+    throttle = constrainf(throttle, lowerLimit, upperLimit);
+
+    // Log ezLandFactor, upper throttle limit, and ezLandFactor if throttle was zero
+    DEBUG_SET(DEBUG_EZLANDING, 0, fminf(1.0f, ezLandFactor) * 10000U);
+    // DEBUG_EZLANDING 1 is the adjusted throttle
+    DEBUG_SET(DEBUG_EZLANDING, 2, upperLimit * 10000U);
+    DEBUG_SET(DEBUG_EZLANDING, 3, fminf(1.0f, ezLandLimit / absMotorMixMin) * 10000U);
+}
+
 static void applyMixerAdjustment(float *motorMix, const float motorMixMin, const float motorMixMax, const bool airmodeEnabled)
 {
 #ifdef USE_AIRMODE_LPF
@@ -665,10 +720,21 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
 #endif
 
     motorMixRange = motorMixMax - motorMixMin;
-    if (mixerConfig()->mixer_type > MIXER_LEGACY) {
-        applyMixerAdjustmentLinear(motorMix, airmodeEnabled);
-    } else {
+
+    switch (mixerConfig()->mixer_type) {
+    case MIXER_LEGACY:
         applyMixerAdjustment(motorMix, motorMixMin, motorMixMax, airmodeEnabled);
+        break;
+    case MIXER_LINEAR:
+    case MIXER_DYNAMIC:
+        applyMixerAdjustmentLinear(motorMix, airmodeEnabled);
+        break;
+    case MIXER_EZLANDING:
+        applyMixerAdjustmentEzLand(motorMix, motorMixMin, motorMixMax);
+        break;
+    default:
+        applyMixerAdjustment(motorMix, motorMixMin, motorMixMax, airmodeEnabled);
+        break;
     }
 
     if (featureIsEnabled(FEATURE_MOTOR_STOP)

src/main/flight/mixer.h

@@ -71,6 +71,7 @@ typedef enum mixerType
     MIXER_LEGACY = 0,
     MIXER_LINEAR = 1,
     MIXER_DYNAMIC = 2,
+    MIXER_EZLANDING = 3,
 } mixerType_e;
 
 // Custom mixer data per motor

src/main/flight/mixer_init.c

@@ -365,6 +365,9 @@ void mixerInitProfile(void)
     pt1FilterInit(&mixerRuntime.rpmLimiterThrottleScaleOffsetFilter, pt1FilterGain(2.0f, pidGetDT()));
     mixerResetRpmLimiter();
 #endif
+
+    mixerRuntime.ezLandingThreshold = 2.0f * currentPidProfile->ez_landing_threshold / 100.0f;
+    mixerRuntime.ezLandingLimit = currentPidProfile->ez_landing_limit / 100.0f;
 }
 
 #ifdef USE_RPM_LIMIT

src/main/flight/mixer_init.h

@@ -63,6 +63,8 @@ typedef struct mixerRuntime_s {
     pt1Filter_t rpmLimiterAverageRpmFilter;
     pt1Filter_t rpmLimiterThrottleScaleOffsetFilter;
 #endif
+    float ezLandingThreshold;
+    float ezLandingLimit;
 } mixerRuntime_t;
 
 extern mixerRuntime_t mixerRuntime;

src/main/flight/pid.c

@@ -227,6 +227,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .tpa_low_rate = 20,
         .tpa_low_breakpoint = 1050,
         .tpa_low_always = 0,
+        .ez_landing_threshold = 25,
+        .ez_landing_limit = 5,
     );
 
 #ifndef USE_D_MIN

src/main/flight/pid.h

@@ -234,12 +234,16 @@ typedef struct pidProfile_s {
     uint8_t tpa_mode;                       // Controls which PID terms TPA effects
     uint8_t tpa_rate;                       // Percent reduction in P or D at full throttle
     uint16_t tpa_breakpoint;                // Breakpoint where TPA is activated
+
     uint8_t angle_feedforward_smoothing_ms; // Smoothing factor for angle feedforward as time constant in milliseconds
     uint8_t angle_earth_ref;                // Control amount of "co-ordination" from yaw into roll while pitched forward in angle mode
     uint16_t horizon_delay_ms;              // delay when Horizon Strength increases, 50 = 500ms time constant
     uint8_t tpa_low_rate;                   // Percent reduction in P or D at zero throttle
     uint16_t tpa_low_breakpoint;            // Breakpoint where lower TPA is deactivated
     uint8_t tpa_low_always;                 // off, on - if OFF then low TPA is only active until tpa_low_breakpoint is reached the first time
+
+    uint8_t ez_landing_threshold;           // Threshold stick position below which motor output is limited
+    uint8_t ez_landing_limit;               // Maximum motor output when all sticks centred and throttle zero
 } pidProfile_t;
 
 PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);