rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Added ITerm limit

This commit is contained in:
Martin Budden 2017-08-23 15:16:14 +01:00
parent 3bae1e78e2
commit f8faf242fa
3 changed files with 10 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/main/fc/settings.c
View File

@ -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) },

11
src/main/flight/pid.c
View File

@ -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

1
src/main/flight/pid.h
View File

@ -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);