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

Removed TPA from Yaw axis and Changed variable name

Browse source 
PIDscaler changed to PIDweight as it suits better and is more
understandable as it is percentage of the PID's used.
Yaw TPA is removed as I don't find it necessary after more testing
This commit is contained in:
borisbstyle 2015-05-11 09:18:05 +02:00
parent 174cf545b1
commit 4ec28b8cce
2 changed files with 17 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -55,8 +55,8 @@ int16_t axisPID[3];
int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
#endif
// PIDscaler is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 1.0x scale means no PID reduction
uint8_t dynP8[3], dynI8[3], dynD8[3], PIDscaler[3];
// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction
uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
static int32_t errorGyroI[3] = { 0, 0, 0 };
static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
@ -183,9 +183,9 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
RateError = AngleRate - gyroRate;
// -----calculate P component
PTerm = RateError * pidProfile->P_f[axis] * PIDscaler[axis] / 100;
// -----calculate I component. Note that PIDscaler is divided by 10, because it is simplified formule from the previous multiply by 10
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * PIDscaler[axis] / 10, -250.0f, 250.0f);
PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100;
// -----calculate I component. Note that PIDweight is divided by 10, because it is simplified formule from the previous multiply by 10
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * PIDweight[axis] / 10, -250.0f, 250.0f);
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings
@ -202,7 +202,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
DTerm = constrainf((deltaSum / 3.0f) * pidProfile->D_f[axis] * PIDscaler[axis] / 100, -300.0f, 300.0f);
DTerm = constrainf((deltaSum / 3.0f) * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm - DTerm), -1000, 1000);
@ -701,13 +701,13 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
RateError = AngleRateTmp - (gyroData[axis] / 4);
// -----calculate P component
PTerm = (RateError * pidProfile->P8[axis] * PIDscaler[axis] / 100) >> 7;
PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
// -----calculate I component
// there should be no division before accumulating the error to integrator, because the precision would be reduced.
// Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
// Time correction (to avoid different I scaling for different builds based on average cycle time)
// is normalized to cycle time = 2048.
errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * pidProfile->I8[axis] * PIDscaler[axis] / 100;
errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * pidProfile->I8[axis] * PIDweight[axis] / 100;
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings
@ -725,7 +725,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
DTerm = (deltaSum * pidProfile->D8[axis] * PIDscaler[axis] / 100) >> 8;
DTerm = (deltaSum * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
// -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm;