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

PID1 and PID2 TPA Implementation

Browse source 
PIDscaler is used to scale down the pids conforming TPA calculation
This commit is contained in:
borisbstyle 2015-05-09 00:52:18 +02:00
parent 7f69537f86
commit 573146788b
2 changed files with 10 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -55,7 +55,7 @@ int16_t axisPID[3];
int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
#endif
uint8_t dynP8[3], dynI8[3], dynD8[3], redP8[3], redI8[3], redD8[3];
uint8_t dynP8[3], dynI8[3], dynD8[3], PIDscaler[3];
static int32_t errorGyroI[3] = { 0, 0, 0 };
static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
@ -182,9 +182,9 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
RateError = AngleRate - gyroRate;
// -----calculate P component
PTerm = RateError * pidProfile->P_f[axis];
PTerm = RateError * pidProfile->P_f[axis] * PIDscaler[axis] / 100;
// -----calculate I component
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * PIDscaler[axis] / 1000, -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
@ -201,7 +201,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], -300.0f, 300.0f);
DTerm = constrainf((deltaSum / 3.0f) * pidProfile->D_f[axis] * PIDscaler[axis] / 100, -300.0f, 300.0f);
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm - DTerm), -1000, 1000);
@ -700,13 +700,13 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
RateError = AngleRateTmp - (gyroData[axis] / 4);
// -----calculate P component
PTerm = (RateError * redP8[axis]) >> 7;
PTerm = (RateError * pidProfile->P8[axis] * PIDscaler[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) * redI8[axis];
errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * pidProfile->I8[axis] * PIDscaler[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
@ -724,7 +724,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 * redD8[axis]) >> 8;
DTerm = (deltaSum * pidProfile->D8[axis] * PIDscaler[axis] / 100) >> 8;
// -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm;

8
src/main/mw.c
View file

@ -100,7 +100,7 @@ int16_t headFreeModeHold;
int16_t telemTemperature1; // gyro sensor temperature
static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
extern uint8_t dynP8[3], dynI8[3], dynD8[3], redP8[3], redI8[3], redD8[3];
extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDscaler[3];
typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype
@ -218,10 +218,8 @@ void annexCode(void)
dynI8[axis] = (uint16_t)currentProfile->pidProfile.I8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t)currentProfile->pidProfile.D8[axis] * prop1 / 100;
// non coupled PID reduction used in PID controller 1
redP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop2 / 100;
redI8[axis] = (uint16_t)currentProfile->pidProfile.I8[axis] * prop2 / 100;
redD8[axis] = (uint16_t)currentProfile->pidProfile.D8[axis] * prop2 / 100;
// non coupled PID reduction used in PID controller 1 and PID controller 2
PIDscaler[axis] = prop2;
if (rcData[axis] < masterConfig.rxConfig.midrc)
rcCommand[axis] = -rcCommand[axis];