rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1069 from Gamma-Software/patch-1 

Little optimisation PID.c
This commit is contained in:
borisbstyle 2016-08-22 23:49:11 +02:00 committed by GitHub
parent 2f14f61190 0f1b00520f
commit c585ad5ac1
1 changed files with 14 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
src/main/flight/pid.c
View File

@ -422,20 +422,7 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina
ITerm = errorGyroI[axis] >> 13;
//-----calculate D-term
if (axis == YAW) {
if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
axisPID[axis] = PTerm + ITerm;
if (motorCount >= 4) {
int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH);
// prevent "yaw jump" during yaw correction
axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW]));
}
DTerm = 0; // needed for blackbox
} else {
if (axis != YAW) {
if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
delta = RateError - lastRateError[axis];
lastRateError[axis] = RateError;
@ -464,6 +451,19 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina
// -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm;
} else {
if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
axisPID[axis] = PTerm + ITerm;
if (motorCount >= 4) {
int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH);
// prevent "yaw jump" during yaw correction
axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW]));
}
DTerm = 0; // needed for blackbox
}
if (!pidStabilisationEnabled) axisPID[axis] = 0;