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

Optimze pidSum calculation by storing the value rather than recalculating in multiple places. 

Saves 72 bytes. Will save at least that additionally when incorporated into Runaway Takeoff Prevention (which also calculates the pidSum in two places).

Additionally adds a slight performance improvement by not repeating the floating point additions to calculate the pidSum in multiple places.  Effectively replaces 2 calculations with 1 (4 with 1 with Runaway Takeoff Prevention).
This commit is contained in:
Bruce Luckcuck 2018-01-30 09:58:41 -05:00
parent 6efe34d4da
commit fba0b2cea3
4 changed files with 12 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/main/flight/mixer.c
View File

@ -694,11 +694,11 @@ void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation)
// Calculate and Limit the PIDsum
const float scaledAxisPidRoll =
constrainf(axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] + axisPID_D[FD_ROLL], -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;
constrainf(axisPIDSum[FD_ROLL], -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;
const float scaledAxisPidPitch =
constrainf(axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH], -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;
constrainf(axisPIDSum[FD_PITCH], -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;
float scaledAxisPidYaw =
constrainf(axisPID_P[FD_YAW] + axisPID_I[FD_YAW], -currentPidProfile->pidSumLimitYaw, currentPidProfile->pidSumLimitYaw) / PID_MIXER_SCALING;
constrainf(axisPIDSum[FD_YAW], -currentPidProfile->pidSumLimitYaw, currentPidProfile->pidSumLimitYaw) / PID_MIXER_SCALING;
if (!mixerConfig()->yaw_motors_reversed) {
scaledAxisPidYaw = -scaledAxisPidYaw;
}

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

@ -56,7 +56,7 @@ static FAST_RAM bool pidStabilisationEnabled;
static FAST_RAM bool inCrashRecoveryMode = false;
FAST_RAM float axisPID_P[3], axisPID_I[3], axisPID_D[3];
FAST_RAM float axisPID_P[3], axisPID_I[3], axisPID_D[3], axisPIDSum[3];
static FAST_RAM float dT;
@ -530,6 +530,9 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
}
}
axisPID_D[axis] = Kd[axis] * delta * tpaFactor;
axisPIDSum[axis] = axisPID_P[axis] + axisPID_I[axis] + axisPID_D[axis];
} else {
axisPIDSum[axis] = axisPID_P[axis] + axisPID_I[axis];
}
// Disable PID control if at zero throttle or if gyro overflow detected
@ -537,6 +540,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
axisPID_P[axis] = 0;
axisPID_I[axis] = 0;
axisPID_D[axis] = 0;
axisPIDSum[axis] = 0;
}
}
}

1
src/main/flight/pid.h
View File

@ -121,6 +121,7 @@ union rollAndPitchTrims_u;
void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim, timeUs_t currentTimeUs);
extern float axisPID_P[3], axisPID_I[3], axisPID_D[3];
extern float axisPIDSum[3];
bool airmodeWasActivated;
extern uint32_t targetPidLooptime;

6
src/main/flight/servos.c
View File

@ -401,9 +401,9 @@ void servoMixer(void)
input[INPUT_STABILIZED_YAW] = rcCommand[YAW];
} else {
// Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
input[INPUT_STABILIZED_ROLL] = (axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] + axisPID_D[FD_ROLL]) * PID_SERVO_MIXER_SCALING;
input[INPUT_STABILIZED_PITCH] = (axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH]) * PID_SERVO_MIXER_SCALING;
input[INPUT_STABILIZED_YAW] = (axisPID_P[FD_YAW] + axisPID_I[FD_YAW]) * PID_SERVO_MIXER_SCALING;
input[INPUT_STABILIZED_ROLL] = axisPIDSum[FD_ROLL] * PID_SERVO_MIXER_SCALING;
input[INPUT_STABILIZED_PITCH] = axisPIDSum[FD_PITCH] * PID_SERVO_MIXER_SCALING;
input[INPUT_STABILIZED_YAW] = axisPIDSum[FD_YAW] * PID_SERVO_MIXER_SCALING;
// Reverse yaw servo when inverted in 3D mode
if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig()->midrc)) {