Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 04:15:44 +03:00
Code Issues Wiki Activity

D_min smoothing changed to PT2 and re-tuned

Browse source
This commit is contained in:
ctzsnooze 2021-08-31 11:37:33 +10:00
parent 86aa5cc84e
commit ff19f3fecb
3 changed files with 16 additions and 16 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1041,7 +1041,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// loop execution to be delayed.
const float delta =
- (gyroRateDterm[axis] - previousGyroRateDterm[axis]) * pidRuntime.pidFrequency;
float preTpaData = pidRuntime.pidCoefficient[axis].Kd * delta;
float preTpaD = pidRuntime.pidCoefficient[axis].Kd * delta;
#if defined(USE_ACC)
if (cmpTimeUs(currentTimeUs, levelModeStartTimeUs) > CRASH_RECOVERY_DETECTION_DELAY_US) {
@ -1052,12 +1052,12 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
#if defined(USE_D_MIN)
float dMinFactor = 1.0f;
if (pidRuntime.dMinPercent[axis] > 0) {
float dMinGyroFactor = biquadFilterApply(&pidRuntime.dMinRange[axis], delta);
float dMinGyroFactor = pt2FilterApply(&pidRuntime.dMinRange[axis], delta);
dMinGyroFactor = fabsf(dMinGyroFactor) * pidRuntime.dMinGyroGain;
const float dMinSetpointFactor = (fabsf(pidSetpointDelta)) * pidRuntime.dMinSetpointGain;
dMinFactor = MAX(dMinGyroFactor, dMinSetpointFactor);
dMinFactor = pidRuntime.dMinPercent[axis] + (1.0f - pidRuntime.dMinPercent[axis]) * dMinFactor;
dMinFactor = pt1FilterApply(&pidRuntime.dMinLowpass[axis], dMinFactor);
dMinFactor = pt2FilterApply(&pidRuntime.dMinLowpass[axis], dMinFactor);
dMinFactor = MIN(dMinFactor, 1.0f);
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_D_MIN, 0, lrintf(dMinGyroFactor * 100));
@ -1069,17 +1069,17 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
}
// Apply the dMinFactor
preTpaData *= dMinFactor;
preTpaD *= dMinFactor;
#endif
pidData[axis].D = preTpaData * tpaFactor;
pidData[axis].D = preTpaD * tpaFactor;
// Log the value of D pre application of TPA
preTpaData *= D_LPF_FILT_SCALE;
preTpaD *= D_LPF_FILT_SCALE;
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_D_LPF, 2, lrintf(preTpaData));
DEBUG_SET(DEBUG_D_LPF, 2, lrintf(preTpaD));
} else if (axis == FD_PITCH) {
DEBUG_SET(DEBUG_D_LPF, 3, lrintf(preTpaData));
DEBUG_SET(DEBUG_D_LPF, 3, lrintf(preTpaD));
}
} else {
pidData[axis].D = 0;