From 10067ad6adb547e3ca8d0ecef1d63e4ea43d92ee Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Fri, 12 May 2023 02:38:15 +1000 Subject: [PATCH] TPA optimisations (#12721) * TPA optimisations * improvement, thanks @ledvinap * update following review comments, thanks karatebrot and ledvinap * include rx.h in pid_init.c to get PWM_RANGE_MIN * review suggestion --- src/main/cli/settings.c | 2 +- src/main/flight/pid.c | 17 +++-------------- src/main/flight/pid.h | 2 ++ src/main/flight/pid_init.c | 5 +++++ 4 files changed, 11 insertions(+), 15 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index e61a826a20..8f3ca71fc2 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1223,7 +1223,7 @@ const clivalue_t valueTable[] = { { PARAM_NAME_TPA_MODE, VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_TPA_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_mode) }, #endif { PARAM_NAME_TPA_RATE, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, TPA_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_rate) }, - { PARAM_NAME_TPA_BREAKPOINT, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_breakpoint) }, + { PARAM_NAME_TPA_BREAKPOINT, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { PWM_RANGE_MIN, PWM_RANGE_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, tpa_breakpoint) }, // PG_TELEMETRY_CONFIG #ifdef USE_TELEMETRY diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 7f96b6f281..8189d019cf 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -277,20 +277,9 @@ void pidResetIterm(void) void pidUpdateTpaFactor(float throttle) { - pidProfile_t *currentPidProfile; - - currentPidProfile = pidProfilesMutable(systemConfig()->pidProfileIndex); - const float tpaBreakpoint = (currentPidProfile->tpa_breakpoint - 1000) / 1000.0f; - float tpaRate = currentPidProfile->tpa_rate / 100.0f; - - if (throttle > tpaBreakpoint) { - if (throttle < 1.0f) { - tpaRate *= (throttle - tpaBreakpoint) / (1.0f - tpaBreakpoint); - } - } else { - tpaRate = 0.0f; - } - pidRuntime.tpaFactor = 1.0f - tpaRate; + const float throttleTemp = fminf(throttle, 1.0f); // don't permit throttle > 1 ? is this needed ? can throttle be > 1 at this point ? + const float throttleDifference = fmaxf(throttleTemp - pidRuntime.tpaBreakpoint, 0.0f); + pidRuntime.tpaFactor = 1.0f - throttleDifference * pidRuntime.tpaMultiplier; } void pidUpdateAntiGravityThrottleFilter(float throttle) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 5511245361..3cd240caf0 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -322,6 +322,8 @@ typedef struct pidRuntime_s { bool zeroThrottleItermReset; bool levelRaceMode; float tpaFactor; + float tpaBreakpoint; + float tpaMultiplier; #ifdef USE_ITERM_RELAX pt1Filter_t windupLpf[XYZ_AXIS_COUNT]; diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c index 977257e7ab..e1563ad9e8 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -40,6 +40,8 @@ #include "flight/pid.h" #include "flight/rpm_filter.h" +#include "rx/rx.h" + #include "sensors/gyro.h" #include "sensors/sensors.h" @@ -423,6 +425,9 @@ void pidInitConfig(const pidProfile_t *pidProfile) #endif pidRuntime.levelRaceMode = pidProfile->level_race_mode; + pidRuntime.tpaBreakpoint = constrainf((pidProfile->tpa_breakpoint - PWM_RANGE_MIN) / 1000.0f, 0.0f, 0.99f); + // default of 1350 returns 0.35. range limited to 0 to 0.99 + pidRuntime.tpaMultiplier = (pidProfile->tpa_rate / 100.0f) / (1.0f - pidRuntime.tpaBreakpoint); } void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex)