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

apply abs control also on yaw

Browse source
This commit is contained in:
Thorsten Laux 2019-03-28 12:05:19 +01:00
parent 173e958daf
commit 2bb8ed76fd
1 changed files with 20 additions and 17 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1117,25 +1117,28 @@ STATIC_UNIT_TESTED void applyItermRelax(const int axis, const float iterm,
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 (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY || itermRelax == ITERM_RELAX_RPY_INC)) { if (itermRelax) {
const float itermRelaxFactor = MAX(0, 1 - setpointHpf / itermRelaxSetpointThreshold); if (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY || itermRelax == ITERM_RELAX_RPY_INC) {
const bool isDecreasingI = const float itermRelaxFactor = MAX(0, 1 - setpointHpf / itermRelaxSetpointThreshold);
((iterm > 0) && (*itermErrorRate < 0)) || ((iterm < 0) && (*itermErrorRate > 0)); const bool isDecreasingI =
if ((itermRelax >= ITERM_RELAX_RP_INC) && isDecreasingI) { ((iterm > 0) && (*itermErrorRate < 0)) || ((iterm < 0) && (*itermErrorRate > 0));
// Do Nothing, use the precalculed itermErrorRate if ((itermRelax >= ITERM_RELAX_RP_INC) && isDecreasingI) {
} else if (itermRelaxType == ITERM_RELAX_SETPOINT) { // Do Nothing, use the precalculed itermErrorRate
*itermErrorRate *= itermRelaxFactor; } else if (itermRelaxType == ITERM_RELAX_SETPOINT) {
} else if (itermRelaxType == ITERM_RELAX_GYRO ) { *itermErrorRate *= itermRelaxFactor;
*itermErrorRate = fapplyDeadband(setpointLpf - gyroRate, setpointHpf); } else if (itermRelaxType == ITERM_RELAX_GYRO ) {
} else { *itermErrorRate = fapplyDeadband(setpointLpf - gyroRate, setpointHpf);
*itermErrorRate = 0.0f; } else {
*itermErrorRate = 0.0f;
}
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 (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)
applyAbsoluteControl(axis, gyroRate, currentPidSetpoint, itermErrorRate); applyAbsoluteControl(axis, gyroRate, currentPidSetpoint, itermErrorRate);
#endif #endif