From 0f1b00520f1e4734c9ca308e2677e617754c17a3 Mon Sep 17 00:00:00 2001 From: Gamma-Software Date: Mon, 22 Aug 2016 16:33:23 +0200 Subject: [PATCH 1/3] 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 b6ffc3d586..a4f42ffedc 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; From f9c827c726b774f503232358d0fa8929872198de Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 23 Aug 2016 00:56:22 +0200 Subject: [PATCH 2/3] Remove Int based PID loop (All target support) --- src/main/mw.c | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/src/main/mw.c b/src/main/mw.c index 80605784e6..fd4f970447 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -815,6 +815,7 @@ void taskMainPidLoopCheck(void) { static uint32_t previousTime; static bool runTaskMainSubprocesses; + static uint8_t pidUpdateCountdown; cycleTime = micros() - previousTime; previousTime = micros(); @@ -824,17 +825,6 @@ void taskMainPidLoopCheck(void) debug[1] = averageSystemLoadPercent; } - const uint32_t startTime = micros(); - - while (true) { - if (gyroSyncCheckUpdate(&gyro)) { - if (debugMode == DEBUG_PIDLOOP) {debug[0] = micros() - startTime;} // time spent busy waiting - break; - } - } - - static uint8_t pidUpdateCountdown; - if (runTaskMainSubprocesses) { subTaskMainSubprocesses(); runTaskMainSubprocesses = false; From e6a11960fd3bb1fdbe4bd99de4b9437847b70221 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 23 Aug 2016 01:06:19 +0200 Subject: [PATCH 3/3] Equal pid.c optimalisation to betaflight pid --- src/main/flight/pid.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a4f42ffedc..8950db6ab1 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -286,13 +286,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc ITerm = errorGyroIf[axis]; //-----calculate D-term (Yaw D not yet supported) - if (axis == YAW) { - if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); - - axisPID[axis] = lrintf(PTerm + ITerm); - - DTerm = 0.0f; // needed for blackbox - } else { + if (axis != YAW) { rD = c[axis] * setpointRate[axis] - PVRate; // cr - y delta = rD - lastRateError[axis]; lastRateError[axis] = rD; @@ -317,6 +311,12 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc // -----calculate total PID output axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900); + } else { + if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); + + axisPID[axis] = lrintf(PTerm + ITerm); + + DTerm = 0.0f; // needed for blackbox } // Disable PID control at zero throttle