From 0f1b00520f1e4734c9ca308e2677e617754c17a3 Mon Sep 17 00:00:00 2001 From: Gamma-Software Date: Mon, 22 Aug 2016 16:33:23 +0200 Subject: [PATCH] Little optimisation PID.c Hi, I know, it's not a huge optimisation but I just switched the condition of the IF condition (line 425). Given that the YAW axis occur one time in the FOR loop (line 366), the IF(axis == YAW) condition will be called one time whereas the IF(axis != YAW) condition will be called two times. Check first the most used condition in an IF condition is a good thing to do in a code source. I just don't know if it will be a major improvement, I let you judge. --- src/main/flight/pid.c | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b6ffc3d58..a4f42ffed 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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;