diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 1a067dc41d..05171d128e 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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; diff --git a/src/main/mw.c b/src/main/mw.c index dbf6cbd75c..471b4294e0 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -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], PIDscaler[3]; +extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[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,8 +218,13 @@ 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 and PID controller 2 - PIDscaler[axis] = prop2; + // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids + if (axis == YAW) { + PIDweight[axis] = 100; + } + else { + PIDweight[axis] = prop2; + } if (rcData[axis] < masterConfig.rxConfig.midrc) rcCommand[axis] = -rcCommand[axis];