mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-26 01:35:41 +03:00
Version 2.5.3 / Configurable yaw_p_limit for all pid controllers
This commit is contained in:
parent
37bd1d65fd
commit
0aac025494
4 changed files with 13 additions and 8 deletions
|
@ -785,11 +785,6 @@ void mixTable(void)
|
||||||
|
|
||||||
bool isFailsafeActive = failsafeIsActive(); // TODO - Find out if failsafe checks are really needed here in mixer code
|
bool isFailsafeActive = failsafeIsActive(); // TODO - Find out if failsafe checks are really needed here in mixer code
|
||||||
|
|
||||||
if (motorLimitReached) {
|
|
||||||
axisPID[YAW] *= constrain(qPercent(mixReduction), 40, 100);
|
|
||||||
if (debugMode == DEBUG_AIRMODE) debug[0] = axisPID[YAW];
|
|
||||||
}
|
|
||||||
|
|
||||||
if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
|
if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
|
||||||
acroPlusApply();
|
acroPlusApply();
|
||||||
}
|
}
|
||||||
|
|
|
@ -213,6 +213,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
||||||
// -----calculate P component
|
// -----calculate P component
|
||||||
PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100;
|
PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100;
|
||||||
|
|
||||||
|
// Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply
|
||||||
|
if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) {
|
||||||
|
PTerm = constrainf(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
|
||||||
|
}
|
||||||
|
|
||||||
// -----calculate I component.
|
// -----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] * 10, -250.0f, 250.0f);
|
||||||
|
|
||||||
|
@ -407,7 +412,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
|
||||||
|
|
||||||
PTerm = (int32_t)error * pidProfile->P8[FD_YAW] >> 6; // TODO: Bitwise shift on a signed integer is not recommended
|
PTerm = (int32_t)error * pidProfile->P8[FD_YAW] >> 6; // TODO: Bitwise shift on a signed integer is not recommended
|
||||||
|
|
||||||
// Constrain YAW by D value if not servo driven in that case servolimits apply
|
// Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply
|
||||||
if(motorCount >= 4 && pidProfile->yaw_p_limit < YAW_P_LIMIT_MAX) {
|
if(motorCount >= 4 && pidProfile->yaw_p_limit < YAW_P_LIMIT_MAX) {
|
||||||
PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
|
PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
|
||||||
}
|
}
|
||||||
|
@ -504,6 +509,11 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
|
||||||
// -----calculate P component
|
// -----calculate P component
|
||||||
PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
|
PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
|
||||||
|
|
||||||
|
// Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply
|
||||||
|
if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) {
|
||||||
|
PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
|
||||||
|
}
|
||||||
|
|
||||||
// -----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.
|
||||||
|
|
|
@ -20,7 +20,7 @@
|
||||||
|
|
||||||
#define GYRO_I_MAX 256 // Gyro I limiter
|
#define GYRO_I_MAX 256 // Gyro I limiter
|
||||||
#define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter
|
#define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter
|
||||||
#define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter
|
#define YAW_P_LIMIT_MAX 300 // Maximum value for yaw P limiter
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
PIDROLL,
|
PIDROLL,
|
||||||
|
|
|
@ -17,7 +17,7 @@
|
||||||
|
|
||||||
#define FC_VERSION_MAJOR 2 // increment when a major release is made (big new feature, etc)
|
#define FC_VERSION_MAJOR 2 // increment when a major release is made (big new feature, etc)
|
||||||
#define FC_VERSION_MINOR 5 // increment when a minor release is made (small new feature, change etc)
|
#define FC_VERSION_MINOR 5 // increment when a minor release is made (small new feature, change etc)
|
||||||
#define FC_VERSION_PATCH_LEVEL 2 // increment when a bug is fixed
|
#define FC_VERSION_PATCH_LEVEL 3 // increment when a bug is fixed
|
||||||
|
|
||||||
#define STR_HELPER(x) #x
|
#define STR_HELPER(x) #x
|
||||||
#define STR(x) STR_HELPER(x)
|
#define STR(x) STR_HELPER(x)
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue