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

Fix DMax calculations (#13933)

Browse source 
Refactor DMax calculations
This commit is contained in:
ctzsnooze 2024-09-29 08:00:11 +10:00 • committed by GitHub
parent 983b510184
commit 4ed21bdd06
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
2 changed files with 16 additions and 13 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1199,26 +1199,28 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
#endif
#ifdef USE_D_MAX
float dMaxFactor = 1.0f;
if (pidRuntime.dMaxPercent[axis] > 0) {
float dMaxMultiplier = 1.0f;
if (pidRuntime.dMaxPercent[axis] > 1.0f) {
float dMaxGyroFactor = pt2FilterApply(&pidRuntime.dMaxRange[axis], delta);
dMaxGyroFactor = fabsf(dMaxGyroFactor) * pidRuntime.dMaxGyroGain;
const float dMaxSetpointFactor = fabsf(pidSetpointDelta) * pidRuntime.dMaxSetpointGain;
dMaxFactor = MAX(dMaxGyroFactor, dMaxSetpointFactor);
dMaxFactor = 1.0f + (1.0f - pidRuntime.dMaxPercent[axis]) * dMaxFactor;
dMaxFactor = pt2FilterApply(&pidRuntime.dMaxLowpass[axis], dMaxFactor);
dMaxFactor = MIN(dMaxFactor, 1.0f / pidRuntime.dMaxPercent[axis]);
const float dMaxBoost = fmaxf(dMaxGyroFactor, dMaxSetpointFactor);
// dMaxBoost starts at zero, and by 1.0 we get Dmax, but it can exceed 1.
dMaxMultiplier += (pidRuntime.dMaxPercent[axis] - 1.0f) * dMaxBoost;
dMaxMultiplier = pt2FilterApply(&pidRuntime.dMaxLowpass[axis], dMaxMultiplier);
// limit the gain to the fraction that DMax is greater than Min
dMaxMultiplier = MIN(dMaxMultiplier, pidRuntime.dMaxPercent[axis]);
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_D_MAX, 0, lrintf(dMaxGyroFactor * 100));
DEBUG_SET(DEBUG_D_MAX, 1, lrintf(dMaxSetpointFactor * 100));
DEBUG_SET(DEBUG_D_MAX, 2, lrintf(pidRuntime.pidCoefficient[axis].Kd * dMaxFactor * 10 / DTERM_SCALE));
DEBUG_SET(DEBUG_D_MAX, 2, lrintf(pidRuntime.pidCoefficient[axis].Kd * dMaxMultiplier * 10 / DTERM_SCALE)); // actual D
} else if (axis == FD_PITCH) {
DEBUG_SET(DEBUG_D_MAX, 3, lrintf(pidRuntime.pidCoefficient[axis].Kd * dMaxFactor * 10 / DTERM_SCALE));
DEBUG_SET(DEBUG_D_MAX, 3, lrintf(pidRuntime.pidCoefficient[axis].Kd * dMaxMultiplier * 10 / DTERM_SCALE));
}
}
// Apply the dMaxFactor
preTpaD *= dMaxFactor;
// Apply the gain that increases D towards Dmax
preTpaD *= dMaxMultiplier;
#endif
pidData[axis].D = preTpaD * pidRuntime.tpaFactor;

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

@ -449,10 +449,11 @@ void pidInitConfig(const pidProfile_t *pidProfile)
#ifdef USE_D_MAX
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
const uint8_t dMax = pidProfile->d_max[axis];
if ((dMax > 0) && (dMax > pidProfile->pid[axis].D)) {
pidRuntime.dMaxPercent[axis] = (float) pidProfile->pid[axis].D / dMax;
if ((pidProfile->pid[axis].D > 0) && dMax > pidProfile->pid[axis].D) {
pidRuntime.dMaxPercent[axis] = (float) dMax / pidProfile->pid[axis].D;
// fraction that Dmax is higher than D, eg if D is 8 and Dmax is 10, Dmax is 1.25 times bigger
} else {
pidRuntime.dMaxPercent[axis] = 0;
pidRuntime.dMaxPercent[axis] = 1.0f;
}
}
pidRuntime.dMaxGyroGain = D_MAX_GAIN_FACTOR * pidProfile->d_max_gain / D_MAX_LOWPASS_HZ;