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

More fixes.

Browse source
This commit is contained in:
mikeller 2018-06-09 19:20:46 +12:00
parent d43c178488
commit 06428c1b05
1 changed files with 11 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -813,25 +813,24 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
#if defined(USE_ITERM_RELAX) #if defined(USE_ITERM_RELAX)
if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) { if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) {
const float gyroTarget = pt1FilterApply(&windupLpf[axis], currentPidSetpoint); const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
const float gyroHpf = fabsf(currentPidSetpoint - gyroTarget); const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf);
if (itermRelaxType == ITERM_RELAX_SETPOINT && gyroHpf < 60) { if (itermRelaxType == ITERM_RELAX_SETPOINT && setpointHpf < 60) {
itermErrorRate = (1 - gyroHpf / 60.0f) * (currentPidSetpoint - gyroRate); itermErrorRate = (1 - setpointHpf / 60.0f) * (currentPidSetpoint - gyroRate);
} }
const float tolerance = gyroHpf * 1.0f;
if (axis == FD_ROLL) { if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_ITERM_RELAX, 0, gyroTarget); DEBUG_SET(DEBUG_ITERM_RELAX, 0, lrintf(setpointLpf));
DEBUG_SET(DEBUG_ITERM_RELAX, 1, gyroTarget + tolerance); DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(setpointHpf));
DEBUG_SET(DEBUG_ITERM_RELAX, 2, gyroTarget - tolerance); DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(currentPidSetpoint));
DEBUG_SET(DEBUG_ITERM_RELAX, 3, axisError[axis] * 10); DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf(axisError[axis] * 10));
} }
if (itermRelaxType == ITERM_RELAX_GYRO ) { if (itermRelaxType == ITERM_RELAX_GYRO ) {
itermErrorRate = fapplyDeadband(gyroTarget - gyroRate, tolerance); itermErrorRate = fapplyDeadband(setpointLpf - gyroRate, setpointHpf);
} }
#if defined(USE_ABSOLUTE_CONTROL) #if defined(USE_ABSOLUTE_CONTROL)
const float gmaxac = gyroTarget + 2 * tolerance; const float gmaxac = setpointLpf + 2 * setpointHpf;
const float gminac = gyroTarget - 2 * tolerance; const float gminac = setpointLpf - 2 * setpointHpf;
if (gyroRate >= gminac && gyroRate <= gmaxac) { if (gyroRate >= gminac && gyroRate <= gmaxac) {
float acErrorRate1 = gmaxac - gyroRate; float acErrorRate1 = gmaxac - gyroRate;
float acErrorRate2 = gminac - gyroRate; float acErrorRate2 = gminac - gyroRate;