mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-18 22:05:17 +03:00
Increase PID performance by removing Dterm for Yaw
This commit is contained in:
parent
feea25edcc
commit
cbc7bc2a61
1 changed files with 44 additions and 38 deletions
|
@ -225,6 +225,10 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
||||||
ITerm = errorGyroIf[axis];
|
ITerm = errorGyroIf[axis];
|
||||||
|
|
||||||
//-----calculate D-term
|
//-----calculate D-term
|
||||||
|
if (axis == YAW) {
|
||||||
|
if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT());
|
||||||
|
DTerm = 0;
|
||||||
|
} else {
|
||||||
if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
|
if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
|
||||||
delta = RateError - lastErrorForDelta[axis];
|
delta = RateError - lastErrorForDelta[axis];
|
||||||
lastErrorForDelta[axis] = RateError;
|
lastErrorForDelta[axis] = RateError;
|
||||||
|
@ -244,12 +248,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
||||||
if (pidProfile->dterm_average_count) delta = filterApplyAveragef(delta, pidProfile->dterm_average_count, deltaState[axis]);
|
if (pidProfile->dterm_average_count) delta = filterApplyAveragef(delta, pidProfile->dterm_average_count, deltaState[axis]);
|
||||||
|
|
||||||
DTerm = constrainf(delta * (float)pidProfile->D8[axis] * 0.001f * tpaFactor, -300.0f, 300.0f);
|
DTerm = constrainf(delta * (float)pidProfile->D8[axis] * 0.001f * tpaFactor, -300.0f, 300.0f);
|
||||||
|
}
|
||||||
|
|
||||||
// -----calculate total PID output
|
// -----calculate total PID output
|
||||||
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
|
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
|
||||||
|
|
||||||
if (pidProfile->yaw_lpf_hz && axis == YAW) axisPID[axis] = filterApplyPt1(axisPID[axis], &yawFilterState, pidProfile->yaw_lpf_hz, getdT());
|
|
||||||
|
|
||||||
if (lowThrottlePidReduction) axisPID[axis] /= 3;
|
if (lowThrottlePidReduction) axisPID[axis] /= 3;
|
||||||
|
|
||||||
#ifdef GTUNE
|
#ifdef GTUNE
|
||||||
|
@ -511,6 +514,10 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
|
||||||
ITerm = errorGyroI[axis] >> 13;
|
ITerm = errorGyroI[axis] >> 13;
|
||||||
|
|
||||||
//-----calculate D-term
|
//-----calculate D-term
|
||||||
|
if (axis == YAW) {
|
||||||
|
if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT());
|
||||||
|
DTerm = 0;
|
||||||
|
} else {
|
||||||
if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
|
if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
|
||||||
delta = RateError - lastErrorForDelta[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
|
delta = RateError - lastErrorForDelta[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
|
||||||
lastErrorForDelta[axis] = RateError;
|
lastErrorForDelta[axis] = RateError;
|
||||||
|
@ -530,12 +537,11 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
|
||||||
if (pidProfile->dterm_average_count) delta = filterApplyAverage(delta, pidProfile->dterm_average_count, deltaState[axis]) * 2;
|
if (pidProfile->dterm_average_count) delta = filterApplyAverage(delta, pidProfile->dterm_average_count, deltaState[axis]) * 2;
|
||||||
|
|
||||||
DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
|
DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
|
||||||
|
}
|
||||||
|
|
||||||
// -----calculate total PID output
|
// -----calculate total PID output
|
||||||
axisPID[axis] = PTerm + ITerm + DTerm;
|
axisPID[axis] = PTerm + ITerm + DTerm;
|
||||||
|
|
||||||
if (pidProfile->yaw_lpf_hz && axis == YAW) axisPID[axis] = filterApplyPt1(axisPID[axis], &yawFilterState, pidProfile->yaw_lpf_hz, getdT());
|
|
||||||
|
|
||||||
if (lowThrottlePidReduction) axisPID[axis] /= 3;
|
if (lowThrottlePidReduction) axisPID[axis] /= 3;
|
||||||
|
|
||||||
#ifdef GTUNE
|
#ifdef GTUNE
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue