diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/controlrate_profile.c index 0d6b330343..85deeba34a 100644 --- a/src/main/fc/controlrate_profile.c +++ b/src/main/fc/controlrate_profile.c @@ -61,7 +61,8 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig) .throttle_limit_percent = 100, .rate_limit[FD_ROLL] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX, .rate_limit[FD_PITCH] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX, - .rate_limit[FD_YAW] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX + .rate_limit[FD_YAW] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX, + .tpaMode = TPA_MODE_PD, ); } } diff --git a/src/main/fc/controlrate_profile.h b/src/main/fc/controlrate_profile.h index 865d1787b8..36d2f211f5 100644 --- a/src/main/fc/controlrate_profile.h +++ b/src/main/fc/controlrate_profile.h @@ -37,6 +37,12 @@ typedef enum { THROTTLE_LIMIT_TYPE_CLIP, } throttleLimitType_e; + +typedef enum { + TPA_MODE_PD, + TPA_MODE_D +} tpaMode_e; + typedef struct controlRateConfig_s { uint8_t thrMid8; uint8_t thrExpo8; @@ -49,6 +55,7 @@ typedef struct controlRateConfig_s { 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 uint16_t rate_limit[3]; // Sets the maximum rate for the axes + uint8_t tpaMode; // Controls which PID terms TPA effects } controlRateConfig_t; PG_DECLARE_ARRAY(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 6d8f8e8db0..b8d202acd1 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -39,6 +39,7 @@ #include "drivers/sound_beeper.h" #include "drivers/time.h" +#include "fc/controlrate_profile.h" #include "fc/core.h" #include "fc/rc.h" @@ -1048,6 +1049,12 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT const float tpaFactor = getThrottlePIDAttenuation(); +#ifdef USE_TPA_MODE + const float tpaFactorKp = (currentControlRateProfile->tpaMode == TPA_MODE_PD) ? tpaFactor : 1.0f; +#else + const float tpaFactorKp = tpaFactor; +#endif + #ifdef USE_YAW_SPIN_RECOVERY const bool yawSpinActive = gyroYawSpinDetected(); #endif @@ -1145,7 +1152,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT // b = 1 and only c (feedforward weight) can be tuned (amount derivative on measurement or error). // -----calculate P component - pidData[axis].P = pidCoefficient[axis].Kp * errorRate * tpaFactor; + pidData[axis].P = pidCoefficient[axis].Kp * errorRate * tpaFactorKp; if (axis == FD_YAW) { pidData[axis].P = ptermYawLowpassApplyFn((filter_t *) &ptermYawLowpass, pidData[axis].P); } diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 624c7ca0fe..f9a2d31496 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -407,6 +407,12 @@ static const char * const lookupTableLaunchControlMode[] = { }; #endif +#ifdef USE_TPA_MODE +static const char * const lookupTableTpaMode[] = { + "PD", "D" +}; +#endif + #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) } const lookupTableEntry_t lookupTables[] = { @@ -510,6 +516,9 @@ const lookupTableEntry_t lookupTables[] = { #ifdef USE_LAUNCH_CONTROL LOOKUP_TABLE_ENTRY(lookupTableLaunchControlMode), #endif +#ifdef USE_TPA_MODE + LOOKUP_TABLE_ENTRY(lookupTableTpaMode), +#endif }; #undef LOOKUP_TABLE_ENTRY @@ -781,6 +790,9 @@ 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) }, +#ifdef USE_TPA_MODE + { "tpa_mode", VAR_UINT8 | PROFILE_RATE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_TPA_MODE }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, tpaMode) }, +#endif { "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) }, { "roll_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmax = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_ROLL]) }, diff --git a/src/main/interface/settings.h b/src/main/interface/settings.h index e54d49efd9..90844e2eb4 100644 --- a/src/main/interface/settings.h +++ b/src/main/interface/settings.h @@ -125,6 +125,9 @@ typedef enum { #endif #ifdef USE_LAUNCH_CONTROL TABLE_LAUNCH_CONTROL_MODE, +#endif +#ifdef USE_TPA_MODE + TABLE_TPA_MODE, #endif LOOKUP_TABLE_COUNT } lookupTableIndex_e; diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index c3887e19ca..1861230040 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -177,6 +177,7 @@ #define USE_RTC_TIME #define USE_RX_MSP #define USE_SERIALRX_FPORT // FrSky FPort +#define USE_TPA_MODE #define USE_TELEMETRY_CRSF #define USE_TELEMETRY_SRXL #define USE_VIRTUAL_CURRENT_METER