Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 14:25:20 +03:00
Code Issues Wiki Activity

Optimized the PID loop a little bit (#5661)

Browse source 
* * Put PID variables into the structure
* Precalculate DTerm gyro filter outside the axis loop
* Removed unused variables PIDweight[3], airmodeWasActivated
* If zero throttle or gyro overflow, we can just set values and exit, this saves checks and jumps in axis loop
* Compute PIDSUM after the axis loop, this saves branching inside the loop because of Yaw has no D term

* * Incorporated review changes from DieHertz and fujin

* * Incorporated another review requests from DieHertz
  - PidSum renamed to Sum
  - pidData[3] redone to pidData[XYZ_AXIS_COUNT]
This commit is contained in:
Miroslav Drbal [ApoC] 2018-04-10 22:22:51 +02:00 • committed by Michael Keller
parent 696478d04c
commit 045557561d
7 changed files with 100 additions and 80 deletions
Download patch file Download diff file Expand all files Collapse all files

14
src/main/fc/fc_core.c
View file

@ -578,10 +578,10 @@ bool processRx(timeUs_t currentTimeUs)
const uint8_t lowThrottleLimit = pidConfig()->runaway_takeoff_deactivate_throttle;
const uint8_t midThrottleLimit = constrain(lowThrottleLimit * 2, lowThrottleLimit * 2, RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT);
if ((((throttlePercent >= lowThrottleLimit) && areSticksActive(RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT)) || (throttlePercent >= midThrottleLimit))
&& (fabsf(axisPIDSum[FD_PITCH]) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)
&& (fabsf(axisPIDSum[FD_ROLL]) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)
&& (fabsf(axisPIDSum[FD_YAW]) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)) {
&& (fabsf(pidData[FD_PITCH].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)
&& (fabsf(pidData[FD_ROLL].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)
&& (fabsf(pidData[FD_YAW].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)) {
inStableFlight = true;
if (runawayTakeoffDeactivateUs == 0) {
runawayTakeoffDeactivateUs = currentTimeUs;
@ -813,9 +813,9 @@ static void subTaskPidController(timeUs_t currentTimeUs)
&& !runawayTakeoffTemporarilyDisabled
&& (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (calculateThrottleStatus() != THROTTLE_LOW))) {
if (((fabsf(axisPIDSum[FD_PITCH]) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD)
|| (fabsf(axisPIDSum[FD_ROLL]) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD)
|| (fabsf(axisPIDSum[FD_YAW]) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD))
if (((fabsf(pidData[FD_PITCH].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD)
|| (fabsf(pidData[FD_ROLL].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD)
|| (fabsf(pidData[FD_YAW].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD))
&& ((ABS(gyroAbsRateDps(FD_PITCH)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP)
|| (ABS(gyroAbsRateDps(FD_ROLL)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP)
|| (ABS(gyroAbsRateDps(FD_YAW)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_YAW))) {