Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-18 05:45:31 +03:00
Code Issues Wiki Activity

Fixed logging inconsistencies.

Browse source
This commit is contained in:
mikeller 2018-06-10 13:26:58 +12:00
parent 53476d45de
commit 5ff68d2c82
1 changed files with 9 additions and 15 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -815,24 +815,19 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) { if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY )) {
const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint); const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf); const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf);
if (axis == FD_ROLL) { const float itermRelaxFactor = 1 - setpointHpf / 30.0f;
DEBUG_SET(DEBUG_ITERM_RELAX, 0, lrintf(setpointLpf));
DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(setpointHpf));
}
if (itermRelaxType == ITERM_RELAX_SETPOINT && setpointHpf < 30) { if (itermRelaxType == ITERM_RELAX_SETPOINT && setpointHpf < 30) {
itermErrorRate = (1 - setpointHpf / 30.0f) * (currentPidSetpoint - gyroRate); itermErrorRate = itermRelaxFactor * (currentPidSetpoint - gyroRate);
if (axis == FD_ROLL) { } else if (itermRelaxType == ITERM_RELAX_GYRO ) {
DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(itermErrorRate));
DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf((1 - setpointHpf / 30.0f) * 100));
}
}
if (itermRelaxType == ITERM_RELAX_GYRO ) {
itermErrorRate = fapplyDeadband(setpointLpf - gyroRate, setpointHpf); itermErrorRate = fapplyDeadband(setpointLpf - gyroRate, setpointHpf);
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(itermErrorRate));
}
} }
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_ITERM_RELAX, 0, lrintf(setpointHpf));
DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(itermRelaxFactor * 100.0f));
DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(itermErrorRate));
}
#if defined(USE_ABSOLUTE_CONTROL) #if defined(USE_ABSOLUTE_CONTROL)
const float gmaxac = setpointLpf + 2 * setpointHpf; const float gmaxac = setpointLpf + 2 * setpointHpf;
const float gminac = setpointLpf - 2 * setpointHpf; const float gminac = setpointLpf - 2 * setpointHpf;
@ -867,7 +862,6 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
currentPidSetpoint += acCorrection; currentPidSetpoint += acCorrection;
itermErrorRate += acCorrection; itermErrorRate += acCorrection;
if (axis == FD_ROLL) { if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(acErrorRate));
DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf(axisError[axis] * 10)); DEBUG_SET(DEBUG_ITERM_RELAX, 3, lrintf(axisError[axis] * 10));
} }
} }