Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 20:35:33 +03:00
Code Issues Wiki Activity

Merge pull request #9740 from ctzsnooze/Thrust-Linear-Update

Browse source
This commit is contained in:
Michael Keller 2020-07-05 14:27:43 +12:00 • committed by GitHub
commit a8e7890d88
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
4 changed files with 7 additions and 10 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/cli/settings.c
View file

@ -1112,7 +1112,7 @@ const clivalue_t valueTable[] = {
#endif #endif
#ifdef USE_THRUST_LINEARIZATION #ifdef USE_THRUST_LINEARIZATION
{ "thrust_linear", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, thrustLinearization) }, { "thrust_linear", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 150 }, PG_PID_PROFILE, offsetof(pidProfile_t, thrustLinearization) },
#endif #endif
#ifdef USE_AIRMODE_LPF #ifdef USE_AIRMODE_LPF
{ "transient_throttle_limit", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 30 }, PG_PID_PROFILE, offsetof(pidProfile_t, transient_throttle_limit) }, { "transient_throttle_limit", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 30 }, PG_PID_PROFILE, offsetof(pidProfile_t, transient_throttle_limit) },

9
src/main/flight/pid.c
View file

@ -290,7 +290,10 @@ void pidAcroTrainerInit(void)
float pidCompensateThrustLinearization(float throttle) float pidCompensateThrustLinearization(float throttle)
{ {
if (pidRuntime.thrustLinearization != 0.0f) { if (pidRuntime.thrustLinearization != 0.0f) {
throttle = throttle * (throttle * pidRuntime.thrustLinearization + 1.0f - pidRuntime.thrustLinearization); // 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;
} }
return throttle; return throttle;
} }
@ -299,8 +302,8 @@ float pidApplyThrustLinearization(float motorOutput)
{ {
if (pidRuntime.thrustLinearization != 0.0f) { if (pidRuntime.thrustLinearization != 0.0f) {
if (motorOutput > 0.0f) { if (motorOutput > 0.0f) {
motorOutput = sqrtf(motorOutput * pidRuntime.thrustLinearizationReciprocal + const float motorOutputReversed = (1.0f - motorOutput);
pidRuntime.thrustLinearizationB * pidRuntime.thrustLinearizationB) - pidRuntime.thrustLinearizationB; motorOutput *= 1.0f + powerf(motorOutputReversed, 2) * pidRuntime.thrustLinearization;
} }
} }
return motorOutput; return motorOutput;

2
src/main/flight/pid.h
View file

@ -352,8 +352,6 @@ typedef struct pidRuntime_s {
#ifdef USE_THRUST_LINEARIZATION #ifdef USE_THRUST_LINEARIZATION
float thrustLinearization; float thrustLinearization;
float thrustLinearizationReciprocal;
float thrustLinearizationB;
#endif #endif
#ifdef USE_AIRMODE_LPF #ifdef USE_AIRMODE_LPF

4
src/main/flight/pid_init.c
View file

@ -375,10 +375,6 @@ void pidInitConfig(const pidProfile_t *pidProfile)
#ifdef USE_THRUST_LINEARIZATION #ifdef USE_THRUST_LINEARIZATION
pidRuntime.thrustLinearization = pidProfile->thrustLinearization / 100.0f; pidRuntime.thrustLinearization = pidProfile->thrustLinearization / 100.0f;
if (pidRuntime.thrustLinearization != 0.0f) {
pidRuntime.thrustLinearizationReciprocal = 1.0f / pidRuntime.thrustLinearization;
pidRuntime.thrustLinearizationB = (1.0f - pidRuntime.thrustLinearization) / (2.0f * pidRuntime.thrustLinearization);
}
#endif #endif
#if defined(USE_D_MIN) #if defined(USE_D_MIN)
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) { for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {