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

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

@ -1117,7 +1117,8 @@ 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) {
if (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY || itermRelax == ITERM_RELAX_RPY_INC) {
const float itermRelaxFactor = MAX(0, 1 - setpointHpf / itermRelaxSetpointThreshold); const float itermRelaxFactor = MAX(0, 1 - setpointHpf / itermRelaxSetpointThreshold);
const bool isDecreasingI = const bool isDecreasingI =
((iterm > 0) && (*itermErrorRate < 0)) || ((iterm < 0) && (*itermErrorRate > 0)); ((iterm > 0) && (*itermErrorRate < 0)) || ((iterm < 0) && (*itermErrorRate > 0));
@ -1136,6 +1137,8 @@ STATIC_UNIT_TESTED void applyItermRelax(const int axis, const float iterm,
DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(itermRelaxFactor * 100.0f)); DEBUG_SET(DEBUG_ITERM_RELAX, 1, lrintf(itermRelaxFactor * 100.0f));
DEBUG_SET(DEBUG_ITERM_RELAX, 2, lrintf(*itermErrorRate)); 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