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

Merge pull request #7890 from joelucid/ac_yaw

Browse source 
apply abs control also on yaw
This commit is contained in:
Michael Keller 2019-03-30 10:50:40 +13:00 • committed by GitHub
commit 83cecb17f5
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
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

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