Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 22:35:23 +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]; int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
#endif #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 // 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], PIDscaler[3]; uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
static int32_t errorGyroI[3] = { 0, 0, 0 }; static int32_t errorGyroI[3] = { 0, 0, 0 };
static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f }; 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; RateError = AngleRate - gyroRate;
// -----calculate P component // -----calculate P component
PTerm = RateError * pidProfile->P_f[axis] * PIDscaler[axis] / 100; PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100;
// -----calculate I component. Note that PIDscaler is divided by 10, because it is simplified formule from the previous multiply by 10 // -----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] * PIDscaler[axis] / 10, -250.0f, 250.0f); 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. // 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 // 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; deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis]; delta2[axis] = delta1[axis];
delta1[axis] = delta; 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 // -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm - DTerm), -1000, 1000); 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); RateError = AngleRateTmp - (gyroData[axis] / 4);
// -----calculate P component // -----calculate P component
PTerm = (RateError * pidProfile->P8[axis] * PIDscaler[axis] / 100) >> 7; PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
// -----calculate I component // -----calculate I component
// there should be no division before accumulating the error to integrator, because the precision would be reduced. // 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. // 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) // Time correction (to avoid different I scaling for different builds based on average cycle time)
// is normalized to cycle time = 2048. // 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. // 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 // 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; deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis]; delta2[axis] = delta1[axis];
delta1[axis] = delta; 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 // -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm; axisPID[axis] = PTerm + ITerm + DTerm;

11
src/main/mw.c
View file

@ -100,7 +100,7 @@ int16_t headFreeModeHold;
int16_t telemTemperature1; // gyro sensor temperature 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 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], PIDscaler[3]; extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype
@ -218,8 +218,13 @@ void annexCode(void)
dynI8[axis] = (uint16_t)currentProfile->pidProfile.I8[axis] * prop1 / 100; dynI8[axis] = (uint16_t)currentProfile->pidProfile.I8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t)currentProfile->pidProfile.D8[axis] * prop1 / 100; dynD8[axis] = (uint16_t)currentProfile->pidProfile.D8[axis] * prop1 / 100;
// non coupled PID reduction used in PID controller 1 and PID controller 2 // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids
PIDscaler[axis] = prop2; if (axis == YAW) {
PIDweight[axis] = 100;
}
else {
PIDweight[axis] = prop2;
}
if (rcData[axis] < masterConfig.rxConfig.midrc) if (rcData[axis] < masterConfig.rxConfig.midrc)
rcCommand[axis] = -rcCommand[axis]; rcCommand[axis] = -rcCommand[axis];