mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-16 21:05:35 +03:00
attempts to improve relax
attempts to improve relax fix attempts to improve relax fix bug fixes spaces changed settings and added isAirmodeActive() fix rebase error
This commit is contained in:
Thorsten Laux
Michael Keller
parent
7936fb07d5
commit
fefedbd686
6 changed files with 101 additions and 67 deletions
|
|
@ -147,8 +147,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
|
|||
.iterm_rotation = false,
|
||||
.smart_feedforward = false,
|
||||
.iterm_relax = ITERM_RELAX_OFF,
|
||||
.iterm_relax_cutoff_low = 3,
|
||||
.iterm_relax_cutoff_high = 30,
|
||||
.iterm_relax_cutoff = 11,
|
||||
.iterm_relax_type = ITERM_RELAX_GYRO,
|
||||
.acro_trainer_angle_limit = 20,
|
||||
.acro_trainer_lookahead_ms = 50,
|
||||
.acro_trainer_debug_axis = FD_ROLL,
|
||||
|
|
@ -206,10 +206,10 @@ static FAST_RAM_ZERO_INIT pt1Filter_t dtermLowpass2[2];
|
|||
static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn;
|
||||
static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass;
|
||||
#if defined(USE_ITERM_RELAX)
|
||||
static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[XYZ_AXIS_COUNT][2];
|
||||
static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
|
||||
static FAST_RAM_ZERO_INIT uint8_t itermRelax;
|
||||
static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffLow;
|
||||
static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoffHigh;
|
||||
static FAST_RAM_ZERO_INIT uint8_t itermRelaxType;
|
||||
static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoff;
|
||||
#endif
|
||||
|
||||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
|
|
@ -300,8 +300,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
|
|||
#if defined(USE_ITERM_RELAX)
|
||||
if (itermRelax) {
|
||||
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
|
||||
pt1FilterInit(&windupLpf[i][0], pt1FilterGain(itermRelaxCutoffLow, dT));
|
||||
pt1FilterInit(&windupLpf[i][1], pt1FilterGain(itermRelaxCutoffHigh, dT));
|
||||
pt1FilterInit(&windupLpf[i], pt1FilterGain(itermRelaxCutoff, dT));
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
@ -427,8 +426,8 @@ void pidInitConfig(const pidProfile_t *pidProfile)
|
|||
#endif
|
||||
#if defined(USE_ITERM_RELAX)
|
||||
itermRelax = pidProfile->iterm_relax;
|
||||
itermRelaxCutoffLow = pidProfile->iterm_relax_cutoff_low;
|
||||
itermRelaxCutoffHigh = pidProfile->iterm_relax_cutoff_high;
|
||||
itermRelaxType = pidProfile->iterm_relax_type;
|
||||
itermRelaxCutoff = pidProfile->iterm_relax_cutoff;
|
||||
#endif
|
||||
|
||||
#ifdef USE_ACRO_TRAINER
|
||||
|
|
@ -803,19 +802,77 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
|
|||
}
|
||||
#endif // USE_YAW_SPIN_RECOVERY
|
||||
|
||||
#if defined(USE_ABSOLUTE_CONTROL)
|
||||
// mix in a correction for accrued attitude error
|
||||
float acCorrection = 0;
|
||||
if (acGain > 0) {
|
||||
acCorrection = constrainf(axisError[axis] * acGain, -acLimit, acLimit);
|
||||
currentPidSetpoint += acCorrection;
|
||||
}
|
||||
#endif
|
||||
|
||||
// -----calculate error rate
|
||||
const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
|
||||
float errorRate = currentPidSetpoint - gyroRate; // r - y
|
||||
|
||||
#ifdef USE_ABSOLUTE_CONTROL
|
||||
float acCorrection = 0;
|
||||
float acErrorRate;
|
||||
#endif
|
||||
float itermErrorRate = 0.0f;
|
||||
|
||||
#if defined(USE_ITERM_RELAX)
|
||||
float stickSetpoint = currentPidSetpoint;
|
||||
if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) {
|
||||
const float gyroTarget = pt1FilterApply(&windupLpf[axis], stickSetpoint);
|
||||
const float gyroHpf = fabsf(stickSetpoint - gyroTarget);
|
||||
if (itermRelaxType == ITERM_RELAX_SETPOINT) {
|
||||
itermErrorRate = (1 - MIN(1, fabsf(gyroHpf) / 60.0f)) * (stickSetpoint - gyroRate);
|
||||
}
|
||||
const float tolerance = gyroHpf * 1.0f;
|
||||
if (axis == FD_ROLL) {
|
||||
DEBUG_SET(DEBUG_ITERM_RELAX, 0, gyroTarget);
|
||||
DEBUG_SET(DEBUG_ITERM_RELAX, 1, gyroTarget + tolerance);
|
||||
DEBUG_SET(DEBUG_ITERM_RELAX, 2, gyroTarget - tolerance);
|
||||
DEBUG_SET(DEBUG_ITERM_RELAX, 3, axisError[axis] * 10);
|
||||
}
|
||||
if (itermRelaxType == ITERM_RELAX_GYRO ) {
|
||||
const float gmax = gyroTarget + tolerance;
|
||||
const float gmin = gyroTarget - tolerance;
|
||||
|
||||
if (gyroRate >= gmin && gyroRate <= gmax) {
|
||||
itermErrorRate = 0;
|
||||
} else {
|
||||
itermErrorRate = (gyroRate > gmax ? gmax : gmin ) - gyroRate;
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(USE_ABSOLUTE_CONTROL)
|
||||
const float gmaxac = gyroTarget + 2 * tolerance;
|
||||
const float gminac = gyroTarget - 2 * tolerance;
|
||||
if (gyroRate >= gminac && gyroRate <= gmaxac) {
|
||||
float acErrorRate1 = gmaxac - gyroRate;
|
||||
float acErrorRate2 = gminac - gyroRate;
|
||||
if (acErrorRate1 * axisError[axis] < 0) {
|
||||
acErrorRate = acErrorRate1;
|
||||
} else {
|
||||
acErrorRate = acErrorRate2;
|
||||
}
|
||||
if (fabsf(acErrorRate*dT) > fabsf(axisError[axis]) ) {
|
||||
acErrorRate = -axisError[axis]/dT;
|
||||
}
|
||||
} else {
|
||||
acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate;
|
||||
}
|
||||
#endif // USE_ABSOLUTE_CONTROL
|
||||
} else
|
||||
#endif // USE_ITERM_RELAX
|
||||
{
|
||||
itermErrorRate = currentPidSetpoint - gyroRate;
|
||||
#if defined(USE_ABSOLUTE_CONTROL)
|
||||
acErrorRate = itermErrorRate;
|
||||
#endif // USE_ABSOLUTE_CONTROL
|
||||
}
|
||||
|
||||
#if defined(USE_ABSOLUTE_CONTROL)
|
||||
if (acGain > 0 && isAirmodeActivated()) {
|
||||
axisError[axis] = constrainf(axisError[axis] + acErrorRate * dT, -acErrorLimit, acErrorLimit);
|
||||
acCorrection = constrainf(axisError[axis] * acGain, -acLimit, acLimit);
|
||||
currentPidSetpoint += acCorrection;
|
||||
itermErrorRate += acCorrection;
|
||||
}
|
||||
#endif
|
||||
float errorRate = currentPidSetpoint - gyroRate; // r - y
|
||||
handleCrashRecovery(
|
||||
pidProfile->crash_recovery, angleTrim, axis, currentTimeUs, gyroRate,
|
||||
¤tPidSetpoint, &errorRate);
|
||||
|
|
@ -831,49 +888,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
|
|||
}
|
||||
|
||||
// -----calculate I component
|
||||
float itermErrorRate = 0.0f;
|
||||
#if defined(USE_ABSOLUTE_CONTROL)
|
||||
float acErrorRate;
|
||||
#endif
|
||||
#if defined(USE_ITERM_RELAX)
|
||||
if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) {
|
||||
float gyroTargetLow = pt1FilterApply(&windupLpf[axis][0], currentPidSetpoint);
|
||||
float gyroTargetHigh = pt1FilterApply(&windupLpf[axis][1], currentPidSetpoint);
|
||||
#if defined(USE_ABSOLUTE_CONTROL)
|
||||
gyroTargetLow += acCorrection;
|
||||
gyroTargetHigh += acCorrection;
|
||||
#endif
|
||||
if (axis < FD_YAW) {
|
||||
int itemOffset = (axis << 1);
|
||||
DEBUG_SET(DEBUG_ITERM_RELAX, itemOffset++, gyroTargetHigh);
|
||||
DEBUG_SET(DEBUG_ITERM_RELAX, itemOffset, gyroTargetLow);
|
||||
}
|
||||
const float gmax = MAX(gyroTargetHigh, gyroTargetLow);
|
||||
const float gmin = MIN(gyroTargetHigh, gyroTargetLow);
|
||||
if (gyroRate < gmin || gyroRate > gmax) {
|
||||
itermErrorRate = (gyroRate > gmax ? gmax : gmin ) - gyroRate;
|
||||
}
|
||||
#if defined(USE_ABSOLUTE_CONTROL)
|
||||
else {
|
||||
itermErrorRate = acCorrection;
|
||||
}
|
||||
acErrorRate = itermErrorRate - acCorrection;
|
||||
#endif
|
||||
} else
|
||||
#endif // USE_ITERM_RELAX
|
||||
{
|
||||
itermErrorRate = errorRate;
|
||||
#if defined(USE_ABSOLUTE_CONTROL)
|
||||
acErrorRate = errorRate;
|
||||
#endif
|
||||
}
|
||||
|
||||
#if defined(USE_ABSOLUTE_CONTROL)
|
||||
if (acGain > 0) {
|
||||
axisError[axis] = constrainf(axisError[axis] + acErrorRate * dT, -acErrorLimit, acErrorLimit);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
const float ITerm = pidData[axis].I;
|
||||
const float ITermNew = constrainf(ITerm + pidCoefficient[axis].Ki * itermErrorRate * dynCi, -itermLimit, itermLimit);
|
||||
const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate);
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue