Add ez_landing_speed parameter (#13411)

* Add ez_landing_speed parameter

The parameter is the speed at which ez_landing will be effectively disabled in tenths of meters per second.
Default value 50 (5 m/s).
EZLANDING bug field 5 is the contribution from this parameter to the ezlanding throttle cap.

* Correct ez_landing_speed logic and scaling

* ez_landing_speed should now raise the limit when the speed is above ez_landing_speed (previously only raised the limit if the speed was bellow, and had no effect above the limit)
* ez_landing_speed should now be scaled so that EzLanding is effectively disabled when speed >= ez_landing_speed (previously EzLanding would be disabeld when speed was at half of ez_landing_speed)

* Add stick input upper limit as EZLANDING debug 4

* Check for gps 3D fix before using gps speed for EzLanding

* Prevent division by 0 if ez_landing_threshold is set to 0

* Scale EzLanding speed to m/s from cm/s

* Update src/main/flight/mixer.c

Co-authored-by: Petr Ledvina <ledvinap@gmail.com>

---------

Co-authored-by: Mark Haslinghuis <mark@numloq.nl>
Co-authored-by: Petr Ledvina <ledvinap@gmail.com>

src/main/blackbox/blackbox.c

@@ -1410,6 +1410,7 @@ static bool blackboxWriteSysinfo(void)
         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(PARAM_NAME_EZ_LANDING_SPEED, "%d",       currentPidProfile->ez_landing_speed);
 
         BLACKBOX_PRINT_HEADER_LINE("rc_rates", "%d,%d,%d",                  currentControlRateProfile->rcRates[ROLL],
                                                                             currentControlRateProfile->rcRates[PITCH],

src/main/cli/settings.c

@@ -1261,6 +1261,7 @@ const clivalue_t valueTable[] = {
 
     { 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) },
+    { PARAM_NAME_EZ_LANDING_SPEED,          VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, ez_landing_speed) },
 
 // PG_TELEMETRY_CONFIG
 #ifdef USE_TELEMETRY

src/main/fc/parameter_names.h

@@ -61,6 +61,7 @@
 #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_EZ_LANDING_SPEED "ez_landing_speed"
 #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/flight/mixer.c

@@ -53,6 +53,8 @@
 #include "flight/pid.h"
 #include "flight/rpm_filter.h"
 
+#include "io/gps.h"
+
 #include "pg/rx.h"
 
 #include "rx/rx.h"
@@ -502,16 +504,21 @@ static void applyMixerAdjustmentLinear(float *motorMix, const bool airmodeEnable
     throttle = constrainf(throttle, -minMotor, 1.0f - maxMotor);
 }
 
-static float calcEzLandLimit(float maxDeflection)
+static float calcEzLandLimit(float maxDeflection, float speed)
 {
     // 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;
+    const float deflectionLimit = mixerRuntime.ezLandingThreshold > 0.0f ? fminf(1.0f, maxDeflection / mixerRuntime.ezLandingThreshold) : 0.0f;
+    DEBUG_SET(DEBUG_EZLANDING, 4, lrintf(deflectionLimit * 10000.0f));
+
+    // calculate limit to where the mixer can raise the throttle based on speed
+    // TODO sanity checks like number of sats, dop, accuracy?
+    const float speedLimit = mixerRuntime.ezLandingSpeed > 0.0f ? fminf(1.0f, speed / mixerRuntime.ezLandingSpeed) : 0.0f;
+    DEBUG_SET(DEBUG_EZLANDING, 5, lrintf(speedLimit * 10000.0f));
+
+    // get the highest of the limits from deflection, speed, and the base ez_landing_limit
+    const float deflectionAndSpeedLimit = fmaxf(deflectionLimit, speedLimit);
+    return fmaxf(mixerRuntime.ezLandingLimit, deflectionAndSpeedLimit);
 }
 
 static void applyMixerAdjustmentEzLand(float *motorMix, const float motorMixMin, const float motorMixMax)
@@ -521,9 +528,13 @@ static void applyMixerAdjustmentEzLand(float *motorMix, const float motorMixMin,
     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());
+#ifdef USE_GPS
+    const float speed = STATE(GPS_FIX) ? gpsSol.speed3d / 100.0f : 0.0f;  // m/s
+#else
+    const float speed = 0.0f;
+#endif
+
+    const float ezLandLimit = calcEzLandLimit(getMaxRcDeflectionAbs(), speed);
     // 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
@@ -554,6 +565,7 @@ static void applyMixerAdjustmentEzLand(float *motorMix, const float motorMixMin,
     // 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);
+    // DEBUG_EZLANDING 4 and 5 is the upper limits based on stick input and speed respectively
 }
 
 static void applyMixerAdjustment(float *motorMix, const float motorMixMin, const float motorMixMax, const bool airmodeEnabled)

src/main/flight/mixer_init.c

@@ -369,6 +369,7 @@ void mixerInitProfile(void)
 
     mixerRuntime.ezLandingThreshold = 2.0f * currentPidProfile->ez_landing_threshold / 100.0f;
     mixerRuntime.ezLandingLimit = currentPidProfile->ez_landing_limit / 100.0f;
+    mixerRuntime.ezLandingSpeed = 2.0f * currentPidProfile->ez_landing_speed / 10.0f;
 }
 
 #ifdef USE_RPM_LIMIT

src/main/flight/mixer_init.h

@@ -66,6 +66,7 @@ typedef struct mixerRuntime_s {
 #endif
     float ezLandingThreshold;
     float ezLandingLimit;
+    float ezLandingSpeed;
 } mixerRuntime_t;
 
 extern mixerRuntime_t mixerRuntime;

src/main/flight/pid.c

@@ -229,6 +229,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .tpa_low_always = 0,
         .ez_landing_threshold = 25,
         .ez_landing_limit = 15,
+        .ez_landing_speed = 50,
     );
 
 #ifndef USE_D_MIN

src/main/flight/pid.h

@@ -244,6 +244,7 @@ typedef struct pidProfile_s {
 
     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
+    uint8_t ez_landing_speed;               // Speed below which motor output is limited
 } pidProfile_t;
 
 PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);