diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 6d8f8e8db0..43ec546d7a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -175,6 +175,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .launchControlAngleLimit = 0, .launchControlGain = 40, .launchControlAllowTriggerReset = true, + .tpaMode = TPA_MODE_PD, ); #ifdef USE_DYN_LPF pidProfile->dterm_lowpass_hz = 120; @@ -434,6 +435,10 @@ static FAST_RAM_ZERO_INIT uint8_t launchControlAngleLimit; static FAST_RAM_ZERO_INIT float launchControlKi; #endif +#ifdef USE_TPA_MODE +static FAST_RAM_ZERO_INIT uint8_t tpaMode; +#endif + void pidResetIterm(void) { for (int axis = 0; axis < 3; axis++) { @@ -576,6 +581,10 @@ void pidInitConfig(const pidProfile_t *pidProfile) } launchControlKi = ITERM_SCALE * pidProfile->launchControlGain; #endif + +#ifdef USE_TPA_MODE + tpaMode = pidProfile->tpaMode; +#endif } void pidInit(const pidProfile_t *pidProfile) @@ -1048,6 +1057,12 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT const float tpaFactor = getThrottlePIDAttenuation(); +#ifdef USE_TPA_MODE + const float tpaFactorKp = (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 +1160,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/flight/pid.h b/src/main/flight/pid.h index 329fbb773e..5b017922c4 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -97,6 +97,11 @@ typedef enum { ITERM_RELAX_SETPOINT } itermRelaxType_e; +typedef enum { + TPA_MODE_PD, + TPA_MODE_D +} tpaMode_e; + typedef struct pidProfile_s { uint16_t yaw_lowpass_hz; // Additional yaw filter when yaw axis too noisy uint16_t dterm_lowpass_hz; // Delta Filter in hz @@ -156,6 +161,7 @@ typedef struct pidProfile_s { uint8_t launchControlAngleLimit; // Optional launch control angle limit (requires ACC) uint8_t launchControlGain; // Iterm gain used while launch control is active uint8_t launchControlAllowTriggerReset; // Controls trigger behavior and whether the trigger can be reset + uint8_t tpaMode; // Controls which PID terms TPA effects } pidProfile_t; PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles); diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index d692d99056..ee1356da15 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 @@ -929,6 +938,10 @@ const clivalue_t valueTable[] = { { "launch_control_gain", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlGain) }, #endif +#ifdef USE_TPA_MODE + { "tpa_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_TPA_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, tpaMode) }, +#endif + // 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) }, 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 6e7cc1dc9e..0c7e608ee1 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