Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 11:29:58 +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;