From fc189e585029eaf827e8ed2a4d65a7e5f92ddcff Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Thu, 8 Nov 2018 19:22:29 -0500 Subject: [PATCH 1/2] Make TPA configurable to affect P/D or D only Adds a new `tpa_mode` parameter that accepts `PD` (default) and `D`. Allows the user to configer to affect P/D as it always has, or switch to D-only mode. Note: the `tpa_mode` parameter was added to the PID Profile instead of rate profiles with the other TPA parameters. This can be discussed, but I didn't think it made sense to have this be part of rate profiles as it affects PID tuning (the same argument could be made for the other TPA parameters). Code is wrapped in `USE_TPA_MODE` so it can be disabled if needed. --- src/main/flight/pid.c | 17 ++++++++++++++++- src/main/flight/pid.h | 6 ++++++ src/main/interface/settings.c | 13 +++++++++++++ src/main/interface/settings.h | 3 +++ src/main/target/common_pre.h | 1 + 5 files changed, 39 insertions(+), 1 deletion(-) 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 From 357d19aa05eb2081f2f6d2fa91e13953fad05a86 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Fri, 9 Nov 2018 08:05:40 -0500 Subject: [PATCH 2/2] Move parameter from PID profile to rate profile --- src/main/fc/controlrate_profile.c | 3 ++- src/main/fc/controlrate_profile.h | 7 +++++++ src/main/flight/pid.c | 12 ++---------- src/main/flight/pid.h | 6 ------ src/main/interface/settings.c | 7 +++---- 5 files changed, 14 insertions(+), 21 deletions(-) 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 43ec546d7a..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" @@ -175,7 +176,6 @@ void resetPidProfile(pidProfile_t *pidProfile) .launchControlAngleLimit = 0, .launchControlGain = 40, .launchControlAllowTriggerReset = true, - .tpaMode = TPA_MODE_PD, ); #ifdef USE_DYN_LPF pidProfile->dterm_lowpass_hz = 120; @@ -435,10 +435,6 @@ 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++) { @@ -581,10 +577,6 @@ 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) @@ -1058,7 +1050,7 @@ 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; + const float tpaFactorKp = (currentControlRateProfile->tpaMode == TPA_MODE_PD) ? tpaFactor : 1.0f; #else const float tpaFactorKp = tpaFactor; #endif diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 5b017922c4..329fbb773e 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -97,11 +97,6 @@ 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 @@ -161,7 +156,6 @@ 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 ee1356da15..3819c4b70d 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -787,6 +787,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]) }, @@ -938,10 +941,6 @@ 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) },