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 #endif
#ifdef USE_D_MAX #ifdef USE_D_MAX
float dMaxFactor = 1.0f; float dMaxMultiplier = 1.0f;
if (pidRuntime.dMaxPercent[axis] > 0) { if (pidRuntime.dMaxPercent[axis] > 1.0f) {
float dMaxGyroFactor = pt2FilterApply(&pidRuntime.dMaxRange[axis], delta); float dMaxGyroFactor = pt2FilterApply(&pidRuntime.dMaxRange[axis], delta);
dMaxGyroFactor = fabsf(dMaxGyroFactor) * pidRuntime.dMaxGyroGain; dMaxGyroFactor = fabsf(dMaxGyroFactor) * pidRuntime.dMaxGyroGain;
const float dMaxSetpointFactor = fabsf(pidSetpointDelta) * pidRuntime.dMaxSetpointGain; const float dMaxSetpointFactor = fabsf(pidSetpointDelta) * pidRuntime.dMaxSetpointGain;
dMaxFactor = MAX(dMaxGyroFactor, dMaxSetpointFactor); const float dMaxBoost = fmaxf(dMaxGyroFactor, dMaxSetpointFactor);
dMaxFactor = 1.0f + (1.0f - pidRuntime.dMaxPercent[axis]) * dMaxFactor; // dMaxBoost starts at zero, and by 1.0 we get Dmax, but it can exceed 1.
dMaxFactor = pt2FilterApply(&pidRuntime.dMaxLowpass[axis], dMaxFactor); dMaxMultiplier += (pidRuntime.dMaxPercent[axis] - 1.0f) * dMaxBoost;
dMaxFactor = MIN(dMaxFactor, 1.0f / pidRuntime.dMaxPercent[axis]); 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) { if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_D_MAX, 0, lrintf(dMaxGyroFactor * 100)); DEBUG_SET(DEBUG_D_MAX, 0, lrintf(dMaxGyroFactor * 100));
DEBUG_SET(DEBUG_D_MAX, 1, lrintf(dMaxSetpointFactor * 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) { } 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 // Apply the gain that increases D towards Dmax
preTpaD *= dMaxFactor; preTpaD *= dMaxMultiplier;
#endif #endif
pidData[axis].D = preTpaD * pidRuntime.tpaFactor; 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 #ifdef USE_D_MAX
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) { for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
const uint8_t dMax = pidProfile->d_max[axis]; const uint8_t dMax = pidProfile->d_max[axis];
if ((dMax > 0) && (dMax > pidProfile->pid[axis].D)) { if ((pidProfile->pid[axis].D > 0) && dMax > pidProfile->pid[axis].D) {
pidRuntime.dMaxPercent[axis] = (float) pidProfile->pid[axis].D / dMax; 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 { } else {
pidRuntime.dMaxPercent[axis] = 0; pidRuntime.dMaxPercent[axis] = 1.0f;
} }
} }
pidRuntime.dMaxGyroGain = D_MAX_GAIN_FACTOR * pidProfile->d_max_gain / D_MAX_LOWPASS_HZ; pidRuntime.dMaxGyroGain = D_MAX_GAIN_FACTOR * pidProfile->d_max_gain / D_MAX_LOWPASS_HZ;