diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/controlrate_profile.c index 131702100..37e8bc457 100644 --- a/src/main/fc/controlrate_profile.c +++ b/src/main/fc/controlrate_profile.c @@ -52,7 +52,9 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig) .rcExpo[FD_YAW] = 0, .rates[FD_ROLL] = 70, .rates[FD_PITCH] = 70, - .rates[FD_YAW] = 70 + .rates[FD_YAW] = 70, + .throttle_limit_type = THROTTLE_LIMIT_TYPE_OFF, + .throttle_limit_percent = 100 ); } } diff --git a/src/main/fc/controlrate_profile.h b/src/main/fc/controlrate_profile.h index 7536856a7..806db4e71 100644 --- a/src/main/fc/controlrate_profile.h +++ b/src/main/fc/controlrate_profile.h @@ -28,6 +28,12 @@ typedef enum { RATES_TYPE_RACEFLIGHT, } ratesType_e; +typedef enum { + THROTTLE_LIMIT_TYPE_OFF = 0, + THROTTLE_LIMIT_TYPE_SCALE, + THROTTLE_LIMIT_TYPE_CLIP, +} throttleLimitType_e; + typedef struct controlRateConfig_s { uint8_t thrMid8; uint8_t thrExpo8; @@ -37,6 +43,8 @@ typedef struct controlRateConfig_s { uint8_t rates[3]; uint8_t dynThrPID; uint16_t tpa_breakpoint; // Breakpoint where TPA is activated + uint8_t throttle_limit_type; // Sets the throttle limiting type - off, scale or clip + uint8_t throttle_limit_percent; // Sets the maximum pilot commanded throttle limit } controlRateConfig_t; PG_DECLARE_ARRAY(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 8a1e317e0..32c35efbf 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -40,6 +40,7 @@ #include "io/motors.h" #include "fc/config.h" +#include "fc/controlrate_profile.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" @@ -733,6 +734,21 @@ void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation) // Calculate voltage compensation const float vbatCompensationFactor = vbatPidCompensation ? calculateVbatPidCompensation() : 1.0f; + // Apply the throttle_limit_percent to scale or limit the throttle based on throttle_limit_type + if ((currentControlRateProfile->throttle_limit_percent < 100) && (currentControlRateProfile->throttle_limit_type != THROTTLE_LIMIT_TYPE_OFF)) { + const float throttleLimitFactor = currentControlRateProfile->throttle_limit_percent / 100.0f; + switch (currentControlRateProfile->throttle_limit_type) { + case THROTTLE_LIMIT_TYPE_SCALE: + throttle = throttle * throttleLimitFactor; + break; + case THROTTLE_LIMIT_TYPE_CLIP: + if (throttle > throttleLimitFactor) { + throttle = throttleLimitFactor; + } + break; + } + } + // Find roll/pitch/yaw desired output float motorMix[MAX_SUPPORTED_MOTORS]; float motorMixMax = 0, motorMixMin = 0; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 2c1e5ce0a..2dd41aa8c 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -134,7 +134,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .itermLimit = 150, .throttle_boost = 0, .throttle_boost_cutoff = 15, - .iterm_rotation = false + .iterm_rotation = false, ); } diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index eedd63b52..e31b2952c 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -321,6 +321,10 @@ static const char * const lookupOverclock[] = { }; #endif +static const char * const lookupTableThrottleLimitType[] = { + "OFF", "SCALE", "CLIP" +}; + #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) } const lookupTableEntry_t lookupTables[] = { @@ -388,6 +392,7 @@ const lookupTableEntry_t lookupTables[] = { #ifdef USE_DUAL_GYRO LOOKUP_TABLE_ENTRY(lookupTableGyro), #endif + LOOKUP_TABLE_ENTRY(lookupTableThrottleLimitType), }; #undef LOOKUP_TABLE_ENTRY @@ -629,6 +634,8 @@ const clivalue_t valueTable[] = { { "yaw_srate", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmax = { 0, CONTROL_RATE_CONFIG_RATE_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rates[FD_YAW]) }, { "tpa_rate", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX}, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, dynThrPID) }, { "tpa_breakpoint", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, tpa_breakpoint) }, + { "throttle_limit_type", VAR_UINT8 | PROFILE_RATE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_THROTTLE_LIMIT_TYPE }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, throttle_limit_type) }, + { "throttle_limit_percent", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmax = { 25, 100 }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, throttle_limit_percent) }, // PG_SERIAL_CONFIG { "reboot_character", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 48, 126 }, PG_SERIAL_CONFIG, offsetof(serialConfig_t, reboot_character) }, diff --git a/src/main/interface/settings.h b/src/main/interface/settings.h index ffbe895ae..33d95e0f1 100644 --- a/src/main/interface/settings.h +++ b/src/main/interface/settings.h @@ -87,6 +87,7 @@ typedef enum { #ifdef USE_DUAL_GYRO TABLE_GYRO, #endif + TABLE_THROTTLE_LIMIT_TYPE, LOOKUP_TABLE_COUNT } lookupTableIndex_e;