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

Minor pid init precalculation optimalisations // Simplify dynC

Browse source
This commit is contained in:
borisbstyle 2017-01-30 10:50:35 +01:00
parent d1944b532c
commit 638ed22fa8
1 changed files with 4 additions and 14 deletions
Download patch file Download diff file Expand all files Collapse all files

18
src/main/flight/pid.c
View file

@ -156,7 +156,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) {
Ki[axis] = ITERM_SCALE * pidProfile->I8[axis]; Ki[axis] = ITERM_SCALE * pidProfile->I8[axis];
Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; Kd[axis] = DTERM_SCALE * pidProfile->D8[axis];
c[axis] = pidProfile->dtermSetpointWeight / 100.0f; c[axis] = pidProfile->dtermSetpointWeight / 100.0f;
relaxFactor[axis] = pidProfile->setpointRelaxRatio / 100.0f; relaxFactor[axis] = 1.0f / (pidProfile->setpointRelaxRatio / 100.0f);
} }
levelGain = pidProfile->P8[PIDLEVEL] / 10.0f; levelGain = pidProfile->P8[PIDLEVEL] / 10.0f;
horizonGain = pidProfile->I8[PIDLEVEL] / 10.0f; horizonGain = pidProfile->I8[PIDLEVEL] / 10.0f;
@ -164,7 +164,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) {
maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 1000 * dT; maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 1000 * dT;
maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT; maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT;
ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f; ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f;
ITermWindupPointInv = 1.0f - ITermWindupPoint; ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint);
itermAcceleratorRateLimit = (float)pidProfile->itermAcceleratorRateLimit; itermAcceleratorRateLimit = (float)pidProfile->itermAcceleratorRateLimit;
} }
@ -214,11 +214,10 @@ static float accelerationLimit(int axis, float currentPidSetpoint) {
void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float tpaFactor) void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float tpaFactor)
{ {
static float previousRateError[2]; static float previousRateError[2];
static float previousSetpoint[3];
const float motorMixRange = getMotorMixRange(); const float motorMixRange = getMotorMixRange();
// Dynamic ki component to gradually scale back integration when above windup point // Dynamic ki component to gradually scale back integration when above windup point
float dynKi = MIN((1.0f - motorMixRange) / ITermWindupPointInv, 1.0f); float dynKi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f);
// ----------PID controller---------- // ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
@ -265,15 +264,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
if (axis != FD_YAW) { if (axis != FD_YAW) {
float dynC = c[axis]; float dynC = c[axis];
if (pidProfile->setpointRelaxRatio < 100) { if (pidProfile->setpointRelaxRatio < 100) {
const float rcDeflection = getRcDeflectionAbs(axis); dynC *= MIN(getRcDeflectionAbs(axis) * relaxFactor[axis], 1.0f);
dynC = c[axis];
if (currentPidSetpoint > 0) {
if ((currentPidSetpoint - previousSetpoint[axis]) < previousSetpoint[axis])
dynC *= MIN((1.0f - rcDeflection) / relaxFactor[axis], 1.0f);
} else if (currentPidSetpoint < 0) {
if ((currentPidSetpoint - previousSetpoint[axis]) > previousSetpoint[axis])
dynC *= MIN((1.0f - rcDeflection) / relaxFactor[axis], 1.0f);
}
} }
const float rD = dynC * currentPidSetpoint - gyroRate; // cr - y const float rD = dynC * currentPidSetpoint - gyroRate; // cr - y
// Divide rate change by dT to get differential (ie dr/dt) // Divide rate change by dT to get differential (ie dr/dt)
@ -288,7 +279,6 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
DTerm = dtermLpfApplyFn(dtermFilterLpf[axis], DTerm); DTerm = dtermLpfApplyFn(dtermFilterLpf[axis], DTerm);
} }
previousSetpoint[axis] = currentPidSetpoint;
// -----calculate total PID output // -----calculate total PID output
axisPIDf[axis] = PTerm + ITerm + DTerm; axisPIDf[axis] = PTerm + ITerm + DTerm;