Merge pull request #9990 from ctzsnooze/refactor-Thrust-Linear-update
This commit is contained in:
commit
7bb2c26c43
|
@ -293,9 +293,8 @@ float pidCompensateThrustLinearization(float throttle)
|
||||||
{
|
{
|
||||||
if (pidRuntime.thrustLinearization != 0.0f) {
|
if (pidRuntime.thrustLinearization != 0.0f) {
|
||||||
// for whoops where a lot of TL is needed, allow more throttle boost
|
// 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);
|
const float throttleReversed = (1.0f - throttle);
|
||||||
throttle /= 1.0f + throttleCompensateAmount * powerf(throttleReversed, 2) * pidRuntime.thrustLinearization;
|
throttle /= 1.0f + pidRuntime.throttleCompensateAmount * powerf(throttleReversed, 2);
|
||||||
}
|
}
|
||||||
return throttle;
|
return throttle;
|
||||||
}
|
}
|
||||||
|
|
|
@ -351,6 +351,7 @@ typedef struct pidRuntime_s {
|
||||||
|
|
||||||
#ifdef USE_THRUST_LINEARIZATION
|
#ifdef USE_THRUST_LINEARIZATION
|
||||||
float thrustLinearization;
|
float thrustLinearization;
|
||||||
|
float throttleCompensateAmount;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_AIRMODE_LPF
|
#ifdef USE_AIRMODE_LPF
|
||||||
|
|
|
@ -375,6 +375,7 @@ 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;
|
||||||
|
pidRuntime.throttleCompensateAmount = pidRuntime.thrustLinearization - 0.5f * powerf(pidRuntime.thrustLinearization, 2);
|
||||||
#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) {
|
||||||
|
|
Loading…
Reference in New Issue