Added ITerm limit
This commit is contained in:
parent
3bae1e78e2
commit
f8faf242fa
|
@ -567,9 +567,10 @@ const clivalue_t valueTable[] = {
|
|||
{ "crash_recovery", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CRASH_RECOVERY }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_recovery) },
|
||||
|
||||
{ "iterm_windup", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 30, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermWindupPointPercent) },
|
||||
{ "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_lpf_hz) },
|
||||
{ "iterm_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermLimit) },
|
||||
{ "pidsum_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimit) },
|
||||
{ "pidsum_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 100, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimitYaw) },
|
||||
{ "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_lpf_hz) },
|
||||
|
||||
{ "p_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].P) },
|
||||
{ "i_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].I) },
|
||||
|
|
|
@ -115,7 +115,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
|
|||
.crash_recovery = PID_CRASH_RECOVERY_OFF, // off by default
|
||||
.horizon_tilt_effect = 75,
|
||||
.horizon_tilt_expert_mode = false,
|
||||
.crash_limit_yaw = 200
|
||||
.crash_limit_yaw = 200,
|
||||
.itermLimit = 100
|
||||
);
|
||||
}
|
||||
|
||||
|
@ -242,7 +243,7 @@ static float maxVelocity[3];
|
|||
static float relaxFactor;
|
||||
static float dtermSetpointWeight;
|
||||
static float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, horizonFactorRatio;
|
||||
static float ITermWindupPoint, ITermWindupPointInv;
|
||||
static float ITermWindupPointInv;
|
||||
static uint8_t horizonTiltExpertMode;
|
||||
static timeDelta_t crashTimeLimitUs;
|
||||
static timeDelta_t crashTimeDelayUs;
|
||||
|
@ -252,6 +253,7 @@ static float crashDtermThreshold;
|
|||
static float crashGyroThreshold;
|
||||
static float crashSetpointThreshold;
|
||||
static float crashLimitYaw;
|
||||
static float itermLimit;
|
||||
|
||||
void pidInitConfig(const pidProfile_t *pidProfile)
|
||||
{
|
||||
|
@ -270,7 +272,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
|
|||
horizonFactorRatio = (100 - pidProfile->horizon_tilt_effect) * 0.01f;
|
||||
maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * dT;
|
||||
maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * dT;
|
||||
ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f;
|
||||
const float ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f;
|
||||
ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint);
|
||||
crashTimeLimitUs = pidProfile->crash_time * 1000;
|
||||
crashTimeDelayUs = pidProfile->crash_delay * 1000;
|
||||
|
@ -280,6 +282,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
|
|||
crashDtermThreshold = pidProfile->crash_dthreshold;
|
||||
crashSetpointThreshold = pidProfile->crash_setpoint_threshold;
|
||||
crashLimitYaw = pidProfile->crash_limit_yaw;
|
||||
itermLimit = pidProfile->itermLimit;
|
||||
}
|
||||
|
||||
void pidInit(const pidProfile_t *pidProfile)
|
||||
|
@ -456,7 +459,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
|
|||
|
||||
// -----calculate I component
|
||||
const float ITerm = axisPID_I[axis];
|
||||
const float ITermNew = ITerm + Ki[axis] * errorRate * dT * dynKi * itermAccelerator;
|
||||
const float ITermNew = constrainf(ITerm + Ki[axis] * errorRate * dT * dynKi * itermAccelerator, -itermLimit, itermLimit);
|
||||
const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate);
|
||||
if (outputSaturated == false || ABS(ITermNew) < ABS(ITerm)) {
|
||||
// Only increase ITerm if output is not saturated
|
||||
|
|
|
@ -106,6 +106,7 @@ typedef struct pidProfile_s {
|
|||
uint8_t crash_recovery_rate; // degree/second
|
||||
pidCrashRecovery_e crash_recovery; // off, on, on and beeps when it is in crash recovery mode
|
||||
uint16_t crash_limit_yaw; // limits yaw errorRate, so crashes don't cause huge throttle increase
|
||||
uint16_t itermLimit;
|
||||
} pidProfile_t;
|
||||
|
||||
PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles);
|
||||
|
|
Loading…
Reference in New Issue