From 9a65b766a22e80fdb62507c682b4a13a4b941ebf Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Tue, 7 Jul 2020 15:11:17 +1000 Subject: [PATCH 1/2] refactor Thrust Linear to initialise throttleCompensateAmount in pid_init.c Maybe this could avoid recaculating throttleCompensateAmount every PID loop? --- src/main/flight/pid.c | 3 +-- src/main/flight/pid.h | 1 + src/main/flight/pid_init.c | 1 + 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 8fbffdd53..ece7425f4 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -294,9 +294,8 @@ float pidCompensateThrustLinearization(float throttle) { if (pidRuntime.thrustLinearization != 0.0f) { // for whoops where a lot of TL is needed, allow more throttle boost - const float throttleCompensateAmount = (1.0f - 0.5f * pidRuntime.thrustLinearization); const float throttleReversed = (1.0f - throttle); - throttle /= 1.0f + throttleCompensateAmount * powerf(throttleReversed, 2) * pidRuntime.thrustLinearization; + throttle /= 1.0f + pidRuntime.throttleCompensateAmount * powerf(throttleReversed, 2) * pidRuntime.thrustLinearization; } return throttle; } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index a13dd6e1f..dfc59fa28 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -351,6 +351,7 @@ typedef struct pidRuntime_s { #ifdef USE_THRUST_LINEARIZATION float thrustLinearization; + float throttleCompensateAmount; #endif #ifdef USE_AIRMODE_LPF diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c index 4b7a7c94f..632a4e86d 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -375,6 +375,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) #ifdef USE_THRUST_LINEARIZATION pidRuntime.thrustLinearization = pidProfile->thrustLinearization / 100.0f; + pidRuntime.throttleCompensateAmount = (1.0f - 0.5f * pidRuntime.thrustLinearization); #endif #if defined(USE_D_MIN) for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) { From 1fbf0ba816fa60bcd0476f7c73dbb9556eb026ef Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Tue, 7 Jul 2020 22:59:58 +1000 Subject: [PATCH 2/2] Further refactoring Thanks, Mike! --- src/main/flight/pid.c | 2 +- src/main/flight/pid_init.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index ece7425f4..0796cd216 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -295,7 +295,7 @@ float pidCompensateThrustLinearization(float throttle) if (pidRuntime.thrustLinearization != 0.0f) { // for whoops where a lot of TL is needed, allow more throttle boost const float throttleReversed = (1.0f - throttle); - throttle /= 1.0f + pidRuntime.throttleCompensateAmount * powerf(throttleReversed, 2) * pidRuntime.thrustLinearization; + throttle /= 1.0f + pidRuntime.throttleCompensateAmount * powerf(throttleReversed, 2); } return throttle; } diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c index 632a4e86d..08545ee65 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -375,7 +375,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) #ifdef USE_THRUST_LINEARIZATION pidRuntime.thrustLinearization = pidProfile->thrustLinearization / 100.0f; - pidRuntime.throttleCompensateAmount = (1.0f - 0.5f * pidRuntime.thrustLinearization); + pidRuntime.throttleCompensateAmount = pidRuntime.thrustLinearization - 0.5f * powerf(pidRuntime.thrustLinearization, 2); #endif #if defined(USE_D_MIN) for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {